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.
Revision 13:2c8f8e77b549, committed 2018-04-01
- Comitter:
- alpha_tango
- Date:
- Sun Apr 01 04:19:32 2018 +0000
- Parent:
- 12:92ba52394c85
- Commit message:
- Final Update
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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
+
+}