Diff: main.cpp

Revision:

11:93c035708249

Parent:

10:ed6ecbb420b8

Child:

12:92ba52394c85

--- a/main.cpp	Sat Mar 31 23:57:23 2018 +0000
+++ b/main.cpp	Sun Apr 01 00:50:47 2018 +0000
@@ -19,6 +19,8 @@
 DigitalOut LED(PTC4);
 DigitalOut green(LED_GREEN);
 
+Ticker timer; // attach this to return home function, set according to round time
+
 
 //   PROTOTYPE FUNCTION DECLARATIONS
 void move(float dist, bool direction); 
@@ -26,18 +28,11 @@
 void dropToken();//Drops the token off
 void kill(); 
 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 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();
-
+void findPathReturn(int color, int i, float scale, float); //Determines where to take the token based on the color and its relative location and then returns to the previous position 
+ 
 
 //    GLOBAL VARIABLES
 const int FORWARD = 0;
@@ -46,9 +41,14 @@
 const float FREQUENCY = 500; //steps per second
 int sensor_addr = 41 << 1;
 float leg; 
+float scale; 
 int color; 
 
 
+
+ 
+
+
 //    NOTES
 /*
     -36 prewriten functions for the drop off decision
@@ -76,7 +76,7 @@
 int main()
 {
     //Start a timer
-    //Will need to be a variable timer based on round number
+    //timer.attach(&returnHome, TIME); 
     
     float radDistance = 0.5;
     float posDistance = 0.5;
@@ -126,33 +126,35 @@
     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 <=7; i++) //begin motion around Nth perimeter   ::No token on one length of the track so we may want to revise this
         {   
             leg = 0.762; //2.5 feet in meters 
+            scale = 1; 
+            if(i % 2 == 0)
+            {
+                turnRight(); 
+            }
             grabToken(); 
             color = findColor(); 
-            if (color = 9)
+            if (color == 9)
             {
-                break;
+                return(0);
             }
             else 
             {
-                findPath(color)
+                findPathReturn(color, i, scale, leg);
+                dropToken(); 
+                //returnPrevious(); 
             }
             move(leg, FORWARD); 
                
         }
-        grabToken(); 
-        move(1,FORWARD);
-        turnLeft();
-        wait(0.5);
-        turnLeft();
-        wait(2); 
-        dropToken(); 
-        wait(2);
+       
      }
 }
 
@@ -166,9 +168,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);
@@ -205,7 +206,7 @@
         enableH = 0;    
 }
 
-void turnRight()
+void turnRight(float dist, bool direction)
 { 
     //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;
@@ -220,22 +221,7 @@
     stepFL.period(0);
     stepFL.write(0);
 }
-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()
+void turnLeft(float dist, bool direction)
 { 
     //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
@@ -250,11 +236,10 @@
     stepFR.period(0);
     stepFR.write(0);
 }
-
 void rot180()
 {
     //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); 
@@ -338,128 +323,396 @@
         //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
         //wait(0.5);
     }
-}
-
-//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)
+ //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   
+void findPathReturn(int color, int i, float scale, float leg);                                           
 {
-    rot180(); //gets robot back to normal orientation pointing in towards the middle
-    if(color == 2 || color == 4 || color == 6 || color == 8)
+    /*******************************************************************///Red
+    if(  i == 0 && color == 1 )//condition for return red at index 0
+    {
+        move((2*leg + 0.3048*scale), FORWARD); 
+        turnLeft(); 
+        move((0.3048*scale), FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move((0.3048*scale), FORWARD);
+        turnRight(); 
+        move((2*leg + 0.3048*scale), FORWARD); 
+        rot180();
+           
+    }
+     if( i == 1 && color == 1 )//condition for return red at index 1
+    {
+        move((leg + 0.3048*scale), FORWARD); 
+        turnLeft(); 
+        move((0.3048*scale), FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move((0.3048*scale), FORWARD); 
+        turnRight(); 
+        move((leg + 0.3048*scale), FORWARD);
+        rot180(); 
+        
+        
+    }
+     if( i == 2 && color == 1 )//condition for return red at index 2
+    {
+        rot180(); 
+        move((0.3048*scale), FORWARD); 
+        turnRight(); 
+        move((0.3048*scale), FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move((0.3048*scale), FORWARD); 
+        turnLeft(); 
+        move((0.3048*scale), FORWARD);
+        
+        
+    }
+     if( i == 4 && color == 1 )//condition for return red at index 4
     {
-        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);
-            }       
+        turnRight(); 
+        move(2*leg + 0.3048*scale, FOWARD);
+        turnRight();
+        move(0.3046*scale, FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move(0.3046*scale, FORWARD);
+        turnLeft();
+        move(2*leg + 0.3048*scale, FOWARD);
+        turnRight();
+           
+    }
+     if( i == 5 && color == 1 )
+    {
+        
+        rot180(); 
+        move(leg + 0.3046*scale, FORWARD); 
+        turnLeft();
+        move(2*leg + 0.3046*scale, FORWARD); 
+        dropToken(); 
+        rot180();
+        move(2*leg + 0.3046*scale, FORWARD); 
+        turnRight(); 
+        move(leg + 0.3046*scale, FORWARD); 
+    }
+     if( i == 6 && color == 1 ) //condition for return red at index 5
+    {
+        turnRight(); 
+        move(2*leg + 0.3046*scale, FORWARD); 
+        turnLeft();
+        move(2*leg + 0.3046*scale, FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move(2*leg + 0.3046*scale, FORWARD); 
+        turnRight(); 
+        move(2*leg + 0.3046*scale, FORWARD);
+        
+    }
+     
+    /*******************************************************************///Green 
+    if( i == 0 && color == 2 )//condition for return Green at index 0
+    {
+        move(leg, FORWARD); 
+        turnLeft(); 
+        move(0.3046*scale); 
+        dropToken(); 
+        rot180(); 
+        move(0.3046*scale);
+        turnRight();
+        move(leg, FORWARD); 
+        rot180(); 
+        
+    }
+     if( i == 1 && color == 2 )//condition for return green at index 1
+    {
+        turnLeft(); 
+        move(0.3046*scale); 
+        dropToken(); 
+        rot180(); 
+        move(0.3046*scale);
+        turnLeft(); 
+        
+    }
+     if( i == 2 && color == 2 )//condition for return green at index 2
+    {
+        turnRight(); 
+        move(leg, FORWARD); 
+        turnRight(); 
+        move(0.3046*scale); 
+        dropToken(); 
+        rot180(); 
+        move(0.3046*scale);
+        turnLeft(); 
+        move(leg, FORWARD); 
+        turnRight(); 
+        
     }
     
-    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);
-            }
+     if( i == 4 && color == 2 ) //condition for return green at index 4
+    {
+       turnRight(); 
+       move(2*leg + 0.3046*scale, FORWARD); 
+       turnLeft(); 
+       move(leg, FORWARD); 
+       dropToken(); 
+       rot180(); 
+       move(leg, FORWARD); 
+       turnRight(); 
+       move(2*leg + 0.3046*scale, FORWARD);
+       turnRight(); 
+       
+       
+        
+    }
+     if( i == 5 && color == 2 )//condition for return green at index 5
+    {
+        turnRight(); 
+        move(2*leg + 0.3046*scale, FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move(2*leg + 0.3046*scale, FORWARD); 
+        turnRight(); 
+        
+    }
+     if( i == 6 && color == 2 )//condition for return green at index 6
+    {
+        move(2*leg + 0.3046*scale, FORWARD); 
+        turnRight(); 
+        move(leg + 0.3046*scale, FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move(leg + 0.3046*scale, FORWARD); 
+        turnLeft(); 
+        move(leg + 0.3046*scale, FORWARD);
+        rot180();  
+    }
+    /*******************************************************************///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); 
+        dropToken(); 
+        rot180(); 
+        move(0.3048*scale, FORWARD); 
+        turnLeft(); 
+        move(0.3048*scale, FORWARD); 
+        
     }
-}
+     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);  
+        dropToken();  
+        rot180(); 
+        move(0.3048*scale, FORWARD); 
+        turnLeft(); 
+        move((leg + 0.3048*scale), FORWARD);    
+        
+    }
+     if( i == 2 && color == 3 ) //condition for return blue at index 2
+    {
+        turnRight();  
+        move((2*leg + 0.3048*scale), FORWARD); 
+        turnRight();  
+        move(0.3048*scale, FORWARD);
+        dropToken(); 
+        rot180(); 
+        move(0.3048*scale, FORWARD);
+        turnLeft();  
+        move((2*leg + 0.3048*scale), FORWARD); 
+        turnRight(); 
+         
+        
+    }
+     if( i == 4 && color == 3 ) //condition for return blue at index 4
+    { 
+        turnRight();
+        move((2*leg + 0.3048*scale), FORWARD); 
+        turnRight();  
+        move((2*leg) + (0.3048*scale), FORWARD); 
+        dropToken();
+        rot180(); 
+        move((2*leg + 0.3048*scale), FORWARD); 
+        turnLeft();  
+        move((2*leg) + (0.3048*scale), FORWARD); 
+        turnRight();
 
+        
+    }
+     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); 
+        dropToken();
+        rot180(); 
+        move(2*leg + 0.3048*scale, FORWARD); 
+        turnLeft();  
+        move(leg + 0.3048*scale, FORWARD); 
+        rot180(); 
+        
+        
 
-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
+        
+    }
+     if( i == 6 && color == 3 ) //condition for return blue at index 5
+    {
+        move(2*leg + 0.3048*scale, FORWARD);
+        turnLeft(); 
+        move(0.3048*scale, FORWARD); 
+        dropToken(); 
+        rot180(); 
+        move(0.3048*scale, FORWARD);
+        turnRight(); 
+        move(2*leg + 0.3048*scale, FORWARD);
+        rot180(); 
+             
+    }
+    /*******************************************************************///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
+        
+    }
+    /*******************************************************************///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
+        
+    }
+    /*******************************************************************///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
+        
+    }
+    /*******************************************************************///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 
+    {} 
+    /*******************************************************************/
+    
+}
+}
\ No newline at end of file