Diff: main.cpp

Revision:

27:16327d1337cf

Parent:

26:c640851fa1e7

Child:

28:d265c64d2bca

--- a/main.cpp	Fri Oct 28 08:29:18 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:37:21 2016 +0000
@@ -56,6 +56,7 @@
 double threshold_Right= 0;
 Ticker      sample_timer;
 Ticker      sample_timer2;
+Ticker      printinfo;
 HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 const double a1 = -1.6475;
@@ -78,23 +79,23 @@
 double lowpassFilterRight = 0;
 
 //setpoints
-const double setpoint_Translation = -200;
-const double setpoint_Back = 0;
-const double setpoint_Rotation = 135;
+const double Setpoint_Translation = -200;
+const double Setpoint_Back = 0;
+const double Setpoint_Rotation = pi;
 double M3_ControlSpeed = 0;
 double M2_ControlSpeed = 0;
-double setpointError_Translation;
-double setpointError_Rotation;
+double SetpointError_Translation = 0;
+double SetpointError_Rotation = 0;
 
 //copied from slides
 //Arm PID
 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
-const double Translation_Kp = 6.9240, Translation_Ki = 0.8160, Translation_Kd = 0.4080;
+const double Translation_Kp = 6.9240, Translation_Ki = 3, Translation_Kd = 0.4080;
 double Translation_error = 0;
 double Translation_e_prev = 0;
 
 //Spatel PID
-const double Rotation_Kp = , Rotation_Ki =  , Rotation_Kd = ;
+const double Rotation_Kp = 0.0499, Rotation_Ki = 0.4299 , Rotation_Kd = 0.0429;
 double Rotation_error = 0;
 double Rotation_e_prev = 0;
 
@@ -120,24 +121,7 @@
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
-void motorRotation(double setpoint)
-{
-    theta = GetRotationM3();
-    setpointError_Rotation =  setpoint - theta;
 
-    //set direction
-    if (setpointError_Rotation >0) {
-        M3_Rotate = 0;
-    } else {
-        M3_Rotate = 1;
-    }
-    M3_ControlSpeed = (Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
-    if (M3_ControlSpeed < 0.02) {
-       M3_ControlSpeed = 0;
-    }
-    M3_Speed = M3_ControlSpeed;
-
-}
 void filterSampleLeft()
 {
     highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
@@ -231,35 +215,80 @@
     float degrees3 = (pulses3/Puls_degree);
     float radians3 = (degrees3/360)*2*pi;
 
-    return degrees3;
+    return radians3;
+}
+void motorRotation(double setpoint)
+{
+    double theta_rotation = GetRotationM3();
+    SetpointError_Rotation =  setpoint - theta_rotation;
+
+    //set direction
+    if (SetpointError_Rotation > 0) {
+        M3_Rotate = 0;
+    } else {
+        M3_Rotate = 1;
+
+    }
+    M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
+    if (M3_ControlSpeed < 0.25) {
+        M3_ControlSpeed = 0;
+    }
+    M3_Speed = M3_ControlSpeed;
+
+}
+void motorTranslation(double setpoint)
+{
+    double theta_translation = GetPositionM2();
+    SetpointError_Translation =  setpoint - theta_translation;
+
+    //set direction
+    if (SetpointError_Translation < 0) {
+        M2_Rotate = 0;
+    } else {
+        M2_Rotate = 1;
+
+    }
+    M2_ControlSpeed = Ts * fabs( pid_control(SetpointError_Translation, Translation_Kp, Translation_Ki, Translation_Kd, Translation_error, Translation_e_prev));
+    if (M2_ControlSpeed < 0.25) {
+        M2_ControlSpeed = 0;
+    }
+    M2_Speed = M2_ControlSpeed;
+
 }
 void GoBack()
 {
-    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;
+    motorRotation(Setpoint_Back);
+    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());
+     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;
+     }
+     M3_Speed = 0;
 
-    turn = 0;
+     turn = 0; */
 }
 
 void Burgerflip()
 {
+    led_r = 0;
+    led_b = 1;
+    motorTranslation(Setpoint_Translation);
+    /*
     pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3());
     if (GetPositionM2()< afstand) {
         M3_Speed = 0.2;
@@ -273,6 +302,11 @@
     if (GetRotationM3() > setpoint_Rotation) {
         GoBack();
     }
+    */
+}
+void print()
+{
+    pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
 }
 int main()
 {
@@ -287,6 +321,7 @@
     //sample_timer.attach(&sample, 0.001953125);
     sample_timer2.attach(&filterSampleLeft, Ts);        //512 Hz
     sample_timer.attach(&filterSampleRight, Ts);
+    //printinfo.attach(&print, Ts);
     pc.baud(115200);
     pc.printf("please push the button to calibrate \n \r");
     while (1) {
@@ -298,6 +333,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);
             GetDirections();
             if (draairechts == true) {
                 M1_Speed = 0.2;