Diff: main.cpp

Revision:

1:1fed86c424fd

Parent:

0:c1b3cc00b091

Child:

2:664717ad13cc

--- a/main.cpp	Fri Nov 06 01:28:38 2015 +0000
+++ b/main.cpp	Sun Nov 08 01:35:36 2015 +0000
@@ -53,53 +53,61 @@
     pc.printf("Hellooooo\r\n");
 }
  
+
+void goForward(int dist)
+{
+    setRightPwm(0.5);
+    setLeftPwm(0.45);
+    while (Rdistance() < dist && Ldistance() < dist)
+    {
+        pc.printf("Lp: %d\nRp: %d\n\n", Ldistance(), Rdistance());
+    }
+    setRightPwm(0);
+    setLeftPwm(0);
+    RENC.reset();
+    LENC.reset();
+}
+
+void turnR()
+{
+    const int turn_dist = 1000;
+    
+    setLeftPwm(0.5);
+    while(Ldistance() < turn_dist)
+    {
+        pc.printf("Turning... Lp: %d\n", Ldistance());
+    }
+    setLeftPwm(0);
+    RENC.reset();
+    LENC.reset();
+}
+
+void turnL()
+{
+    const int turn_dist = 1000;
+    
+    setRightPwm(0.5);
+    while(Rdistance() < turn_dist)
+    {
+        pc.printf("Turning... Rp: %d\n", Rdistance());
+    }
+    setRightPwm(0);
+    RENC.reset();
+    LENC.reset();
+}
+
 int main() {
     mybutton.fall(&pressed);
     led3 = 0;
     motor_enabler = 1;
-    pc.printf("\n\n\nHello world\n");
     
-    /*
-    while (Rdistance() < 5000) {   //while encoder pulses are less than 5000, turn led3 on and set LMOTORA to 0.50
-        pc.printf("Pulse Count: %d\n", Rdistance());
-        led3 = 1;
-        setRightPwm(0.5);
-    }
-    RENC.reset();                       // reset encoder pulse count to zero
-    while (Rdistance() < 5000) {   //while encoder pulses are less than 5000, turn led3 off and set LMOTORA to 0.25
-        pc.printf("Pulse Count: %d\n", Rdistance());
-        led3 = 0;
-        setRightPwm(0.25);
-    }
-    RENC.reset();
-    while (Rdistance() < 10000) {  //while encoder pulses are less than 10000, turn led3 on and set LMOTORA to 1.00
-        pc.printf("Pulse Count: %d\n", Rdistance());
-        led3 = 1;
-        setRightPwm(1.0);
-    }
-    setRightPwm(0.0);
-    */
-    
-    
-    
-    while (Ldistance() < 5000) {   //while encoder pulses are less than 5000, turn led3 on and set LMOTORA to 0.50
-        pc.printf("Pulse Count: %d\n", Ldistance());
-        led3 = 1;
-        setLeftPwm(0.5);
-    }
-    LENC.reset();                       // reset encoder pulse count to zero
-    while (Ldistance() < 5000) {   //while encoder pulses are less than 5000, turn led3 off and set LMOTORA to 0.25
-        pc.printf("Pulse Count: %d\n", Ldistance());
-        led3 = 0;
-        setLeftPwm(0.25);
-    }
-    LENC.reset();
-    while (Ldistance() < 10000) {  //while encoder pulses are less than 10000, turn led3 on and set LMOTORA to 1.00
-        pc.printf("Pulse Count: %d\n", Ldistance());
-        led3 = 1;
-        setLeftPwm(1.0);
-    }
-    setLeftPwm(0.0);
-    
+    goForward(1400);
+    //wait(1);
+    turnR();
+    //wait(1);
+    goForward(700);
+    turnL();
+    goForward(1200);
+
     pc.printf("Done.\n");
 }
\ No newline at end of file