Revision 7:c7bdeee1bbac, committed 2016-10-21

Comitter:

Frostworks

Date:

Fri Oct 21 10:28:27 2016 +0000

Parent:

6:4e4df2f6157e

Commit message:

motor 3 roteert niet terug en te hard

Changed in this revision

--- a/main.cpp	Fri Oct 21 08:56:33 2016 +0000
+++ b/main.cpp	Fri Oct 21 10:28:27 2016 +0000
@@ -30,8 +30,17 @@
 bool draailinks;
 bool turn = 0;
 float waiter = 0.1;
-float afstand = 150;
+float afstand = 50;
 float translation = 0;
+float degrees3 = 0;
+
+float Puls_degree = (8400/360);
+float wheel1 = 16;
+float wheel2 = 31;
+float wheel3 = 41;
+float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
+float pi = 3.14159265359;
+
 void GetDirections()
 {
     pc.baud(115200);
@@ -74,39 +83,69 @@
     wait(2*waiter);
 }
 
-void GetPosition()
+float GetPositionM2()
+{
+    float pulses2 = motor2.getPulses();
+    float degrees2 = (pulses2/Puls_degree);
+    float radians2 = (degrees2/360)*2*pi;
+    float translation = ((radians2/overbrenging)*32.25);
+
+    return translation;
+}
+float GetRotationM3()
 {
-    float pulses = motor2.getPulses();
-    float Puls_degree = (8400/360);
-    float degrees = (pulses/Puls_degree);
-    float wheel1 = 16;
-    float wheel2 = 31;
-    float wheel3 = 41;
-    float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1));
-    float pi = 3.14159265359;
-    float radians = (degrees/360)*2*pi;
-    float translation = ((radians/overbrenging)*32.25);
-    pc.printf("translation %f , degrees %f, radians %f, pulses %f \n \r ",translation, degrees, radians, pulses);
+    float pulses3 = motor3.getPulses();
+    float degrees3 = (pulses3/Puls_degree);
+    float radians3 = (degrees3/360)*2*pi;
+
+    return degrees3;
+}
+void GoBack()
+{
+    while (GetPositionM2() > 0) {
+
+        M2_Speed = 0.8;
+        M2_Rotate = 0;
+        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
+    }
+    M2_Speed = 0;
 
 
+    while (GetRotationM3 < 0) {
+        M3_Rotate = 1;
+        M3_Speed = 0.2;
+        
+    }
+    M3_Speed = 0;
+
+    turn = 0;
+    
 }
 void Burgerflip()
 {
-    M2_Speed = 0.5;
-    M2_Rotate = 1;
-    M3_Speed = 0.5;
-    M3_Rotate = 1;
+    if (GetPositionM2() > afstand) {
+        M3_Speed = 0.2;
+        M3_Rotate = 0;
+        M2_Speed = 0;
+    } else if (GetPositionM2() < afstand) {
+        M2_Speed = 0.8;
+        M2_Rotate = 1;
+    }
+    if (GetRotationM3() > 75) {
+        GoBack();
+    }
+}
 
-}
 int main()
 {
     while (true) {
+        
         GetDirections();
         if (draairechts == true) {
-            M1_Speed = 0.5;
+            M1_Speed = 0.2;
             M1_Rotate = 0;
         } else if (draailinks == true) {
-            M1_Speed = 0.5;
+            M1_Speed = 0.2;
             M1_Rotate = 1;
         } else if (turn == 1) {
             /*M2_Speed = 0.5;
@@ -121,7 +160,7 @@
         if ((draailinks == false) && (draairechts == false)) {
             M1_Speed = 0;
         }
-        GetPosition();
+        pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2());
         /* pulses = 8400 */
     }
 }