Diff: main.cpp

Revision:

31:21a112643dc9

Parent:

30:492595db0fc3

--- a/main.cpp	Wed Nov 02 13:30:57 2016 +0000
+++ b/main.cpp	Mon Nov 07 11:33:59 2016 +0000
@@ -33,11 +33,12 @@
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
 DigitalIn   buttonCalibrate(SW3);
+DigitalIn   buttonCalibrateComplete(SW2);
 
 bool draairechts;
 bool draailinks;
 bool turn = 0;
-float waiter = 0.2;
+float waiter = 0.12;
 float translation = 0;
 float degrees3 = 0;
 
@@ -53,6 +54,7 @@
 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
 
 bool calibrate = false;
+bool calibrate_complete = false;
 double threshold_Left = 0;
 double threshold_Right= 0;
 Ticker      sample_timer;
@@ -95,6 +97,15 @@
 double SetpointError_Rotation = 0;
 double theta_translation;
 double theta_rotation;
+
+//gemiddelde
+int counter = 0;
+double Setpoint1 = 0;
+double Setpoint2 = 0;
+double Setpoint3 = 0;
+double Setpoint4 = 0;
+double Setpoint5 = 0;
+double SetpointAvg = 0;
 //booleans for control
 bool booltranslate = false;
 bool boolrotate = false;
@@ -106,7 +117,10 @@
 double Translation_e_prev = 0;
 
 //Spatel PID
-const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
+const double Rotation_Kp = 0.23, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
+//const double Rotation_Kp = 0.3, Rotation_Ki = 0.0429 , Rotation_Kd = 2; is best
+//const double Rotation_Kp = 0.10, Rotation_Ki = 0.0429 , Rotation_Kd = 2;
+//const double Rotation_Kp = 0.05, Rotation_Ki = 0.0429 , Rotation_Kd = 0.4;
 double Rotation_error = 0;
 double Rotation_e_prev = 0;
 
@@ -151,7 +165,7 @@
     // Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
     scope.set(0, emg0.read() );
     scope.set(1, emg1.read() );
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
+    * Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once
 
@@ -165,57 +179,6 @@
     pc.printf("%f, %f \n \r ", x, b);
 } \*/
 
-void GetDirections()
-{
-    if (lowpassFilterRight < threshold_Right)
-        rechts = 0;
-    if (lowpassFilterRight > threshold_Right)
-        rechts = 1;
-    if (lowpassFilterLeft < threshold_Left)
-        links = 0;
-    if (lowpassFilterLeft > threshold_Left)
-        links = 1;
-
-    pc.baud(115200);
-    if ((rechts == 1) && (links == 1) && (turn == 0)) {
-        draailinks = 0;
-        draairechts = 0;
-        turn = 1;
-        pc.printf("begin de actie \n \r ");
-        wait(waiter);
-
-    } else if ((rechts == 1) && (links == 1) && (turn == 1)) {
-        draailinks = 0;
-        draairechts = 0;
-        turn = 0;
-        pc.printf("breek de actie af \n \r ");
-        wait(waiter);
-    } else if ((rechts == 0) && (links == 0)&& (turn == 0)) {
-
-    } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
-        /* if the right button is pressed and the motor isn't rotating to the left,
-        then start rotating to the right etc*/
-        draairechts = !draairechts;
-        pc.printf("draai naar rechts \n \r ");
-        wait(waiter);
-    } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
-        draailinks = 0;
-        draairechts = !draairechts;
-        pc.printf("draai naar rechts na links \n \r ");
-        wait(waiter);
-    } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
-        draailinks = !draailinks;
-        pc.printf("draai naar links \n \r ");
-        wait(waiter);
-    } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
-        draairechts = 0;
-        draailinks = !draailinks;
-        pc.printf("draai naar links na rechts \n \r ");
-        wait(waiter);
-    }
-    wait(2*waiter);
-}
-
 float GetPositionM2()
 {
     float pulses2 = motor2.getPulses();
@@ -235,8 +198,20 @@
 }
 void CheckErrorRotation()
 {
-    theta_rotation = GetRotationM3();
+    counter++;
+    if (counter > 50) {
+        theta_rotation = GetRotationM3();
+        Setpoint5 = Setpoint4;
+        Setpoint4 = Setpoint3;
+        Setpoint3 = Setpoint2;
+        Setpoint2 = Setpoint1;
+        Setpoint1 = SetpointError_Rotation;
+        counter = 0;
+    }
     SetpointError_Rotation =  setpointRotation -theta_rotation;
+
+    SetpointAvg = ((SetpointError_Rotation + Setpoint1 + Setpoint2 + Setpoint3 + Setpoint4 + Setpoint5)/6);
+
 }
 void CheckErrorTranslation()
 {
@@ -253,13 +228,21 @@
         M3_Rotate = 1;
 
     }
-    M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
-    if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
+    double speedfactor = 1;
+    if (setpointRotation != Setpoint_Rotation) {
+        speedfactor = 0.3;
+    }
+    double tolerance = 0.1;
+    if (setpointRotation == Setpoint_Rotation){
+        tolerance = 1;   
+    }    
+    M3_ControlSpeed = speedfactor * Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
+    if (fabs(SetpointAvg) < 0.1) {
         M3_ControlSpeed = 0;
     }
-    if (theta_rotation > (Setpoint_Rotation*0.9))
+    if (theta_rotation > tolerance)
         boolrotate = true;
-    if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0))
+    if ((fabs(theta_rotation) < tolerance ) && (M3_ControlSpeed == 0))
         boolrotate = false;
     M3_Speed = M3_ControlSpeed;
 }
@@ -275,13 +258,13 @@
         M2_Rotate = 1;
     }
     M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
-    if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
+    if (fabs(SetpointError_Translation) < 8) {
         M2_ControlSpeed = 0;
 
     }
-    if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0))
+    if ((theta_translation < -192) && (M2_ControlSpeed == 0))
         booltranslate = true;
-    if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0))
+    if ((theta_translation > -8) && (M2_ControlSpeed == 0))
         booltranslate = false;
     M2_Speed = M2_ControlSpeed;
 
@@ -323,6 +306,57 @@
 {
     pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
 }
+void GetDirections()
+{
+    if (lowpassFilterRight < threshold_Right)
+        rechts = 0;
+    if (lowpassFilterRight > threshold_Right)
+        rechts = 1;
+    if (lowpassFilterLeft < threshold_Left)
+        links = 0;
+    if (lowpassFilterLeft > threshold_Left)
+        links = 1;
+
+    pc.baud(115200);
+    if ((rechts == 1) && (links == 1) && (turn == 0)) {
+        draailinks = 0;
+        draairechts = 0;
+        turn = 1;
+        pc.printf("begin de actie \n \r ");
+        wait(waiter);
+
+    } else if ((rechts == 1) && (links == 1) && (turn == 1)) {
+        draailinks = 0;
+        draairechts = 0;
+        turn = 0;
+        pc.printf("breek de actie af \n \r ");
+        GoBack();
+        wait(waiter);
+    } else if ((rechts == 0) && (links == 0)&& (turn == 0)) {
+
+    } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) {
+        /* if the right button is pressed and the motor isn't rotating to the left,
+        then start rotating to the right etc*/
+        draairechts = !draairechts;
+        pc.printf("draai naar rechts \n \r ");
+        wait(waiter);
+    } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) {
+        draailinks = 0;
+        draairechts = !draairechts;
+        pc.printf("draai naar rechts na links \n \r ");
+        wait(waiter);
+    } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) {
+        draailinks = !draailinks;
+        pc.printf("draai naar links \n \r ");
+        wait(waiter);
+    } else if ((links == 1) && (draairechts == 1) && (turn == 0)) {
+        draairechts = 0;
+        draailinks = !draailinks;
+        pc.printf("draai naar links na rechts \n \r ");
+        wait(waiter);
+    }
+    wait(2*waiter);
+}
 int main()
 {
     //Leds
@@ -337,6 +371,7 @@
     sample_timer.attach(&filterSample, Ts);        //512 Hz
     checkSetpointTranslation.attach(&CheckErrorTranslation,Ts);
     checkSetpointRotation.attach(&CheckErrorRotation,Ts);
+
     //printinfo.attach(&print, Ts);
     pc.baud(115200);
     pc.printf("please push the button to calibrate \n \r");
@@ -345,18 +380,22 @@
             calibrate = true;
             threshold_Left = lowpassFilterLeft*0.9;
             threshold_Right = lowpassFilterRight*0.9;
+            pc.printf("calibration complete, press to continue \n \r");
         }
-        if (calibrate == true) {
+        if ((buttonCalibrateComplete == 0) && (calibrate == true)) {
+            calibrate_complete = true;
+        }
+        if (calibrate_complete == true) {
 
-            pc.printf("calibration complete, calL = %f, L=%f  CalR = %f, R = %f, boolL=%b boolR=%b  \n \r", threshold_Left, lowpassFilterLeft, threshold_Right, lowpassFilterRight, links, rechts);
-            //pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
+            //pc.printf("calibration complete, calL = %f, L=%f  CalR = %f, R = %f, boolL=%b boolR=%b  \n \r", threshold_Left, lowpassFilterLeft, threshold_Right, lowpassFilterRight, links, rechts);
+            pc.printf("rotation is %f, setpoint %f, error = %f en translation = %f en de error %f \n \r", GetRotationM3(), Setpoint_Back, SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
             GetDirections();
             if (draairechts == true) {
-                M1_Speed = 0.2;
-                M1_Rotate = 0;
+                M1_Speed = 0.1;
+                M1_Rotate = 1;
             } else if (draailinks == true) {
-                M1_Speed = 0.2;
-                M1_Rotate = 1;
+                M1_Speed = 0.1;
+                M1_Rotate = 0;
             } else if (turn == 1) {
                 BurgerflipActie();
             } else if (turn == 0) {