Revision 28:d265c64d2bca, committed 2016-11-01

Comitter:

Frostworks

Date:

Tue Nov 01 09:18:30 2016 +0000

Parent:

27:16327d1337cf

Commit message:

wouter boyyy

Changed in this revision

--- a/main.cpp	Fri Oct 28 10:37:21 2016 +0000
+++ b/main.cpp	Tue Nov 01 09:18:30 2016 +0000
@@ -87,15 +87,18 @@
 double SetpointError_Translation = 0;
 double SetpointError_Rotation = 0;
 
+//booleans for control
+bool booltranslate = false;
+bool boolrotate = false;
 //copied from slides
 //Arm PID
 const double Ts = 0.001953125; //Ts=1/fs (sample frequency)
-const double Translation_Kp = 6.9240, Translation_Ki = 3, Translation_Kd = 0.4080;
+const double Translation_Kp = 6.9, Translation_Ki = 0.8, Translation_Kd = 0.4;
 double Translation_error = 0;
 double Translation_e_prev = 0;
 
 //Spatel PID
-const double Rotation_Kp = 0.0499, Rotation_Ki = 0.4299 , Rotation_Kd = 0.0429;
+const double Rotation_Kp = 0.0499, Rotation_Ki = 0.0429 , Rotation_Kd = 0.0429;
 double Rotation_error = 0;
 double Rotation_e_prev = 0;
 
@@ -230,11 +233,14 @@
 
     }
     M3_ControlSpeed = Ts * fabs( pid_control(SetpointError_Rotation, Rotation_Kp, Rotation_Ki, Rotation_Kd, Rotation_error, Rotation_e_prev));
-    if (M3_ControlSpeed < 0.25) {
+    if (fabs(SetpointError_Rotation) < fabs(Setpoint_Rotation*0.05)) {
         M3_ControlSpeed = 0;
     }
-    M3_Speed = M3_ControlSpeed;
-
+    if (theta_rotation > (Setpoint_Rotation*0.9))
+        boolrotate = true;
+    if ((theta_rotation < (Setpoint_Rotation*0.07) ) && (M3_Speed == 0))
+        boolrotate = false;
+        M3_Speed = M3_ControlSpeed;
 }
 void motorTranslation(double setpoint)
 {
@@ -246,20 +252,31 @@
         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) {
+    if (fabs(SetpointError_Translation) < fabs(Setpoint_Translation*0.05)) {
         M2_ControlSpeed = 0;
+        
     }
+    if ((theta_translation < Setpoint_Translation*0.95) && (M2_ControlSpeed == 0))
+        booltranslate = true;
+    if ((theta_translation > Setpoint_Translation*0.05) && (M2_ControlSpeed == 0))
+        booltranslate = false;
     M2_Speed = M2_ControlSpeed;
-
+  
 }
 void GoBack()
 {
-    motorRotation(Setpoint_Back);
+    motorTranslation(Setpoint_Back);
+    if (booltranslate == false) {
+        motorRotation(Setpoint_Back);
+    }
+    if (boolrotate == false) {
+        turn = 0;
+    }
     led_r = 1;
     led_b = 0;
+
     /* while (GetPositionM2() < 0) {
          M3_Speed = 0;
          M2_Speed = 1;
@@ -288,6 +305,9 @@
     led_r = 0;
     led_b = 1;
     motorTranslation(Setpoint_Translation);
+    if (booltranslate == true) {
+        motorRotation(Setpoint_Rotation);
+    }
     /*
     pc.printf("get position %f, get rotation %f \n \r", GetPositionM2(), GetRotationM3());
     if (GetPositionM2()< afstand) {
@@ -304,6 +324,13 @@
     }
     */
 }
+void BurgerflipActie()
+{
+    Burgerflip();
+    if (boolrotate == true) {
+        GoBack();
+    }
+}
 void print()
 {
     pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
@@ -346,7 +373,7 @@
                 M2_Rotate = 1;
                 M3_Speed = 0.5;
                 M3_Rotate = 1;*/
-                Burgerflip();
+                BurgerflipActie();
             } else if (turn == 0) {
                 M2_Speed = 0;
                 M3_Speed = 0;