Diff: main.cpp

Revision:

8:a6080c27f8c5

Parent:

7:1640572360de

Child:

9:a5a6d3a48145

diff -r 1640572360de -r a6080c27f8c5 main.cpp
--- a/main.cpp	Sat Mar 31 19:42:16 2018 +0000
+++ b/main.cpp	Sat Mar 31 21:43:07 2018 +0000
@@ -19,6 +19,8 @@
 DigitalOut LED(PTC4);
 DigitalOut green(LED_GREEN);
 
+Ticker time; // attach this to return home function, set according to round time
+
 
 //   PROTOTYPE FUNCTION DECLARATIONS
 void move(float dist, bool direction); 
@@ -29,6 +31,8 @@
 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
+void findPath(int color, int i); //Determines where to take the token based on the color and its relative location
+ 
 
 //    GLOBAL VARIABLES
 const int FORWARD = 0;
@@ -37,7 +41,9 @@
 const float FREQUENCY = 500; //steps per second
 int sensor_addr = 41 << 1;
 float leg; 
-int color; 
+
+
+ 
 
 
 //    NOTES
@@ -67,7 +73,7 @@
 int main()
 {
     //Start a timer
-    //Will need to be a variable timer based on round number
+    time.attach(&returnHome, TIME); 
     
     float radDistance = 0.5;
     float posDistance = 0.5;
@@ -117,12 +123,18 @@
     move((0.6096-radDistance+posDistance+armDistance),FORWARD);
     turnLeft();
     move(radDistance,BACKWARD);
+    move(0.762, FORWARD); 
+    
     
     while(true)
      { 
-        for(int i = 0; i <=8; i++)
+        for(int i = 0; i <=8; i++) //begin motion around Nth perimeter
         {   
             leg = 0.762; //2.5 feet in meters 
+            if(i % 2 == 0)
+            {
+                turnRight(); 
+            }
             grabToken(); 
             color = findColor(); 
             if (color = 9)
@@ -131,19 +143,21 @@
             }
             else 
             {
-                findPath(color)
+                findPath(color, i);
+                dropToken(); 
+                //returnPrevious(); 
             }
             move(leg, FORWARD); 
                
         }
-        grabToken(); 
+       /* grabToken(); 
         move(1,FORWARD);
         turnLeft();
         wait(0.5);
         turnLeft();
         wait(2); 
         dropToken(); 
-        wait(2);
+        wait(2);*/
      }
 }
 
@@ -157,9 +171,8 @@
         stepFR.period(1/FREQUENCY);
         stepFL.write(0.5f); 
         stepFR.write(0.5f);
-        //dist/stepSize is the number of steps
-        //1/FREQUENCY is the time per step
-        wait(4*(dist/stepSize)*(1/FREQUENCY));
+       
+        wait(4*(dist/stepSize)*(1/FREQUENCY));  //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step
         stepFL.period(0.0f);
         stepFR.period(0.0f);
         stepFL.write(0.0f);
@@ -313,4 +326,9 @@
         //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
         //wait(0.5);
     }
+void findPath(int color, int i)
+{
+    
+    
+}
 }
\ No newline at end of file