Alpha Tango / Mbed 2 deprecated 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.

Dependencies:   mbed

main.cpp@9:a5a6d3a48145, 2018-03-31 (annotated)

Committer:
alpha_tango
Date:
Sat Mar 31 23:16:56 2018 +0000
Revision:
9:a5a6d3a48145
Parent:
8:a6080c27f8c5
Child:
10:ed6ecbb420b8
Fully set up sort function for the color blue at each position of the map ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alpha_tango 0:7e54f0e2be67 1 #include "mbed.h"
alpha_tango 0:7e54f0e2be67 2
alpha_tango 4:9b1c6b9dae1c 3
alpha_tango 4:9b1c6b9dae1c 4 // PIN DECLARATIONS
alpha_tango 0:7e54f0e2be67 5 DigitalOut FLdirection(PTB18);
alpha_tango 0:7e54f0e2be67 6 DigitalOut FRdirection(PTA4);
alpha_tango 0:7e54f0e2be67 7 DigitalOut magDirection(PTB19);
alpha_tango 0:7e54f0e2be67 8 PwmOut stepFL(PTD3);
alpha_tango 0:7e54f0e2be67 9 PwmOut stepFR(PTA5);
alpha_tango 0:7e54f0e2be67 10 PwmOut magArm(PTA12);
alpha_tango 0:7e54f0e2be67 11 InterruptIn killAll(PTC3);
alpha_tango 1:4ab886b72870 12 DigitalIn Start(PTC12);
alpha_tango 0:7e54f0e2be67 13 DigitalOut enableH(PTC11);
alpha_tango 0:7e54f0e2be67 14 DigitalOut highH(PTC10);
alpha_tango 4:9b1c6b9dae1c 15 DigitalOut enableL(PTC11);
alpha_tango 4:9b1c6b9dae1c 16 DigitalOut highL(PTC7);
alpha_tango 4:9b1c6b9dae1c 17 I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
alpha_tango 4:9b1c6b9dae1c 18 Serial pc(USBTX, USBRX);
alpha_tango 4:9b1c6b9dae1c 19 DigitalOut LED(PTC4);
alpha_tango 4:9b1c6b9dae1c 20 DigitalOut green(LED_GREEN);
alpha_tango 0:7e54f0e2be67 21
alpha_tango 8:a6080c27f8c5 22 Ticker time; // attach this to return home function, set according to round time
alpha_tango 8:a6080c27f8c5 23
alpha_tango 4:9b1c6b9dae1c 24
alpha_tango 4:9b1c6b9dae1c 25 // PROTOTYPE FUNCTION DECLARATIONS
alpha_tango 4:9b1c6b9dae1c 26 void move(float dist, bool direction);
alpha_tango 4:9b1c6b9dae1c 27 void grabToken();//Picks up the token for reading
alpha_tango 4:9b1c6b9dae1c 28 void dropToken();//Drops the token off
alpha_tango 4:9b1c6b9dae1c 29 void kill();
alpha_tango 4:9b1c6b9dae1c 30 void turnRight(float, bool);
alpha_tango 4:9b1c6b9dae1c 31 void turnLeft(float, bool);
alpha_tango 4:9b1c6b9dae1c 32 void rot180(); //Turns the robot around
alpha_tango 5:17a8d8395a50 33 int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 9:a5a6d3a48145 34 void findPath(int color, int i, float scale, float); //Determines where to take the token based on the color and its relative location
alpha_tango 8:a6080c27f8c5 35
alpha_tango 4:9b1c6b9dae1c 36
alpha_tango 4:9b1c6b9dae1c 37 // GLOBAL VARIABLES
alpha_tango 4:9b1c6b9dae1c 38 const int FORWARD = 0;
alpha_tango 4:9b1c6b9dae1c 39 const int BACKWARD = 1;
alpha_tango 4:9b1c6b9dae1c 40 const float stepSize = 0.001212; //in meters
alpha_tango 4:9b1c6b9dae1c 41 const float FREQUENCY = 500; //steps per second
alpha_tango 4:9b1c6b9dae1c 42 int sensor_addr = 41 << 1;
alpha_tango 7:1640572360de 43 float leg;
alpha_tango 8:a6080c27f8c5 44
alpha_tango 8:a6080c27f8c5 45
alpha_tango 8:a6080c27f8c5 46
alpha_tango 4:9b1c6b9dae1c 47
alpha_tango 4:9b1c6b9dae1c 48
alpha_tango 4:9b1c6b9dae1c 49 // NOTES
alpha_tango 0:7e54f0e2be67 50 /*
alpha_tango 0:7e54f0e2be67 51 -36 prewriten functions for the drop off decision
alpha_tango 0:7e54f0e2be67 52 -Possibly use another RGB sensor as a line follower
alpha_tango 0:7e54f0e2be67 53 -Decides function based on color
alpha_tango 0:7e54f0e2be67 54 -findPath
alpha_tango 0:7e54f0e2be67 55 -starting at bottomLeft
alpha_tango 0:7e54f0e2be67 56 -travel up one leg
alpha_tango 0:7e54f0e2be67 57 -turn right if rgb
alpha_tango 0:7e54f0e2be67 58 -turn left if cmy
alpha_tango 0:7e54f0e2be67 59 -turn right, left, or stay based on color choice
alpha_tango 4:9b1c6b9dae1c 60
alpha_tango 0:7e54f0e2be67 61 void findPath(); //Figures out the path to take to take the disk to its drop off position
alpha_tango 0:7e54f0e2be67 62 void returnHome(); //Returns to the home white square
alpha_tango 0:7e54f0e2be67 63 void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
alpha_tango 0:7e54f0e2be67 64
alpha_tango 0:7e54f0e2be67 65 //Variables
alpha_tango 4:9b1c6b9dae1c 66
alpha_tango 0:7e54f0e2be67 67 -boxSizes, 2x2, 3x3, 4x4, etc
alpha_tango 0:7e54f0e2be67 68 -legSize, 1 foot, 1.5 feet, 2 feet, etc.
alpha_tango 0:7e54f0e2be67 69 -direction choices for findPath
alpha_tango 0:7e54f0e2be67 70 */
alpha_tango 0:7e54f0e2be67 71
alpha_tango 0:7e54f0e2be67 72
alpha_tango 0:7e54f0e2be67 73 int main()
alpha_tango 0:7e54f0e2be67 74 {
alpha_tango 0:7e54f0e2be67 75 //Start a timer
alpha_tango 8:a6080c27f8c5 76 time.attach(&returnHome, TIME);
alpha_tango 0:7e54f0e2be67 77
alpha_tango 5:17a8d8395a50 78 float radDistance = 0.5;
alpha_tango 5:17a8d8395a50 79 float posDistance = 0.5;
alpha_tango 5:17a8d8395a50 80 float armDistance = 0.5;
alpha_tango 5:17a8d8395a50 81
alpha_tango 0:7e54f0e2be67 82
alpha_tango 0:7e54f0e2be67 83 enableH = 0; //Making sure the H-Bridge starts low and off
alpha_tango 4:9b1c6b9dae1c 84 highH = 0; //This starts high for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 85 highL = 1; //This starts low for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 86 while(true) //The start button
alpha_tango 1:4ab886b72870 87 {
alpha_tango 1:4ab886b72870 88 if (Start == 0)
alpha_tango 1:4ab886b72870 89 break;
alpha_tango 1:4ab886b72870 90 }
alpha_tango 4:9b1c6b9dae1c 91
alpha_tango 4:9b1c6b9dae1c 92 killAll.rise(&kill); //The kill interupt
alpha_tango 5:17a8d8395a50 93
alpha_tango 4:9b1c6b9dae1c 94
alpha_tango 5:17a8d8395a50 95 // RGB Sensor Settings
alpha_tango 4:9b1c6b9dae1c 96 pc.baud(115200);
alpha_tango 5:17a8d8395a50 97 green = 1; // off
alpha_tango 7:1640572360de 98 i2c.frequency(200000);
alpha_tango 7:1640572360de 99 char id_regval[1] = {146};
alpha_tango 7:1640572360de 100 char data[1] = {0};
alpha_tango 7:1640572360de 101 i2c.write(sensor_addr,id_regval,1, true);
alpha_tango 7:1640572360de 102 i2c.read(sensor_addr,data,1,false);
alpha_tango 7:1640572360de 103 if (data[0]==68)
alpha_tango 7:1640572360de 104 {
alpha_tango 4:9b1c6b9dae1c 105 green = 0;
alpha_tango 4:9b1c6b9dae1c 106 wait (2);
alpha_tango 4:9b1c6b9dae1c 107 green = 1;
alpha_tango 4:9b1c6b9dae1c 108 } else {
alpha_tango 4:9b1c6b9dae1c 109 green = 1;
alpha_tango 7:1640572360de 110 }
alpha_tango 4:9b1c6b9dae1c 111
alpha_tango 4:9b1c6b9dae1c 112 // Initialize color sensor
alpha_tango 4:9b1c6b9dae1c 113 char timing_register[2] = {129,0};
alpha_tango 4:9b1c6b9dae1c 114 i2c.write(sensor_addr,timing_register,2,false);
alpha_tango 4:9b1c6b9dae1c 115
alpha_tango 4:9b1c6b9dae1c 116 char control_register[2] = {143,0};
alpha_tango 4:9b1c6b9dae1c 117 i2c.write(sensor_addr,control_register,2,false);
alpha_tango 4:9b1c6b9dae1c 118
alpha_tango 4:9b1c6b9dae1c 119 char enable_register[2] = {128,3};
alpha_tango 4:9b1c6b9dae1c 120 i2c.write(sensor_addr,enable_register,2,false);
alpha_tango 4:9b1c6b9dae1c 121
alpha_tango 5:17a8d8395a50 122 // Initialize the robot position
alpha_tango 5:17a8d8395a50 123 move((0.6096-radDistance+posDistance+armDistance),FORWARD);
alpha_tango 5:17a8d8395a50 124 turnLeft();
alpha_tango 5:17a8d8395a50 125 move(radDistance,BACKWARD);
alpha_tango 8:a6080c27f8c5 126 move(0.762, FORWARD);
alpha_tango 8:a6080c27f8c5 127
alpha_tango 0:7e54f0e2be67 128
alpha_tango 0:7e54f0e2be67 129 while(true)
alpha_tango 4:9b1c6b9dae1c 130 {
alpha_tango 9:a5a6d3a48145 131 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
alpha_tango 7:1640572360de 132 {
alpha_tango 7:1640572360de 133 leg = 0.762; //2.5 feet in meters
alpha_tango 9:a5a6d3a48145 134 scale = 1;
alpha_tango 8:a6080c27f8c5 135 if(i % 2 == 0)
alpha_tango 8:a6080c27f8c5 136 {
alpha_tango 8:a6080c27f8c5 137 turnRight();
alpha_tango 8:a6080c27f8c5 138 }
alpha_tango 7:1640572360de 139 grabToken();
alpha_tango 7:1640572360de 140 color = findColor();
alpha_tango 7:1640572360de 141 if (color = 9)
alpha_tango 7:1640572360de 142 {
alpha_tango 7:1640572360de 143 break;
alpha_tango 7:1640572360de 144 }
alpha_tango 7:1640572360de 145 else
alpha_tango 7:1640572360de 146 {
alpha_tango 9:a5a6d3a48145 147 findPath(color, i, leg);
alpha_tango 8:a6080c27f8c5 148 dropToken();
alpha_tango 8:a6080c27f8c5 149 //returnPrevious();
alpha_tango 7:1640572360de 150 }
alpha_tango 7:1640572360de 151 move(leg, FORWARD);
alpha_tango 7:1640572360de 152
alpha_tango 7:1640572360de 153 }
alpha_tango 9:a5a6d3a48145 154
alpha_tango 0:7e54f0e2be67 155 }
alpha_tango 0:7e54f0e2be67 156 }
alpha_tango 4:9b1c6b9dae1c 157
alpha_tango 0:7e54f0e2be67 158 //Distance is in meters
alpha_tango 4:9b1c6b9dae1c 159 void move(float dist, bool direction)
alpha_tango 0:7e54f0e2be67 160 {
alpha_tango 0:7e54f0e2be67 161 FLdirection = direction;
alpha_tango 0:7e54f0e2be67 162 FRdirection = !direction;
alpha_tango 0:7e54f0e2be67 163
alpha_tango 4:9b1c6b9dae1c 164 stepFL.period(1.0/FREQUENCY);
alpha_tango 0:7e54f0e2be67 165 stepFR.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 166 stepFL.write(0.5f);
alpha_tango 4:9b1c6b9dae1c 167 stepFR.write(0.5f);
alpha_tango 8:a6080c27f8c5 168
alpha_tango 8:a6080c27f8c5 169 wait(4*(dist/stepSize)*(1/FREQUENCY)); //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 170 stepFL.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 171 stepFR.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 172 stepFL.write(0.0f);
alpha_tango 4:9b1c6b9dae1c 173 stepFR.write(0.0f);
alpha_tango 0:7e54f0e2be67 174
alpha_tango 0:7e54f0e2be67 175 }
alpha_tango 4:9b1c6b9dae1c 176
alpha_tango 0:7e54f0e2be67 177 void grabToken()
alpha_tango 0:7e54f0e2be67 178 {
alpha_tango 4:9b1c6b9dae1c 179 highL = 0;
alpha_tango 4:9b1c6b9dae1c 180 highH = 1;
alpha_tango 0:7e54f0e2be67 181 enableH = 1;
alpha_tango 0:7e54f0e2be67 182 wait(1);
alpha_tango 0:7e54f0e2be67 183 magDirection = 1;
alpha_tango 0:7e54f0e2be67 184 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 185 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 186 wait(0.65);
alpha_tango 0:7e54f0e2be67 187 magArm.period(0);
alpha_tango 0:7e54f0e2be67 188 magArm.write(0);
alpha_tango 0:7e54f0e2be67 189
alpha_tango 0:7e54f0e2be67 190 }
alpha_tango 4:9b1c6b9dae1c 191
alpha_tango 0:7e54f0e2be67 192 void dropToken()
alpha_tango 0:7e54f0e2be67 193 {
alpha_tango 0:7e54f0e2be67 194 magDirection = 0;
alpha_tango 0:7e54f0e2be67 195 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 196 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 197 wait(0.65);
alpha_tango 0:7e54f0e2be67 198 magArm.period(0);
alpha_tango 4:9b1c6b9dae1c 199 magArm.write(0);
alpha_tango 4:9b1c6b9dae1c 200 highL = 1;
alpha_tango 4:9b1c6b9dae1c 201 highH = 0;
alpha_tango 4:9b1c6b9dae1c 202 wait(2);
alpha_tango 4:9b1c6b9dae1c 203 enableH = 0;
alpha_tango 0:7e54f0e2be67 204 }
alpha_tango 4:9b1c6b9dae1c 205
alpha_tango 4:9b1c6b9dae1c 206 void turnRight(float dist, bool direction)
alpha_tango 2:5f29bc7daa49 207 {
alpha_tango 2:5f29bc7daa49 208 //Get rid of all FR occurences which will turn right motor off
alpha_tango 5:17a8d8395a50 209 FLdirection = 0; //to turn right we want this going FORWARD so a 0;
alpha_tango 2:5f29bc7daa49 210
alpha_tango 2:5f29bc7daa49 211
alpha_tango 2:5f29bc7daa49 212 stepFL.period(1/FREQUENCY);
alpha_tango 2:5f29bc7daa49 213 stepFL.write(0.5);
alpha_tango 2:5f29bc7daa49 214
alpha_tango 2:5f29bc7daa49 215 //dist/stepSize is the number of steps
alpha_tango 2:5f29bc7daa49 216 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 217 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 2:5f29bc7daa49 218 stepFL.period(0);
alpha_tango 2:5f29bc7daa49 219 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 220 }
alpha_tango 4:9b1c6b9dae1c 221 void turnLeft(float dist, bool direction)
alpha_tango 3:d3264a6f7a62 222 {
alpha_tango 3:d3264a6f7a62 223 //Get rid of all FL occurences which will turn left motor off
alpha_tango 5:17a8d8395a50 224 FRdirection = 1; //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
alpha_tango 3:d3264a6f7a62 225
alpha_tango 3:d3264a6f7a62 226
alpha_tango 3:d3264a6f7a62 227 stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
alpha_tango 3:d3264a6f7a62 228 stepFR.write(0.5);
alpha_tango 3:d3264a6f7a62 229
alpha_tango 3:d3264a6f7a62 230 //dist/stepSize is the number of steps
alpha_tango 3:d3264a6f7a62 231 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 232 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 3:d3264a6f7a62 233 stepFR.period(0);
alpha_tango 3:d3264a6f7a62 234 stepFR.write(0);
alpha_tango 3:d3264a6f7a62 235 }
alpha_tango 0:7e54f0e2be67 236 void rot180()
alpha_tango 0:7e54f0e2be67 237 {
alpha_tango 4:9b1c6b9dae1c 238 //Get rid of all FR occurences which will turn right motor off
alpha_tango 4:9b1c6b9dae1c 239 FLdirection = direction; //to turn right we want this going FORWARD so a 0;
alpha_tango 4:9b1c6b9dae1c 240
alpha_tango 4:9b1c6b9dae1c 241
alpha_tango 4:9b1c6b9dae1c 242 stepFL.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 243 stepFL.write(0.5);
alpha_tango 4:9b1c6b9dae1c 244
alpha_tango 4:9b1c6b9dae1c 245 //dist/stepSize is the number of steps
alpha_tango 4:9b1c6b9dae1c 246 //1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 247 wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 248 stepFL.period(0);
alpha_tango 4:9b1c6b9dae1c 249 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 250 }
alpha_tango 0:7e54f0e2be67 251 void kill()
alpha_tango 0:7e54f0e2be67 252 {
alpha_tango 0:7e54f0e2be67 253 exit(0);
alpha_tango 0:7e54f0e2be67 254 }
alpha_tango 4:9b1c6b9dae1c 255
alpha_tango 4:9b1c6b9dae1c 256 int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 4:9b1c6b9dae1c 257
alpha_tango 4:9b1c6b9dae1c 258 while (true) {
alpha_tango 4:9b1c6b9dae1c 259 wait(1);
alpha_tango 4:9b1c6b9dae1c 260 char clear_reg[1] = {148};
alpha_tango 4:9b1c6b9dae1c 261 char clear_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 262 i2c.write(sensor_addr,clear_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 263 i2c.read(sensor_addr,clear_data,2, false);
alpha_tango 4:9b1c6b9dae1c 264
alpha_tango 4:9b1c6b9dae1c 265 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
alpha_tango 4:9b1c6b9dae1c 266
alpha_tango 4:9b1c6b9dae1c 267 char red_reg[1] = {150};
alpha_tango 4:9b1c6b9dae1c 268 char red_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 269 i2c.write(sensor_addr,red_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 270 i2c.read(sensor_addr,red_data,2, false);
alpha_tango 4:9b1c6b9dae1c 271
alpha_tango 4:9b1c6b9dae1c 272 int red_value = ((int)red_data[1] << 8) | red_data[0];
alpha_tango 4:9b1c6b9dae1c 273
alpha_tango 4:9b1c6b9dae1c 274 char green_reg[1] = {152};
alpha_tango 4:9b1c6b9dae1c 275 char green_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 276 i2c.write(sensor_addr,green_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 277 i2c.read(sensor_addr,green_data,2, false);
alpha_tango 4:9b1c6b9dae1c 278
alpha_tango 4:9b1c6b9dae1c 279 int green_value = ((int)green_data[1] << 8) | green_data[0];
alpha_tango 4:9b1c6b9dae1c 280
alpha_tango 4:9b1c6b9dae1c 281 char blue_reg[1] = {154};
alpha_tango 4:9b1c6b9dae1c 282 char blue_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 283 i2c.write(sensor_addr,blue_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 284 i2c.read(sensor_addr,blue_data,2, false);
alpha_tango 4:9b1c6b9dae1c 285
alpha_tango 4:9b1c6b9dae1c 286 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
alpha_tango 4:9b1c6b9dae1c 287
alpha_tango 4:9b1c6b9dae1c 288
alpha_tango 4:9b1c6b9dae1c 289
alpha_tango 7:1640572360de 290 //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing
alpha_tango 4:9b1c6b9dae1c 291 if(blue_value<10000 && red_value>10000){
alpha_tango 4:9b1c6b9dae1c 292 return(1);
alpha_tango 4:9b1c6b9dae1c 293 }
alpha_tango 4:9b1c6b9dae1c 294 else if(green_value>18000 && blue_value<30000){
alpha_tango 4:9b1c6b9dae1c 295 return(2);
alpha_tango 4:9b1c6b9dae1c 296 }
alpha_tango 4:9b1c6b9dae1c 297 else if(red_value<10000 && blue_value>15000){
alpha_tango 4:9b1c6b9dae1c 298 return(3);
alpha_tango 4:9b1c6b9dae1c 299 }
alpha_tango 4:9b1c6b9dae1c 300 else if(blue_value>30000 && red_value<20000){
alpha_tango 4:9b1c6b9dae1c 301 return(4);
alpha_tango 4:9b1c6b9dae1c 302 }
alpha_tango 4:9b1c6b9dae1c 303 else if(red_value>25000 && green_value<15000){
alpha_tango 4:9b1c6b9dae1c 304 return(5);
alpha_tango 4:9b1c6b9dae1c 305 }
alpha_tango 4:9b1c6b9dae1c 306 else if(red_value>50000){
alpha_tango 4:9b1c6b9dae1c 307 return(6);
alpha_tango 4:9b1c6b9dae1c 308 }
alpha_tango 4:9b1c6b9dae1c 309 else if(red_value<10000 && blue_value<10000){
alpha_tango 4:9b1c6b9dae1c 310 return(7);
alpha_tango 4:9b1c6b9dae1c 311 }
alpha_tango 4:9b1c6b9dae1c 312 else if(red_value==0){
alpha_tango 4:9b1c6b9dae1c 313 return(8);
alpha_tango 4:9b1c6b9dae1c 314 }
alpha_tango 7:1640572360de 315 else{
alpha_tango 7:1640572360de 316 return(9);
alpha_tango 7:1640572360de 317 }
alpha_tango 4:9b1c6b9dae1c 318 // print sensor readings
alpha_tango 4:9b1c6b9dae1c 319
alpha_tango 4:9b1c6b9dae1c 320 //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
alpha_tango 4:9b1c6b9dae1c 321 //wait(0.5);
alpha_tango 4:9b1c6b9dae1c 322 }
alpha_tango 9:a5a6d3a48145 323 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
alpha_tango 9:a5a6d3a48145 324 // 3
alpha_tango 9:a5a6d3a48145 325 //2*--------*--------*4
alpha_tango 9:a5a6d3a48145 326 // | |
alpha_tango 9:a5a6d3a48145 327 // | |
alpha_tango 9:a5a6d3a48145 328 // | |
alpha_tango 9:a5a6d3a48145 329 // | |
alpha_tango 9:a5a6d3a48145 330 //1* *5
alpha_tango 9:a5a6d3a48145 331 // | |
alpha_tango 9:a5a6d3a48145 332 // | |
alpha_tango 9:a5a6d3a48145 333 // | |
alpha_tango 9:a5a6d3a48145 334 // | |
alpha_tango 9:a5a6d3a48145 335 // *--------*--------*6
alpha_tango 9:a5a6d3a48145 336 // 0 7
alpha_tango 9:a5a6d3a48145 337
alpha_tango 8:a6080c27f8c5 338 {
alpha_tango 9:a5a6d3a48145 339 /*******************************************************************///Red
alpha_tango 9:a5a6d3a48145 340 if( i == 0 && color == 1 )
alpha_tango 9:a5a6d3a48145 341 {
alpha_tango 9:a5a6d3a48145 342 //condition for return red at index 0
alpha_tango 9:a5a6d3a48145 343
alpha_tango 9:a5a6d3a48145 344 }
alpha_tango 9:a5a6d3a48145 345 if( i == 1 && color == 1 )
alpha_tango 9:a5a6d3a48145 346 {
alpha_tango 9:a5a6d3a48145 347 //condition for return red at index 1
alpha_tango 9:a5a6d3a48145 348
alpha_tango 9:a5a6d3a48145 349 }
alpha_tango 9:a5a6d3a48145 350 if( i == 2 && color == 1 )
alpha_tango 9:a5a6d3a48145 351 {
alpha_tango 9:a5a6d3a48145 352 //condition for return red at index 2
alpha_tango 9:a5a6d3a48145 353
alpha_tango 9:a5a6d3a48145 354 }
alpha_tango 9:a5a6d3a48145 355 if( i == 4 && color == 1 )
alpha_tango 9:a5a6d3a48145 356 {
alpha_tango 9:a5a6d3a48145 357 //condition for return red at index 4
alpha_tango 9:a5a6d3a48145 358
alpha_tango 9:a5a6d3a48145 359 }
alpha_tango 9:a5a6d3a48145 360 if( i == 5 && color == 1 )
alpha_tango 9:a5a6d3a48145 361 {
alpha_tango 9:a5a6d3a48145 362 //condition for return red at index 5
alpha_tango 9:a5a6d3a48145 363
alpha_tango 9:a5a6d3a48145 364 }
alpha_tango 9:a5a6d3a48145 365 if( i == 6 && color == 1 )
alpha_tango 9:a5a6d3a48145 366 {
alpha_tango 9:a5a6d3a48145 367 //condition for return red at index 5
alpha_tango 9:a5a6d3a48145 368
alpha_tango 9:a5a6d3a48145 369 }
alpha_tango 9:a5a6d3a48145 370
alpha_tango 9:a5a6d3a48145 371 /*******************************************************************///Green
alpha_tango 9:a5a6d3a48145 372 if( i == 0 && color == 2 )
alpha_tango 9:a5a6d3a48145 373 {
alpha_tango 9:a5a6d3a48145 374 //condition for return Green at index 0
alpha_tango 9:a5a6d3a48145 375
alpha_tango 9:a5a6d3a48145 376 }
alpha_tango 9:a5a6d3a48145 377 if( i == 1 && color == 2 )
alpha_tango 9:a5a6d3a48145 378 {
alpha_tango 9:a5a6d3a48145 379 //condition for return green at index 1
alpha_tango 9:a5a6d3a48145 380
alpha_tango 9:a5a6d3a48145 381 }
alpha_tango 9:a5a6d3a48145 382 if( i == 2 && color == 2 )
alpha_tango 9:a5a6d3a48145 383 {
alpha_tango 9:a5a6d3a48145 384 //condition for return green at index 2
alpha_tango 9:a5a6d3a48145 385
alpha_tango 9:a5a6d3a48145 386 }
alpha_tango 8:a6080c27f8c5 387
alpha_tango 9:a5a6d3a48145 388 if( i == 4 && color == 2 )
alpha_tango 9:a5a6d3a48145 389 {
alpha_tango 9:a5a6d3a48145 390 //condition for return green at index 4
alpha_tango 9:a5a6d3a48145 391
alpha_tango 9:a5a6d3a48145 392 }
alpha_tango 9:a5a6d3a48145 393 if( i == 5 && color == 2 )
alpha_tango 9:a5a6d3a48145 394 {
alpha_tango 9:a5a6d3a48145 395 //condition for return green at index 5
alpha_tango 9:a5a6d3a48145 396
alpha_tango 9:a5a6d3a48145 397 }
alpha_tango 9:a5a6d3a48145 398 if( i == 6 && color == 2 )
alpha_tango 9:a5a6d3a48145 399 {
alpha_tango 9:a5a6d3a48145 400 //condition for return green at index 6
alpha_tango 9:a5a6d3a48145 401
alpha_tango 9:a5a6d3a48145 402 }
alpha_tango 9:a5a6d3a48145 403 /*******************************************************************///Blue
alpha_tango 9:a5a6d3a48145 404 if( i == 0 && color == 3 )//condition for return Blue at index 0 ::
alpha_tango 9:a5a6d3a48145 405 {
alpha_tango 9:a5a6d3a48145 406 rot180();
alpha_tango 9:a5a6d3a48145 407 move(0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 408 turnRight();
alpha_tango 9:a5a6d3a48145 409 move(0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 410 break;
alpha_tango 9:a5a6d3a48145 411
alpha_tango 9:a5a6d3a48145 412 }
alpha_tango 9:a5a6d3a48145 413 if( i == 1 && color == 3 ) //condition for return blue at index 1
alpha_tango 9:a5a6d3a48145 414 {
alpha_tango 9:a5a6d3a48145 415 rot180();
alpha_tango 9:a5a6d3a48145 416 move((leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 417 turnRight();
alpha_tango 9:a5a6d3a48145 418 move(0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 419 break;
alpha_tango 9:a5a6d3a48145 420
alpha_tango 9:a5a6d3a48145 421 }
alpha_tango 9:a5a6d3a48145 422 if( i == 2 && color == 3 ) //condition for return blue at index 2
alpha_tango 9:a5a6d3a48145 423 {
alpha_tango 9:a5a6d3a48145 424 rot180();
alpha_tango 9:a5a6d3a48145 425 move((2*leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 426 turnRight();
alpha_tango 9:a5a6d3a48145 427 move(0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 428 break;
alpha_tango 9:a5a6d3a48145 429
alpha_tango 9:a5a6d3a48145 430 }
alpha_tango 9:a5a6d3a48145 431 if( i == 4 && color == 3 ) //condition for return blue at index 4
alpha_tango 9:a5a6d3a48145 432 {
alpha_tango 9:a5a6d3a48145 433 move((2*leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 434 turnRight();
alpha_tango 9:a5a6d3a48145 435 move(2*leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 436 break;
alpha_tango 9:a5a6d3a48145 437
alpha_tango 9:a5a6d3a48145 438 }
alpha_tango 9:a5a6d3a48145 439 if( i == 5 && color == 3 ) //condition for return blue at index 5
alpha_tango 9:a5a6d3a48145 440 {
alpha_tango 9:a5a6d3a48145 441 move(leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 442 turnRight();
alpha_tango 9:a5a6d3a48145 443 move(2*leg + 0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 444 break;
alpha_tango 9:a5a6d3a48145 445
alpha_tango 9:a5a6d3a48145 446 }
alpha_tango 9:a5a6d3a48145 447 if( i == 6 && color == 3 ) //condition for return blue at index 5
alpha_tango 9:a5a6d3a48145 448 {
alpha_tango 9:a5a6d3a48145 449 move(2*leg + 0.3048*scale);
alpha_tango 9:a5a6d3a48145 450 turnLeft();
alpha_tango 9:a5a6d3a48145 451 move(0.3048*scale, FORWARD);
alpha_tango 9:a5a6d3a48145 452 break;
alpha_tango 9:a5a6d3a48145 453
alpha_tango 9:a5a6d3a48145 454 }
alpha_tango 9:a5a6d3a48145 455 else break;
alpha_tango 9:a5a6d3a48145 456
alpha_tango 9:a5a6d3a48145 457 /*******************************************************************///Cyan
alpha_tango 9:a5a6d3a48145 458 if( i == 0 && color == 4 )
alpha_tango 9:a5a6d3a48145 459 {
alpha_tango 9:a5a6d3a48145 460 //condition for return Cyan at index 0
alpha_tango 9:a5a6d3a48145 461
alpha_tango 9:a5a6d3a48145 462 }
alpha_tango 9:a5a6d3a48145 463 if( i == 1 && color == 4 )
alpha_tango 9:a5a6d3a48145 464 {
alpha_tango 9:a5a6d3a48145 465 //condition for return Cyan at index 1
alpha_tango 9:a5a6d3a48145 466
alpha_tango 9:a5a6d3a48145 467 }
alpha_tango 9:a5a6d3a48145 468 if( i == 2 && color == 4 )
alpha_tango 9:a5a6d3a48145 469 {
alpha_tango 9:a5a6d3a48145 470 //condition for return Cyan at index 2
alpha_tango 9:a5a6d3a48145 471
alpha_tango 9:a5a6d3a48145 472 }
alpha_tango 9:a5a6d3a48145 473 if( i == 4 && color == 4 )
alpha_tango 9:a5a6d3a48145 474 {
alpha_tango 9:a5a6d3a48145 475 //condition for return Cyan at index 4
alpha_tango 9:a5a6d3a48145 476
alpha_tango 9:a5a6d3a48145 477 }
alpha_tango 9:a5a6d3a48145 478 if( i == 5 && color == 4 )
alpha_tango 9:a5a6d3a48145 479 {
alpha_tango 9:a5a6d3a48145 480 //condition for return Cyan at index 5
alpha_tango 9:a5a6d3a48145 481
alpha_tango 9:a5a6d3a48145 482 }
alpha_tango 9:a5a6d3a48145 483 if( i == 6 && color == 4 )
alpha_tango 9:a5a6d3a48145 484 {
alpha_tango 9:a5a6d3a48145 485 //condition for return Cyan at index 6
alpha_tango 9:a5a6d3a48145 486
alpha_tango 9:a5a6d3a48145 487 }
alpha_tango 9:a5a6d3a48145 488 else break;
alpha_tango 9:a5a6d3a48145 489 /*******************************************************************///Magenta
alpha_tango 9:a5a6d3a48145 490 if( i == 0 && color == 5 )
alpha_tango 9:a5a6d3a48145 491 {
alpha_tango 9:a5a6d3a48145 492 //condition for return Magenta at index 0
alpha_tango 9:a5a6d3a48145 493
alpha_tango 9:a5a6d3a48145 494 }
alpha_tango 9:a5a6d3a48145 495 if( i == 1 && color == 5 )
alpha_tango 9:a5a6d3a48145 496 {
alpha_tango 9:a5a6d3a48145 497 //condition for return Magenta at index 1
alpha_tango 9:a5a6d3a48145 498
alpha_tango 9:a5a6d3a48145 499 }
alpha_tango 9:a5a6d3a48145 500 if( i == 2 && color == 5 )
alpha_tango 9:a5a6d3a48145 501 {
alpha_tango 9:a5a6d3a48145 502 //condition for return Magenta at index 2
alpha_tango 9:a5a6d3a48145 503
alpha_tango 9:a5a6d3a48145 504 }
alpha_tango 9:a5a6d3a48145 505 if( i == 4 && color == 5 )
alpha_tango 9:a5a6d3a48145 506 {
alpha_tango 9:a5a6d3a48145 507 //condition for return Magenta at index 4
alpha_tango 9:a5a6d3a48145 508
alpha_tango 9:a5a6d3a48145 509 }
alpha_tango 9:a5a6d3a48145 510 if( i == 5 && color == 5 )
alpha_tango 9:a5a6d3a48145 511 {
alpha_tango 9:a5a6d3a48145 512 //condition for return Magenta at index 5
alpha_tango 9:a5a6d3a48145 513
alpha_tango 9:a5a6d3a48145 514 }
alpha_tango 9:a5a6d3a48145 515 if( i == 6 && color == 5 )
alpha_tango 9:a5a6d3a48145 516 {
alpha_tango 9:a5a6d3a48145 517 //condition for return Magenta at index 6
alpha_tango 9:a5a6d3a48145 518
alpha_tango 9:a5a6d3a48145 519 }
alpha_tango 9:a5a6d3a48145 520 else break;
alpha_tango 9:a5a6d3a48145 521 /*******************************************************************///Yellow
alpha_tango 9:a5a6d3a48145 522 if( i == 0 && color == 6 )
alpha_tango 9:a5a6d3a48145 523 {
alpha_tango 9:a5a6d3a48145 524 //condition for return Yellow at index 0
alpha_tango 9:a5a6d3a48145 525
alpha_tango 9:a5a6d3a48145 526 }
alpha_tango 9:a5a6d3a48145 527 if( i == 1 && color == 6 )
alpha_tango 9:a5a6d3a48145 528 {
alpha_tango 9:a5a6d3a48145 529 //condition for return Yellow at index 1
alpha_tango 9:a5a6d3a48145 530
alpha_tango 9:a5a6d3a48145 531 }
alpha_tango 9:a5a6d3a48145 532 if( i == 2 && color == 6 )
alpha_tango 9:a5a6d3a48145 533 {
alpha_tango 9:a5a6d3a48145 534 //condition for return Yellow at index 2
alpha_tango 9:a5a6d3a48145 535
alpha_tango 9:a5a6d3a48145 536 }
alpha_tango 9:a5a6d3a48145 537 if( i == 4 && color == 6 )
alpha_tango 9:a5a6d3a48145 538 {
alpha_tango 9:a5a6d3a48145 539 //condition for return Yellow at index 4
alpha_tango 9:a5a6d3a48145 540
alpha_tango 9:a5a6d3a48145 541 }
alpha_tango 9:a5a6d3a48145 542 if( i == 5 && color == 6 )
alpha_tango 9:a5a6d3a48145 543 {
alpha_tango 9:a5a6d3a48145 544 //condition for return Yellow at index 5
alpha_tango 9:a5a6d3a48145 545
alpha_tango 9:a5a6d3a48145 546 }
alpha_tango 9:a5a6d3a48145 547 if( i == 6 && color == 6 )
alpha_tango 9:a5a6d3a48145 548 {
alpha_tango 9:a5a6d3a48145 549 //condition for return Yellow at index 6
alpha_tango 9:a5a6d3a48145 550
alpha_tango 9:a5a6d3a48145 551 }
alpha_tango 9:a5a6d3a48145 552 else break;
alpha_tango 9:a5a6d3a48145 553 /*******************************************************************///Gray
alpha_tango 9:a5a6d3a48145 554 if( i == 0 && color == 7 )
alpha_tango 9:a5a6d3a48145 555 {
alpha_tango 9:a5a6d3a48145 556 //condition for return Gray at index 0
alpha_tango 9:a5a6d3a48145 557
alpha_tango 9:a5a6d3a48145 558 }
alpha_tango 9:a5a6d3a48145 559 if( i == 1 && color == 7 )
alpha_tango 9:a5a6d3a48145 560 {
alpha_tango 9:a5a6d3a48145 561 //condition for return Gray at index 1
alpha_tango 9:a5a6d3a48145 562
alpha_tango 9:a5a6d3a48145 563 }
alpha_tango 9:a5a6d3a48145 564 if( i == 2 && color == 7 )
alpha_tango 9:a5a6d3a48145 565 {
alpha_tango 9:a5a6d3a48145 566 //condition for return Gray at index 2
alpha_tango 9:a5a6d3a48145 567
alpha_tango 9:a5a6d3a48145 568 }
alpha_tango 9:a5a6d3a48145 569 if( i == 4 && color == 7 )
alpha_tango 9:a5a6d3a48145 570 {
alpha_tango 9:a5a6d3a48145 571 //condition for return Gray at index 4
alpha_tango 9:a5a6d3a48145 572
alpha_tango 9:a5a6d3a48145 573 }
alpha_tango 9:a5a6d3a48145 574 if( i == 5 && color == 7 )
alpha_tango 9:a5a6d3a48145 575 {
alpha_tango 9:a5a6d3a48145 576 //condition for return Gray at index 5
alpha_tango 9:a5a6d3a48145 577
alpha_tango 9:a5a6d3a48145 578 }
alpha_tango 9:a5a6d3a48145 579 if( i == 6 && color == 7 )
alpha_tango 9:a5a6d3a48145 580 {
alpha_tango 9:a5a6d3a48145 581 //condition for return Gray at index 6
alpha_tango 9:a5a6d3a48145 582
alpha_tango 9:a5a6d3a48145 583 }
alpha_tango 9:a5a6d3a48145 584 else break;
alpha_tango 9:a5a6d3a48145 585 /*******************************************************************/
alpha_tango 8:a6080c27f8c5 586
alpha_tango 8:a6080c27f8c5 587 }
alpha_tango 4:9b1c6b9dae1c 588 }