Diff: main.cpp

Revision:

1:4465c9e203ce

Parent:

0:76bc19ed12ee

Child:

2:da3b7dd2beb0

--- a/main.cpp	Tue Oct 06 16:34:46 2015 +0000
+++ b/main.cpp	Thu Oct 15 10:42:30 2015 +0000
@@ -4,14 +4,20 @@
 #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
+//Motor 1
+const float M1_Ks = 0.4;           //minimum power required to move
+const float M1_Kp = 0.1;          //strentgh of proportional control
+const float M1_Kd = 0.1;          //strentgh of differential control Include timestep in constant
+const float M1_Ki = 0.1;           //strentgh of integrational control Include timestep in constant
+const double M1_friction = 0.55;    //minimum power required to make the motor move
+//Motor 2
+const float M2_Ks = 0.4;           //minimum power required to move
+const float M2_Kp = 0.1;          //strentgh of proportional control
+const float M2_Kd = 0.1;          //strentgh of differential control Include timestep in constant
+const float M2_Ki = 0.1;           //strentgh of integrational control Include timestep in constant
+const double M2_friction = 0.55;    //minimum power required to make the motor move
 //----------End of control parameters
 
-
-
 //encoder
 QEI Motor1 (D13, D12, NC, 32);
 QEI Motor2 (D11, D10, NC, 32);
@@ -29,40 +35,40 @@
 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;
+double M1_dpos, M1_pos;
+double M2_dpos, M2_pos;
+double M_error, M1_error, M2_error;
+double position;
+double pref,pref2;
+bool M1_dir, M2_dir;
+const double long gearbox=0.08587786259541984732824427480916;
+double M_error1 = 0;
 
 //Timers and Tickers
 Timer t;
 Ticker t1,t2,t3;
 
 //booleans run program
-volatile bool send_go = false, direction_go = false, speed_go = false;
+volatile bool send_go = false, setpoint_go = false, speed_go = false, control_go= false;
 
 //------------------Activate programs-----------
 //Activate send data pc
-void Send_True()
+void D_Send_True()
 {
     send_go = true;
 }
 // Activate desired location
-void M_position_True()
+void M_Setpoint_True()
 {
-    direction_go = true;
+    setpoint_go = true;
+}
+// Controll if motor should go or not
+void M_Control_True()
+{
+    control_go = true;
 }
 //read position of motor and drive motor to set position.
-void M_speed_True()
+void M_Speed_True()
 {
     speed_go = true;
 }
@@ -77,53 +83,64 @@
     scope.set(0,Motor1.getPulses());
     scope.set(1,Motor2.getPulses());
     scope.send();
-    pc.printf("Deg M1: %f M2: %f\n", Deg1, Deg2);
+//    pc.printf("Deg M1: %f M2: %f\n", M1_pos, M2_pos);
 }
 
 //Desired Position Motors --> future script emg
-void direction()
+void Setpoint()
+{
+    M1_dpos=pot1.read()*360-180;
+    M2_dpos=pot2.read()*360-180;
+    pc.printf(" Desired: M1_dpos: %f M1_dir: %i M2_dpos: %f M2_dir: %i \n",M1_dpos, M1_dir, M2_dpos, M2_dir);
+}
+double Pw_control (double Pulse,double setpoint,double Ks, double Kp,double Ki,double Kd, double friction)
 {
-    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);
+    //read position motor
+    position = gearbox*Pulse;
+    // Motor Power
+    M_error = abs(setpoint-position);
+    double M_error_Int= M_error_Int+M_error/1e4;
+    double Ps = Ks;
+    double Pp = Kp * M_error;                                        //Proportional control
+    double Pi = Ki * M_error_Int*M_error;  //check dit --> nog niet goed                            //int controll
+    double Pd = Kd * (M_error-M_error1);    //Differtial control
+    M_error1 = M_error;
+    double Power = Ps + Pp + Pi + Pd;
+    pc.printf("Power bf: %f Pi: %f \n", Power, Pi);
+
+    // overcome minimum power required to turn and stop the motor from 'piepen'.
+    if (Power<friction) {
+        Power=0;
+    } else {}
+
+    //print error
+//    pc.printf("Error: %f Direction: %f Position M: %f Setpoint %f \n", M_error, position, setpoint);
+
+    pc.printf("Power: %f\n", Power);
+    return Power ;//is it possible to return 2 things for
+}
+// direction controll
+bool Dr_control (double Pulse,double setpoint)
+{
+    //read position motor
+    double position = gearbox*Pulse;
+    // Direction motor should turn
+    int Direction = (position > setpoint) ? false:true;
+    return Direction;
+}
+// begin to set point
+void calibration()
+{
+    M1_dpos=0;
+    M2_dpos=0;
+    if (button==true) {
+        M1_pos=0;
+        M2_pos=0;
+    } else {
+    }
 }
 
-//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()
 {
@@ -138,25 +155,35 @@
 
 
     //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
+    t1.attach_us(&D_Send_True, 1e4);         //Send data to pc
+    t2.attach_us(&M_Setpoint_True, 1e4);   //Desired position motor(EMG goes here)
+    t3.attach_us(M_Control_True, 1e4);      //Speed control here
 
 
 //-------------Scedule programs-------------------
     while(1) {
-        if (send_go==true) {
+        if (setpoint_go == true) {
+            Setpoint();
+
+            setpoint_go = false;
+        }  else if (control_go == true) {
+            // control Motor1
+            E1 = Pw_control (Motor1.getPulses(), M1_dpos, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
+            M1 = Dr_control (Motor1.getPulses(), M1_dpos);
+            M1_pos = position;
+            M1_error = M_error;
+            // control Motor2
+            E2 = Pw_control (Motor2.getPulses(), M2_dpos, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
+            //M2 = Dr_control (Motor2.getPulses(),M2_dpos);
+            M2_pos = position;
+            M2_error = M_error;
+            //print error
+            pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error); // once this works place it in 'send'
+            control_go = false;
+        } else 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----------------