Revision 29:b6d7bcb26f47, committed 2016-11-01

Comitter:

Frostworks

Date:

Tue Nov 01 14:05:14 2016 +0000

Parent:

28:d265c64d2bca

Commit message:

Voor het implementeren van emg;

Changed in this revision

--- a/main.cpp	Tue Nov 01 09:18:30 2016 +0000
+++ b/main.cpp	Tue Nov 01 14:05:14 2016 +0000
@@ -57,6 +57,8 @@
 Ticker      sample_timer;
 Ticker      sample_timer2;
 Ticker      printinfo;
+Ticker      checkSetpointTranslation;
+Ticker      checkSetpointRotation;
 HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 const double a1 = -1.6475;
@@ -79,6 +81,8 @@
 double lowpassFilterRight = 0;
 
 //setpoints
+volatile float setpointRotation;
+volatile float setpointTranslation;
 const double Setpoint_Translation = -200;
 const double Setpoint_Back = 0;
 const double Setpoint_Rotation = pi;
@@ -86,7 +90,8 @@
 double M2_ControlSpeed = 0;
 double SetpointError_Translation = 0;
 double SetpointError_Rotation = 0;
-
+double theta_translation;
+double theta_rotation;
 //booleans for control
 bool booltranslate = false;
 bool boolrotate = false;
@@ -220,11 +225,17 @@
 
     return radians3;
 }
-void motorRotation(double setpoint)
+void CheckErrorRotation(){
+    theta_rotation = GetRotationM3();
+    SetpointError_Rotation =  setpointRotation -theta_rotation;
+}
+void CheckErrorTranslation(){
+    theta_translation = GetPositionM2();
+    SetpointError_Translation =  setpointTranslation -theta_translation;
+}
+void motorRotation()
 {
-    double theta_rotation = GetRotationM3();
-    SetpointError_Rotation =  setpoint - theta_rotation;
-
+        printf("setpoint = %f \n\r", setpointRotation);
     //set direction
     if (SetpointError_Rotation > 0) {
         M3_Rotate = 0;
@@ -242,10 +253,10 @@
         boolrotate = false;
         M3_Speed = M3_ControlSpeed;
 }
-void motorTranslation(double setpoint)
+void motorTranslation()
 {
-    double theta_translation = GetPositionM2();
-    SetpointError_Translation =  setpoint - theta_translation;
+    theta_translation = GetPositionM2();
+    SetpointError_Translation =  setpointTranslation - theta_translation;
 
     //set direction
     if (SetpointError_Translation < 0) {
@@ -267,62 +278,29 @@
 }
 void GoBack()
 {
-    motorTranslation(Setpoint_Back);
+    setpointTranslation = Setpoint_Back;
+    motorTranslation();
     if (booltranslate == false) {
-        motorRotation(Setpoint_Back);
+        setpointRotation = Setpoint_Back;
+        motorRotation();
     }
     if (boolrotate == false) {
         turn = 0;
     }
     led_r = 1;
     led_b = 0;
-
-    /* while (GetPositionM2() < 0) {
-         M3_Speed = 0;
-         M2_Speed = 1;
-         M2_Rotate = 1;
-         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
-         led_r = 0;
-     }
-     M2_Speed = 0;
-
-
-     while (GetRotationM3() > 0) {
-         M3_Rotate = 1;
-         M3_Speed = 0.2;
-         led_r = 1;
-         led_b = 0;
-         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
-
-     }
-     M3_Speed = 0;
-
-     turn = 0; */
 }
 
 void Burgerflip()
 {
     led_r = 0;
     led_b = 1;
-    motorTranslation(Setpoint_Translation);
+    setpointTranslation = Setpoint_Translation;
+    motorTranslation();
     if (booltranslate == true) {
-        motorRotation(Setpoint_Rotation);
+        setpointRotation = Setpoint_Rotation;
+        motorRotation();
     }
-    /*
-    pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3());
-    if (GetPositionM2()< afstand) {
-        M3_Speed = 0.2;
-        M3_Rotate = 0;
-        M2_Speed = 0;
-    } else if (GetPositionM2() > afstand) {
-        M2_Speed = 1;
-        M2_Rotate = 0;
-
-    }
-    if (GetRotationM3() > setpoint_Rotation) {
-        GoBack();
-    }
-    */
 }
 void BurgerflipActie()
 {
@@ -348,6 +326,8 @@
     //sample_timer.attach(&sample, 0.001953125);
     sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
     sample_timer.attach(&filterSampleRight, Ts);
+    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");
@@ -356,11 +336,11 @@
             calibrate = true;
             threshold_Left = lowpassFilterLeft*0.7;
             threshold_Right = lowpassFilterRight*0.7;
-
         }
         if (calibrate == true) {
-            //pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
-            pc.printf("rotation is %f error = %f en translation = %f en de error %f \n \r", GetRotationM3(), SetpointError_Rotation, GetPositionM2(), SetpointError_Translation);
+            
+            pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
+            //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;
@@ -369,10 +349,6 @@
                 M1_Speed = 0.2;
                 M1_Rotate = 1;
             } else if (turn == 1) {
-                /*M2_Speed = 0.5;
-                M2_Rotate = 1;
-                M3_Speed = 0.5;
-                M3_Rotate = 1;*/
                 BurgerflipActie();
             } else if (turn == 0) {
                 M2_Speed = 0;
@@ -381,9 +357,6 @@
             if ((draailinks == false) && (draairechts == false)) {
                 M1_Speed = 0;
             }
-            //pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
-            /* pulses = 8400 */
-            /*empty loop, sample() is executed periodically*/
         }
     }
 }
\ No newline at end of file