Diff: main.cpp

Revision:

0:76bc19ed12ee

Child:

1:4465c9e203ce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 06 16:34:46 2015 +0000
@@ -0,0 +1,165 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+//---------- Change motor control parameters-------
+float Ks = 0.5; //minimum power required to move
+float Kp = 0.65; //strentgh of proportional control
+float Kd = 0.12; //strentgh of differential control
+float Ki = 0.5; //strentgh of integrational control
+//----------End of control parameters
+
+
+
+//encoder
+QEI Motor1 (D13, D12, NC, 32);
+QEI Motor2 (D11, D10, NC, 32);
+
+//Define pins
+DigitalOut      M2(D7);   //direction 2 //1 is cc 0=cw
+PwmOut          E2(D6);   //speed 2
+DigitalOut      M1(D4);  //direction 1 //1 is cc 0=cw
+PwmOut          E1(D5);   //speed 1
+AnalogIn        pot1(A0);   //read value of pot1 for position
+AnalogIn        pot2(A1);   //read value of pot2 for position
+DigitalOut      myled(LED_GREEN);
+MODSERIAL       pc(USBTX, USBRX);
+DigitalIn       button(PTA4);
+HIDScope        scope(2);
+
+//Define Variables
+double Deg1;
+double Deg2;
+double M1error;
+double M2error;
+double M1output;
+double M2output;
+double pos1;
+double pos2;
+double output;
+double pref1,pref2;
+bool dir1;
+bool dir2;
+double long gearbox=0.08587786259541984732824427480916;
+
+//Timers and Tickers
+Timer t;
+Ticker t1,t2,t3;
+
+//booleans run program
+volatile bool send_go = false, direction_go = false, speed_go = false;
+
+//------------------Activate programs-----------
+//Activate send data pc
+void Send_True()
+{
+    send_go = true;
+}
+// Activate desired location
+void M_position_True()
+{
+    direction_go = true;
+}
+//read position of motor and drive motor to set position.
+void M_speed_True()
+{
+    speed_go = true;
+}
+//------------------End of activate programs--------
+
+
+//------------------Start of control programs-------
+
+//Send values over HIDScope & Serial port
+void Send()
+{
+    scope.set(0,Motor1.getPulses());
+    scope.set(1,Motor2.getPulses());
+    scope.send();
+    pc.printf("Deg M1: %f M2: %f\n", Deg1, Deg2);
+}
+
+//Desired Position Motors --> future script emg
+void direction()
+{
+    pos1=pot1.read()*360-180;
+    pos2=pot2.read()*360-180;
+    pc.printf(" Desired: pos1: %f dir1: %i pos2: %f dir2: %i \n",pos1, dir1, pos2, dir2);
+}
+
+//translate desired position to movement motor
+void speed()
+{
+//Read position --> Turn pulses to Degrees
+    Deg1=gearbox*Motor1.getPulses();
+    Deg2=gearbox*Motor2.getPulses();
+//Direction the motors should turn
+    dir1=(Deg1 > pos1) ? false: true;
+    dir2=(Deg2 > pos1) ? false: true;
+    //Motor power
+    M1error=Ks+Kp*abs((Deg1-pos1)/pos1)+Kd*abs((pref1-abs((Deg1-pos1))/pos1))+Ki*0;
+    M2error=Ks+Kp*abs((Deg2-pos1)/pos1)+Kd*0+Ki*0;
+    pref1=abs((Deg1-pos1));//prefious error
+    pref2=abs((Deg2-pos2));//prefious error
+    pc.printf("M1error: %f\n", M1error);
+
+//Motor controll
+    //Motor 1
+    if ((Deg1< pos1) || (Deg1> pos1)) {
+        E1=M1error;
+        M1=dir1;
+        pc.printf("M1>P1\n");
+    } else {
+        E1=0;
+        pc.printf("I am in Else\n");
+    }
+    //Motor 2
+    if ((Deg2< pos2) || (Deg2> pos2)) {
+        E2=M2error;
+        M2=dir2;
+        pc.printf("M2>P2\n");
+    } else {
+        E1=0;
+        pc.printf("I am in Else\n");
+    }
+}
+//--------------- End of control programs----------
+int main()
+{
+    //turn that led off!It hurts my eyes! Ow, I do boot.
+    myled=1;
+
+    //PWM period motors
+    E1.period(0.0001f);
+    E2.period(0.0001f);
+
+    pc.baud(115200);
+
+
+    //sub programs - time how fast everything occurs
+    t1.attach_us(&Send_True, 1e4);         //Send data to pc
+    t2.attach_us(&M_position_True, 1e4);   //Desired position motor(EMG goes here)
+    t3.attach_us(&M_speed_True, 1e4);      //Speed control here
+
+
+//-------------Scedule programs-------------------
+    while(1) {
+        if (send_go==true) {
+            Send();
+
+            send_go = false;
+        } else if (direction_go == true) {
+            direction();
+
+            direction_go = false;
+        } else if (speed_go == true) {
+            speed();
+
+            speed_go = false;
+        } else {
+
+//-----------End of scedule progrmas----------------
+        }
+    }
+}