Alpha Tango
/
R5_Robotics
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 13:2c8f8e77b549
- Parent:
- 12:92ba52394c85
diff -r 92ba52394c85 -r 2c8f8e77b549 main.cpp
--- a/main.cpp Sun Apr 01 01:46:03 2018 +0000
+++ b/main.cpp Sun Apr 01 04:19:32 2018 +0000
@@ -19,20 +19,19 @@
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);
void grabToken();//Picks up the token for reading
void dropToken();//Drops the token off
void kill();
-void turnRight();
-void turnLeft();
+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
-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
-
+int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
+void findPath(); //Figures out the path to take to take the disk to its drop off position
+void returnHome(); //Returns to the home white square
+void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
// GLOBAL VARIABLES
const int FORWARD = 0;
@@ -40,18 +39,11 @@
const float stepSize = 0.001212; //in meters
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
+ -42 prewriten functions for the drop off decision
-Possibly use another RGB sensor as a line follower
-Decides function based on color
-findPath
@@ -76,7 +68,7 @@
int main()
{
//Start a timer
- //timer.attach(&returnHome, TIME);
+ //Will need to be a variable timer based on round number
float radDistance = 0.5;
float posDistance = 0.5;
@@ -96,21 +88,20 @@
// RGB Sensor Settings
- pc.baud(115200);
- green = 1; // off
- i2c.frequency(200000);
- char id_regval[1] = {146};
- char data[1] = {0};
- i2c.write(sensor_addr,id_regval,1, true);
- i2c.read(sensor_addr,data,1,false);
- if (data[0]==68)
- {
+ pc.baud(115200);
+ green = 1; // off
+ i2c.frequency(200000);
+ char id_regval[1] = {146};
+ char data[1] = {0};
+ i2c.write(sensor_addr,id_regval,1, true);
+ i2c.read(sensor_addr,data,1,false);
+ if (data[0]==68) {
green = 0;
wait (2);
green = 1;
} else {
green = 1;
- }
+ }
// Initialize color sensor
char timing_register[2] = {129,0};
@@ -126,35 +117,17 @@
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
- {
- leg = 0.762; //2.5 feet in meters
- scale = 1;
- if(i % 2 == 0)
- {
- turnRight();
- }
- grabToken();
- color = findColor();
- if (color == 9)
- {
- return(0);
- }
- else
- {
- findPathReturn(color, i, scale, leg);
- dropToken();
- //returnPrevious();
- }
- move(leg, FORWARD);
-
- }
-
+ grabToken();
+ move(1,FORWARD);
+ turnLeft();
+ wait(0.5);
+ turnLeft();
+ wait(2);
+ dropToken();
+ wait(2);
}
}
@@ -168,8 +141,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);
@@ -239,7 +213,7 @@
void rot180()
{
//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;
+ FLdirection = direction; //to turn right we want this going FORWARD so a 0;
stepFL.period(1/FREQUENCY);
@@ -290,7 +264,7 @@
- //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing
+ //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error
if(blue_value<10000 && red_value>10000){
return(1);
}
@@ -315,593 +289,22 @@
else if(red_value==0){
return(8);
}
- else{
- return(9);
- }
+
// print sensor readings
//pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
//wait(0.5);
}
- //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);
-{
- /*******************************************************************///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
- {
- 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();
-
- }
-
- 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 findPath(){ //Figures out the path to take to take the disk to its drop off position
+
+}
-
- }
- 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
- {
- move(2*leg + 0.3046*scale, FORWARD)
- turnRight();
- move(2*leg + 0.3046*scale, FORWARD)
- dropToken();
- rot180();
- move(2*leg + 0.3046*scale, FORWARD)
- turnLeft();
- move(2*leg + 0.3046*scale, FORWARD)
- rot180();
- }
- if( i == 1 && color == 4 )//condition for return Cyan at index 1
- {
- move(leg + 0.3046*scale, FOWARD);
- turnRight();
- move(2*leg + 0.3046*scale, FOWARD);
- dropToken();
- rot180();
- move(2*leg + 0.3046*scale, FOWARD);
- turnleft();
- move(leg + 0.3046*scale, FOWARD);
- rot180();
-
- }
- if( i == 2 && color == 4 )//condition for return Cyan at index 2
- {
- move(2*leg + 0.3046*scale, FOWARD);
- turnLeft();
- move(0.3046*scale);
- dropToken();
- rot180();
- move(0.3046*scale);
- turnRight();
- move(2*leg + 0.3046*scale, FOWARD);
- rot180();
-
- }
- if( i == 4 && color == 4 )//condition for return Cyan at index 4
- {
- rot180();
- move(0.3046*scale, FORWARD);
- turnRight();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180();
- move(0.3046*scale, FORWARD);
- turnLeft();
- move(0.3046*scale, FORWARD);
- turnleft();
- move(0.3046*scale, FORWARD);
- turnRight();
-
- }
- if( i == 5 && color == 4 )//condition for return Cyan at index 5
- {
- rot180();
- move(leg + 0.3046*scale, FORWARD);
- turnRight();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180();
- move(0.3046*scale, FORWARD);
- turnLeft();
- move(leg + 0.3046*scale, FORWARD);
-
- }
- if( i == 6 && color == 4 )//condition for return Cyan at index 6
- {
- turnRight();
- move(2*leg + 0.3046*scale, FORWARD);
- turnRight();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180();
- move(0.3046*scale, FORWARD);
- turnLeft();
- move(leg + 0.3046*scale, FORWARD);
- turnRight();
-
- }
- /*******************************************************************///Magenta
- if( i == 0 && color == 5 )//condition for return Magenta at index 0
- {
- move(leg, FORWARD);
- turnRight();
- move(2*leg + 0.3046*scale);
- dropToken();
- rot180();
- move(2*leg + 0.3046*scale);
- turnLeft();
- move(leg, FORWARD);
- rot180();
-
- }
- if( i == 1 && color == 5 )//condition for return Magenta at index 1
- {
- turnRight();
- move(2*leg + 0.3046*scale);
- dropToken();
- rot180();
- move(2*leg + 0.3046*scale);
- turnRight();
-
- }
- if( i == 2 && color == 5 )//condition for return Magenta at index 2
- {
- move(leg + 0.3046*scale, FORWARD);
- turnRight();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- turnLeft();
- move(2*leg + 0.3046*scalem FORWARD);
- rot180();
-
- }
- if( i == 4 && color == 5 )//condition for return Magenta at index 4
- {
- move(leg, FORWARD);
- turnLeft();
- move(0.3046*scale);
- dropToken();
- rot180();
- move(0.3046*scale);
- turnRight();
- Move(leg, FORWARD);
- rot180();
-
- }
- if( i == 5 && color == 5 )//condition for return Magenta at index 5
- {
- turnLeft();
- move(0.3046*scale);
- dropToken();
- rot180();
- move(0.3046*scale);
- turnLeft();
-
- }
- if( i == 6 && color == 5 )//condition for return Magenta at index 6
- {
- rot180();
- move(leg, FORWARD);
- turnRight();
- move(0.3046*scale);
- dropToken();
- rot180();
- move(0.3046*scale);
- turnLeft();
- move(lef, FORWARD);
- turnRight();
-
- }
- /*******************************************************************///Yellow
- if( i == 0 && color == 6 )//condition for return Yellow at index 0
- {
- turnRight();
- move(2*leg + 0.3046*scale);
- turnRight();
- move(0.3046*scale);
- dropToken();
- rot180();
- move(0.3046*scale);
- turnLeft();
- move(2*leg + 0.3046*scale);
- turnRight();
-
- }
- if( i == 1 && color == 6 )//condition for return Yellow at index 1
- {
- rot180();
- move(leg + 0.3046*scale, FORWARD);
- turnLeft();
- move(2*leg, FORWARD);
- dropToken();
- rot180();
- move(2*leg, FOWARD);
- turnRight();
- move(leg + 0.3046*scale, FORWARD);
-
-
- }
- if( i == 2 && color == 6 )//condition for return Yellow at index 2
- {
- move(2*leg + 0.3046*scale, FORWARD);
- turnRight();
- move(2*leg, FORWARD);
- dropToken();
- rot180();
- move(2*leg, FORWARD);
- turnLeft();
- move(2*leg + 0.3046*scale, FORWARD);
- rot180();
-
- }
- if( i == 4 && color == 6 )//condition for return Yellow at index 4
- {
- move(2*leg + 0.3046*scale, FORWARD);
- turnLeft();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180(0);
- move(0.3046*scale, FORWARD);
- turnRight();
- move(2*leg + 0.3046*scale, FORWARD);
- rot180();
-
-
- }
- if( i == 5 && color == 6 )//condition for return Yellow at index 5
- {
- move(leg + 0.3046*scale, FORWARD);
- turnLeft();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180();
- move(0.3046*scale, FORWARD);
- turnRight();
- move(Leg + 0.3046*scale, FORWARD);
- rot180();
-
-
- }
- if( i == 6 && color == 6 )//condition for return Yellow at index 6
- {
- rot180();
- move(0.3046*scale, FORWARD);
- turnRight();
- move(0.3046*scale, FORWARD);
- dropToken();
- rot180();
- move(0.3046*scale, FORWARD);
- turnLeft();
- move(0.3046*scale, FORWARD);
-
- }
- /*******************************************************************///Gray
- if( i == 0 && color == 7 )//condition for return Gray at index 0
- {
- move(leg, FORWARD);
- turnRight();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- turnLeft();
- move(leg, FORWARD);
-
-
- }
- if( i == 1 && color == 7 )//condition for return Gray at index 1
- {
- turnRight();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- turnRight();
- }
- if( i == 2 && color == 7 ) //condition for return Gray at index 2
- {
- rot180();
- move(leg, FORWARD);
- turnLeft();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- turnRight();
- move(leg, FORWARD);
- turnRight();
-
- }
- if( i == 4 && color == 7 ) //condition for return Gray at index 4
- {
- move(leg, FORWARD);
- turnRight();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORARD);
- turnLeft();
- move(lef, FORWARD);
- rot180();
-
- }
- if( i == 5 && color == 7 )//condition for return Gray at index 5
- {
- turnRight();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- turnRight();
-
- }
- if( i == 6 && color == 7 )//condition for return Gray at index 6
- {
- turnRight();
- move(leg, FORWARD);
- turnLeft();
- move(leg, FORWARD);
- dropToken();
- rot180();
- move(leg, FORWARD);
- rightRight();
- move(leg, FORWARD);
- turnRight();
-
- }
- else
- {}
- /*******************************************************************/
-
+void returnHome(){ //Returns to the home white square
+
}
-}
\ No newline at end of file
+
+void returnPrevPos(){ //Does the opposite of findPath() to return to the previous position
+
+}