Diff: main.cpp

Revision:

10:ed6ecbb420b8

Parent:

9:a5a6d3a48145

Child:

11:93c035708249

--- a/main.cpp	Sat Mar 31 23:16:56 2018 +0000
+++ b/main.cpp	Sat Mar 31 23:57:23 2018 +0000
@@ -19,20 +19,25 @@
 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); 
 void grabToken();//Picks up the token for reading
 void dropToken();//Drops the token off
 void kill(); 
-void turnRight(float, bool);
-void turnLeft(float, bool);
+void turnRight();
+void turnRight45();
+void turnLeft();
 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, float scale, float); //Determines where to take the token based on the color and its relative location
- 
+void returnToPrevious(int, int);
+void positionIsBlue();      // uses these 6 function to return back to previous position
+void positionIsRed();
+void positionIsGreen();
+void positionIsCyan();
+void positionIsMagenta();
+void positionIsYellow();
+
 
 //    GLOBAL VARIABLES
 const int FORWARD = 0;
@@ -41,9 +46,7 @@
 const float FREQUENCY = 500; //steps per second
 int sensor_addr = 41 << 1;
 float leg; 
-
-
- 
+int color; 
 
 
 //    NOTES
@@ -73,7 +76,7 @@
 int main()
 {
     //Start a timer
-    time.attach(&returnHome, TIME); 
+    //Will need to be a variable timer based on round number
     
     float radDistance = 0.5;
     float posDistance = 0.5;
@@ -123,19 +126,12 @@
     move((0.6096-radDistance+posDistance+armDistance),FORWARD);
     turnLeft();
     move(radDistance,BACKWARD);
-    move(0.762, FORWARD); 
-    
     
     while(true)
      { 
-        for(int i = 0; i <=7; i++) //begin motion around Nth perimeter   ::No token on one length of the track so we may want to revise this
+        for(int i = 0; i <=8; i++)
         {   
             leg = 0.762; //2.5 feet in meters 
-            scale = 1; 
-            if(i % 2 == 0)
-            {
-                turnRight(); 
-            }
             grabToken(); 
             color = findColor(); 
             if (color = 9)
@@ -144,14 +140,19 @@
             }
             else 
             {
-                findPath(color, i, leg);
-                dropToken(); 
-                //returnPrevious(); 
+                findPath(color)
             }
             move(leg, FORWARD); 
                
         }
-       
+        grabToken(); 
+        move(1,FORWARD);
+        turnLeft();
+        wait(0.5);
+        turnLeft();
+        wait(2); 
+        dropToken(); 
+        wait(2);
      }
 }
 
@@ -165,8 +166,9 @@
         stepFR.period(1/FREQUENCY);
         stepFL.write(0.5f); 
         stepFR.write(0.5f);
-       
-        wait(4*(dist/stepSize)*(1/FREQUENCY));  //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step
+        //dist/stepSize is the number of steps
+        //1/FREQUENCY is the time per step
+        wait(4*(dist/stepSize)*(1/FREQUENCY));
         stepFL.period(0.0f);
         stepFR.period(0.0f);
         stepFL.write(0.0f);
@@ -203,7 +205,7 @@
         enableH = 0;    
 }
 
-void turnRight(float dist, bool direction)
+void turnRight()
 { 
     //Get rid of all FR occurences which will turn right motor off
     FLdirection = 0;    //to turn right we want this going FORWARD so a 0;
@@ -218,7 +220,22 @@
     stepFL.period(0);
     stepFL.write(0);
 }
-void turnLeft(float dist, bool direction)
+void turnRight45()
+{ 
+    //Get rid of all FR occurences which will turn right motor off
+    FLdirection = 0;    //to turn right we want this going FORWARD so a 0;
+                                
+    
+    stepFL.period(1/FREQUENCY); 
+    stepFL.write(0.5); 
+    
+    //dist/stepSize is the number of steps
+    //1/FREQUENCY is the time per step
+    wait(2*(0.35343/stepSize)*(1/FREQUENCY));
+    stepFL.period(0);
+    stepFL.write(0);
+}
+void turnLeft()
 { 
     //Get rid of all FL occurences which will turn left motor off
     FRdirection = 1;    //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
@@ -233,6 +250,7 @@
     stepFR.period(0);
     stepFR.write(0);
 }
+
 void rot180()
 {
     //Get rid of all FR occurences which will turn right motor off
@@ -320,269 +338,128 @@
         //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, float scale) //This function decides where to go based on its reletive position and the color written in from the findColor function. Indexes/Positions 3 & 7 not used
-                                             //          3          
-                                             //2*--------*--------*4
-                                             // |                 |
-                                             // |                 |
-                                             // |                 |
-                                             // |                 |
-                                             //1*                 *5
-                                             // |                 |
-                                             // |                 |
-                                             // |                 |
-                                             // |                 |
-                                             // *--------*--------*6
-                                             // 0        7   
-                                             
+}
+
+//this function will use the iterator (i) to find the position it was just at and return there
+//it will also use the color global variable to determine it's path there and the square num to determine 
+//how many steps it will need to take in once it gets to its iterator position
+// square num must = 0 for the outer square, 1 for next one in, 2 for following and 3 for innermost square. 
+
+void returnToPrevious(int i, int square_num)
 {
-    /*******************************************************************///Red
-    if( i == 0 && color == 1 )
-    {
-        //condition for return red at index 0
-        
-    }
-     if( i == 1 && color == 1 )
-    {
-        //condition for return red at index 1
-        
-    }
-     if( i == 2 && color == 1 )
+    rot180(); //gets robot back to normal orientation pointing in towards the middle
+    if(color == 2 || color == 4 || color == 6 || color == 8)
     {
-        //condition for return red at index 2
-        
-    }
-     if( i == 4 && color == 1 )
-    {
-        //condition for return red at index 4
-        
-    }
-     if( i == 5 && color == 1 )
-    {
-        //condition for return red at index 5
-        
-    }
-     if( i == 6 && color == 1 )
-    {
-        //condition for return red at index 5
-        
-    }
-     
-    /*******************************************************************///Green 
-    if( i == 0 && color == 2 )
-    {
-        //condition for return Green at index 0
-        
-    }
-     if( i == 1 && color == 2 )
-    {
-        //condition for return green at index 1
-        
-    }
-     if( i == 2 && color == 2 )
-    {
-        //condition for return green at index 2
-        
+        move(0.2173224, 0); // moves the robot forward 0.7128 feet forward which is the diagonal from color square to back on playing field
+        if(color == 2){
+            positionIsBlue();
+            turnLeft();
+            move(square_num*0.1524, 0);
+            }
+        else if(color == 3){
+            positionIsGreen();
+            }
+        else if(color == 4){
+            positionIsRed();
+            move(square_num*0.1524, 0);
+            }
+        else if(color == 6){
+            positionIsCyan();
+            move(square_num*0.1524, 0);
+            }
+        else if(color == 7){
+            positionIsMagenta();
+            }
+        else if(color == 8){
+            positionIsYellow();
+            move(square_num*0.1524, 0);
+            }       
     }
     
-     if( i == 4 && color == 2 )
-    {
-        //condition for return green at index 4
-        
-    }
-     if( i == 5 && color == 2 )
-    {
-        //condition for return green at index 5
-        
-    }
-     if( i == 6 && color == 2 )
-    {
-        //condition for return green at index 6
-        
-    }
-    /*******************************************************************///Blue
-    if( i == 0 && color == 3 )//condition for return Blue at index 0 ::
-    {
-        rot180(); 
-        move(0.3048*scale, FORWARD); 
-        turnRight(); 
-        move(0.3048*scale, FORWARD); 
-        break; 
-        
-    }
-     if( i == 1 && color == 3 ) //condition for return blue at index 1
-    {
-        rot180(); 
-        move((leg + 0.3048*scale, FORWARD); 
-        turnRight(); 
-        move(0.3048*scale, FORWARD); 
-        break;      
-        
-    }
-     if( i == 2 && color == 3 ) //condition for return blue at index 2
-    {
-        rot180(); 
-        move((2*leg + 0.3048*scale, FORWARD); 
-        turnRight();  
-        move(0.3048*scale, FORWARD); 
-        break;
-        
-    }
-     if( i == 4 && color == 3 ) //condition for return blue at index 4
-    { 
-        move((2*leg + 0.3048*scale, FORWARD); 
-        turnRight();  
-        move(2*leg + 0.3048*scale, FORWARD); 
-        break;
-        
-    }
-     if( i == 5 && color == 3 ) //condition for return blue at index 5
-    {
-        move(leg + 0.3048*scale, FORWARD); 
-        turnRight();  
-        move(2*leg + 0.3048*scale, FORWARD); 
-        break;
-        
-    }
-     if( i == 6 && color == 3 ) //condition for return blue at index 5
-    {
-       move(2*leg + 0.3048*scale);
-       turnLeft(); 
-       move(0.3048*scale, FORWARD);  
-       break;
-        
-    }
-    else break; 
-     
-    /*******************************************************************///Cyan
-    if( i == 0 && color == 4 )
-    {
-        //condition for return Cyan at index 0
-        
-    }
-     if( i == 1 && color == 4 )
-    {
-        //condition for return Cyan at index 1
-        
-    }
-     if( i == 2 && color == 4 )
-    {
-        //condition for return Cyan at index 2
-        
-    }
-     if( i == 4 && color == 4 )
-    {
-        //condition for return Cyan at index 4
-        
-    }
-     if( i == 5 && color == 4 )
-    {
-        //condition for return Cyan at index 5
-        
-    }
-     if( i == 6 && color == 4 )
-    {
-        //condition for return Cyan at index 6
-        
+    else{
+        move(0.1524, 0); // moves the robot forward half a foot, or the distance from edge of box to line
+        if(color == 3){
+            positionIsGreen();
+            move(square_num*0.1524, 0);
+            }
+        else if(color == 7){
+            positionIsMagenta();
+            move(square_num*0.1524, 0);
+            }
     }
-    else break; 
-    /*******************************************************************///Magenta
-    if( i == 0 && color == 5 )
-    {
-        //condition for return Magenta at index 0
-        
-    }
-     if( i == 1 && color == 5 )
-    {
-        //condition for return Magenta at index 1
-        
-    }
-     if( i == 2 && color == 5 )
-    {
-        //condition for return Magenta at index 2
-        
-    }
-     if( i == 4 && color == 5 )
-    {
-        //condition for return Magenta at index 4
-        
-    }
-     if( i == 5 && color == 5 )
-    {
-        //condition for return Magenta at index 5
-        
-    }
-     if( i == 6 && color == 5 )
-    {
-        //condition for return Magenta at index 6
-        
-    }
-    else break; 
-    /*******************************************************************///Yellow
-    if( i == 0 && color == 6 )
-    {
-        //condition for return Yellow at index 0
-        
-    }
-     if( i == 1 && color == 6 )
-    {
-        //condition for return Yellow at index 1
-        
-    }
-     if( i == 2 && color == 6 )
-    {
-        //condition for return Yellow at index 2
-        
-    }
-     if( i == 4 && color == 6 )
-    {
-        //condition for return Yellow at index 4
-        
-    }
-     if( i == 5 && color == 6 )
-    {
-        //condition for return Yellow at index 5
-        
-    }
-     if( i == 6 && color == 6 )
-    {
-        //condition for return Yellow at index 6
-        
-    }
-    else break; 
-    /*******************************************************************///Gray
-    if( i == 0 && color == 7 )
-    {
-        //condition for return Gray at index 0
-        
-    }
-     if( i == 1 && color == 7 )
-    {
-        //condition for return Gray at index 1
-        
-    }
-     if( i == 2 && color == 7 )
-    {
-        //condition for return Gray at index 2
-        
-    }
-     if( i == 4 && color == 7 )
-    {
-        //condition for return Gray at index 4
-        
-    }
-     if( i == 5 && color == 7 )
-    {
-        //condition for return Gray at index 5
-        
-    }
-     if( i == 6 && color == 7 )
-    {
-        //condition for return Gray at index 6
-        
-    }
-    else break; 
-    /*******************************************************************/
-    
 }
-}
\ No newline at end of file
+
+
+void positionIsBlue()
+{
+    int pos;
+    turnright45();
+    for(int j = color; j=i; j--){
+        pos = j % 8;
+        move(leg, 0);
+        if( pos%2 == 1){
+            turnLeft();
+        }
+    }          
+} 
+void positionIsGreen()
+{
+    int pos;
+    turnright();
+    for(int j = color; j=i; j--){
+        pos = j % 8;
+        move(leg, 0);
+        if( pos%2 == 1){
+            turnLeft();
+        }
+    }          
+} 
+void positionIsRed()
+{
+    int pos;
+    turnright45();
+    for(int j = color; j=i; j--){
+        pos = j % 8;
+        move(leg, 0);
+        if( pos%2 == 1){
+            turnLeft();
+        }
+    }          
+} 
+void positionIsCyan()
+{
+    int pos;
+    turnright45();
+    for(int j = color; j=i; j--){
+        pos = j % 8;
+        move(leg, 0);
+        if( pos%2 == 1){
+            turnLeft();
+        }
+    }          
+}  
+void positionIsMagenta()
+{
+    turnright();
+    for(int j = color; j=i; j++){
+        j = j%8;
+        move(leg, 0);
+        if( j == 7 || j == 5 || j == 3 || j==1){
+            turnLeft();
+        }
+    }          
+}  
+
+void positionIsYellow()
+{
+    int pos;
+    turnright45();
+    for(int j = color; j=i; j--){
+        pos = j % 8;
+        move(leg, 0);
+        if( pos%2 == 1){
+            turnLeft();
+        }
+    }          
+}  
+    
\ No newline at end of file