Diff: main.cpp

Revision:

3:d3264a6f7a62

Parent:

2:5f29bc7daa49

Child:

4:9b1c6b9dae1c

--- a/main.cpp	Sat Mar 24 18:04:21 2018 +0000
+++ b/main.cpp	Sat Mar 24 18:19:37 2018 +0000
@@ -46,7 +46,9 @@
 void grabToken();//Picks up the token for reading
 void dropToken();//Drops the token off
 void kill(); 
+void turnRight(int, bool);
 void turnLeft(int, bool);
+
 //void rot90(); //Rotates the robot 90 degrees to the left
 //void rot180(); //Turns the robot around
 
@@ -88,6 +90,7 @@
         killAll.rise(&kill); 
         grabToken(); 
         move(1,FORWARD);
+        turnLeft(1, FORWARD);
         wait(2); 
         dropToken(); 
         wait(2);
@@ -136,10 +139,10 @@
         magArm.period(0); 
         magArm.write(0);    
 }
-void turnLeft(int dist, bool direction)
+void turnRight(int dist, bool direction)
 { 
     //Get rid of all FR occurences which will turn right motor off
-    FLdirection = direction;    //to turn left we want this going FORWARD so a 0;
+    FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
                                 
     
     stepFL.period(1/FREQUENCY); 
@@ -151,6 +154,21 @@
     stepFL.period(0);
     stepFL.write(0);
 }
+void turnLeft(int dist, bool direction)
+{ 
+    //Get rid of all FL occurences which will turn left motor off
+    FRdirection = !direction;    //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
+                                
+    
+    stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
+    stepFR.write(0.5); 
+    
+    //dist/stepSize is the number of steps
+    //1/FREQUENCY is the time per step
+    wait((dist/stepSize)*(1/FREQUENCY));
+    stepFR.period(0);
+    stepFR.write(0);
+}
 void rot180()
 {