Diff: main.cpp

Revision:

5:17a8d8395a50

Parent:

4:9b1c6b9dae1c

Child:

6:3e133819d792

diff -r 9b1c6b9dae1c -r 17a8d8395a50 main.cpp
--- a/main.cpp	Sat Mar 31 18:47:48 2018 +0000
+++ b/main.cpp	Sat Mar 31 19:07:50 2018 +0000
@@ -28,6 +28,7 @@
 void turnRight(float, bool);
 void turnLeft(float, bool);
 void rot180(); //Turns the robot around
+int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
 
 //    GLOBAL VARIABLES
 const int FORWARD = 0;
@@ -63,19 +64,14 @@
 
 int main()
 {
-    //Wait for a button press
-    //Can use an interupt
-    
     //Start a timer
     //Will need to be a variable timer based on round number
     
-    //Start Sequence - Get to the lines to start reading tokens
-    /*
-        -Travel 2 feet forward minus the radDistance plus the posDistance plus the armDistance
-        -Turn 90 degrees to the left with the left wheel stationary
-        -Back up the radDistance
     
-    */
+    float radDistance = 0.5;
+    float posDistance = 0.5;
+    float armDistance = 0.5;
+
 
      enableH = 0; //Making sure the H-Bridge starts low and off
      highH = 0; //This starts high for the H-Bridge
@@ -87,15 +83,12 @@
      }
      
      killAll.rise(&kill); //The kill interupt
+    
      
+    //   RGB Sensor Settings     
      pc.baud(115200);
-     green = 1; // off
-    
-     // Connect to the Color sensor and verify whether we connected to the correct sensor. 
-    
+     green = 1; // off  
     i2c.frequency(200000);
-    
-    //   RGB Sensor Settings
     char id_regval[1] = {146};
     char data[1] = {0};
     i2c.write(sensor_addr,id_regval,1, true);
@@ -118,15 +111,18 @@
     char enable_register[2] = {128,3};
     i2c.write(sensor_addr,enable_register,2,false);
     
-    
+    //  Initialize the robot position
+    move((0.6096-radDistance+posDistance+armDistance),FORWARD);
+    turnLeft();
+    move(radDistance,BACKWARD);
     
     while(true)
      { 
         grabToken(); 
         move(1,FORWARD);
-        turnLeft(0.35343, FORWARD);
+        turnLeft();
         wait(0.5);
-        turnLeft(0.35343, FORWARD);
+        turnLeft();
         wait(2); 
         dropToken(); 
         wait(2);
@@ -185,7 +181,7 @@
 void turnRight(float dist, bool direction)
 { 
     //Get rid of all FR occurences which will turn right motor off
-    FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
+    FLdirection = 0;    //to turn right we want this going FORWARD so a 0;
                                 
     
     stepFL.period(1/FREQUENCY); 
@@ -193,14 +189,14 @@
     
     //dist/stepSize is the number of steps
     //1/FREQUENCY is the time per step
-    wait(4*(dist/stepSize)*(1/FREQUENCY));
+    wait(4*(0.35343/stepSize)*(1/FREQUENCY));
     stepFL.period(0);
     stepFL.write(0);
 }
 void turnLeft(float 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
+    FRdirection = 1;    //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.
@@ -208,7 +204,7 @@
     
     //dist/stepSize is the number of steps
     //1/FREQUENCY is the time per step
-    wait(4*(dist/stepSize)*(1/FREQUENCY));
+    wait(4*(0.35343/stepSize)*(1/FREQUENCY));
     stepFR.period(0);
     stepFR.write(0);
 }