Diff: main.cpp

Revision:

8:8a3ab396853f

Parent:

7:c7bdeee1bbac

--- a/main.cpp	Fri Oct 21 10:28:27 2016 +0000
+++ b/main.cpp	Fri Oct 21 12:40:43 2016 +0000
@@ -2,6 +2,9 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 
+DigitalOut led_g(LED_GREEN);
+DigitalOut led_b(LED_BLUE);
+DigitalOut led_r(LED_RED);
 
 DigitalOut M1_Rotate(D2); // voltage only base rotation
 PwmOut M1_Speed(D3);      // voltage only base rotation
@@ -30,7 +33,7 @@
 bool draailinks;
 bool turn = 0;
 float waiter = 0.1;
-float afstand = 50;
+float afstand = 200;
 float translation = 0;
 float degrees3 = 0;
 
@@ -103,23 +106,28 @@
 void GoBack()
 {
     while (GetPositionM2() > 0) {
-
-        M2_Speed = 0.8;
+        M3_Speed = 0;
+        M2_Speed = 1;
         M2_Rotate = 0;
         pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
+        led_r = 0;
     }
     M2_Speed = 0;
 
 
-    while (GetRotationM3 < 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;
+        M3_Speed = 0;
 
-    turn = 0;
+        turn = 0;
     
+
 }
 void Burgerflip()
 {
@@ -128,18 +136,23 @@
         M3_Rotate = 0;
         M2_Speed = 0;
     } else if (GetPositionM2() < afstand) {
-        M2_Speed = 0.8;
+        M2_Speed = 1;
         M2_Rotate = 1;
+
     }
-    if (GetRotationM3() > 75) {
+    if (GetRotationM3() > 150) {
         GoBack();
     }
 }
 
 int main()
 {
+    led_g = 1;
+    led_b = 1;
+    led_r = 1;
+
     while (true) {
-        
+
         GetDirections();
         if (draairechts == true) {
             M1_Speed = 0.2;