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@8:a6080c27f8c5, 2018-03-31 (annotated)

Committer:
alpha_tango
Date:
Sat Mar 31 21:43:07 2018 +0000
Revision:
8:a6080c27f8c5
Parent:
7:1640572360de
Child:
9:a5a6d3a48145
Made the robot go to the first token on the course after initial movement. Created if statement to allow for a right turn at each corner of the field.

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 8:a6080c27f8c5 34 void findPath(int color, int i); //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 8:a6080c27f8c5 131 for(int i = 0; i <=8; i++) //begin motion around Nth perimeter
alpha_tango 7:1640572360de 132 {
alpha_tango 7:1640572360de 133 leg = 0.762; //2.5 feet in meters
alpha_tango 8:a6080c27f8c5 134 if(i % 2 == 0)
alpha_tango 8:a6080c27f8c5 135 {
alpha_tango 8:a6080c27f8c5 136 turnRight();
alpha_tango 8:a6080c27f8c5 137 }
alpha_tango 7:1640572360de 138 grabToken();
alpha_tango 7:1640572360de 139 color = findColor();
alpha_tango 7:1640572360de 140 if (color = 9)
alpha_tango 7:1640572360de 141 {
alpha_tango 7:1640572360de 142 break;
alpha_tango 7:1640572360de 143 }
alpha_tango 7:1640572360de 144 else
alpha_tango 7:1640572360de 145 {
alpha_tango 8:a6080c27f8c5 146 findPath(color, i);
alpha_tango 8:a6080c27f8c5 147 dropToken();
alpha_tango 8:a6080c27f8c5 148 //returnPrevious();
alpha_tango 7:1640572360de 149 }
alpha_tango 7:1640572360de 150 move(leg, FORWARD);
alpha_tango 7:1640572360de 151
alpha_tango 7:1640572360de 152 }
alpha_tango 8:a6080c27f8c5 153 /* grabToken();
alpha_tango 0:7e54f0e2be67 154 move(1,FORWARD);
alpha_tango 5:17a8d8395a50 155 turnLeft();
alpha_tango 4:9b1c6b9dae1c 156 wait(0.5);
alpha_tango 5:17a8d8395a50 157 turnLeft();
alpha_tango 0:7e54f0e2be67 158 wait(2);
alpha_tango 0:7e54f0e2be67 159 dropToken();
alpha_tango 8:a6080c27f8c5 160 wait(2);*/
alpha_tango 0:7e54f0e2be67 161 }
alpha_tango 0:7e54f0e2be67 162 }
alpha_tango 4:9b1c6b9dae1c 163
alpha_tango 0:7e54f0e2be67 164 //Distance is in meters
alpha_tango 4:9b1c6b9dae1c 165 void move(float dist, bool direction)
alpha_tango 0:7e54f0e2be67 166 {
alpha_tango 0:7e54f0e2be67 167 FLdirection = direction;
alpha_tango 0:7e54f0e2be67 168 FRdirection = !direction;
alpha_tango 0:7e54f0e2be67 169
alpha_tango 4:9b1c6b9dae1c 170 stepFL.period(1.0/FREQUENCY);
alpha_tango 0:7e54f0e2be67 171 stepFR.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 172 stepFL.write(0.5f);
alpha_tango 4:9b1c6b9dae1c 173 stepFR.write(0.5f);
alpha_tango 8:a6080c27f8c5 174
alpha_tango 8:a6080c27f8c5 175 wait(4*(dist/stepSize)*(1/FREQUENCY)); //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 176 stepFL.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 177 stepFR.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 178 stepFL.write(0.0f);
alpha_tango 4:9b1c6b9dae1c 179 stepFR.write(0.0f);
alpha_tango 0:7e54f0e2be67 180
alpha_tango 0:7e54f0e2be67 181 }
alpha_tango 4:9b1c6b9dae1c 182
alpha_tango 0:7e54f0e2be67 183 void grabToken()
alpha_tango 0:7e54f0e2be67 184 {
alpha_tango 4:9b1c6b9dae1c 185 highL = 0;
alpha_tango 4:9b1c6b9dae1c 186 highH = 1;
alpha_tango 0:7e54f0e2be67 187 enableH = 1;
alpha_tango 0:7e54f0e2be67 188 wait(1);
alpha_tango 0:7e54f0e2be67 189 magDirection = 1;
alpha_tango 0:7e54f0e2be67 190 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 191 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 192 wait(0.65);
alpha_tango 0:7e54f0e2be67 193 magArm.period(0);
alpha_tango 0:7e54f0e2be67 194 magArm.write(0);
alpha_tango 0:7e54f0e2be67 195
alpha_tango 0:7e54f0e2be67 196 }
alpha_tango 4:9b1c6b9dae1c 197
alpha_tango 0:7e54f0e2be67 198 void dropToken()
alpha_tango 0:7e54f0e2be67 199 {
alpha_tango 0:7e54f0e2be67 200 magDirection = 0;
alpha_tango 0:7e54f0e2be67 201 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 202 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 203 wait(0.65);
alpha_tango 0:7e54f0e2be67 204 magArm.period(0);
alpha_tango 4:9b1c6b9dae1c 205 magArm.write(0);
alpha_tango 4:9b1c6b9dae1c 206 highL = 1;
alpha_tango 4:9b1c6b9dae1c 207 highH = 0;
alpha_tango 4:9b1c6b9dae1c 208 wait(2);
alpha_tango 4:9b1c6b9dae1c 209 enableH = 0;
alpha_tango 0:7e54f0e2be67 210 }
alpha_tango 4:9b1c6b9dae1c 211
alpha_tango 4:9b1c6b9dae1c 212 void turnRight(float dist, bool direction)
alpha_tango 2:5f29bc7daa49 213 {
alpha_tango 2:5f29bc7daa49 214 //Get rid of all FR occurences which will turn right motor off
alpha_tango 5:17a8d8395a50 215 FLdirection = 0; //to turn right we want this going FORWARD so a 0;
alpha_tango 2:5f29bc7daa49 216
alpha_tango 2:5f29bc7daa49 217
alpha_tango 2:5f29bc7daa49 218 stepFL.period(1/FREQUENCY);
alpha_tango 2:5f29bc7daa49 219 stepFL.write(0.5);
alpha_tango 2:5f29bc7daa49 220
alpha_tango 2:5f29bc7daa49 221 //dist/stepSize is the number of steps
alpha_tango 2:5f29bc7daa49 222 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 223 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 2:5f29bc7daa49 224 stepFL.period(0);
alpha_tango 2:5f29bc7daa49 225 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 226 }
alpha_tango 4:9b1c6b9dae1c 227 void turnLeft(float dist, bool direction)
alpha_tango 3:d3264a6f7a62 228 {
alpha_tango 3:d3264a6f7a62 229 //Get rid of all FL occurences which will turn left motor off
alpha_tango 5:17a8d8395a50 230 FRdirection = 1; //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
alpha_tango 3:d3264a6f7a62 231
alpha_tango 3:d3264a6f7a62 232
alpha_tango 3:d3264a6f7a62 233 stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
alpha_tango 3:d3264a6f7a62 234 stepFR.write(0.5);
alpha_tango 3:d3264a6f7a62 235
alpha_tango 3:d3264a6f7a62 236 //dist/stepSize is the number of steps
alpha_tango 3:d3264a6f7a62 237 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 238 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 3:d3264a6f7a62 239 stepFR.period(0);
alpha_tango 3:d3264a6f7a62 240 stepFR.write(0);
alpha_tango 3:d3264a6f7a62 241 }
alpha_tango 0:7e54f0e2be67 242 void rot180()
alpha_tango 0:7e54f0e2be67 243 {
alpha_tango 4:9b1c6b9dae1c 244 //Get rid of all FR occurences which will turn right motor off
alpha_tango 4:9b1c6b9dae1c 245 FLdirection = direction; //to turn right we want this going FORWARD so a 0;
alpha_tango 4:9b1c6b9dae1c 246
alpha_tango 4:9b1c6b9dae1c 247
alpha_tango 4:9b1c6b9dae1c 248 stepFL.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 249 stepFL.write(0.5);
alpha_tango 4:9b1c6b9dae1c 250
alpha_tango 4:9b1c6b9dae1c 251 //dist/stepSize is the number of steps
alpha_tango 4:9b1c6b9dae1c 252 //1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 253 wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 254 stepFL.period(0);
alpha_tango 4:9b1c6b9dae1c 255 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 256 }
alpha_tango 0:7e54f0e2be67 257 void kill()
alpha_tango 0:7e54f0e2be67 258 {
alpha_tango 0:7e54f0e2be67 259 exit(0);
alpha_tango 0:7e54f0e2be67 260 }
alpha_tango 4:9b1c6b9dae1c 261
alpha_tango 4:9b1c6b9dae1c 262 int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 4:9b1c6b9dae1c 263
alpha_tango 4:9b1c6b9dae1c 264 while (true) {
alpha_tango 4:9b1c6b9dae1c 265 wait(1);
alpha_tango 4:9b1c6b9dae1c 266 char clear_reg[1] = {148};
alpha_tango 4:9b1c6b9dae1c 267 char clear_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 268 i2c.write(sensor_addr,clear_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 269 i2c.read(sensor_addr,clear_data,2, false);
alpha_tango 4:9b1c6b9dae1c 270
alpha_tango 4:9b1c6b9dae1c 271 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
alpha_tango 4:9b1c6b9dae1c 272
alpha_tango 4:9b1c6b9dae1c 273 char red_reg[1] = {150};
alpha_tango 4:9b1c6b9dae1c 274 char red_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 275 i2c.write(sensor_addr,red_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 276 i2c.read(sensor_addr,red_data,2, false);
alpha_tango 4:9b1c6b9dae1c 277
alpha_tango 4:9b1c6b9dae1c 278 int red_value = ((int)red_data[1] << 8) | red_data[0];
alpha_tango 4:9b1c6b9dae1c 279
alpha_tango 4:9b1c6b9dae1c 280 char green_reg[1] = {152};
alpha_tango 4:9b1c6b9dae1c 281 char green_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 282 i2c.write(sensor_addr,green_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 283 i2c.read(sensor_addr,green_data,2, false);
alpha_tango 4:9b1c6b9dae1c 284
alpha_tango 4:9b1c6b9dae1c 285 int green_value = ((int)green_data[1] << 8) | green_data[0];
alpha_tango 4:9b1c6b9dae1c 286
alpha_tango 4:9b1c6b9dae1c 287 char blue_reg[1] = {154};
alpha_tango 4:9b1c6b9dae1c 288 char blue_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 289 i2c.write(sensor_addr,blue_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 290 i2c.read(sensor_addr,blue_data,2, false);
alpha_tango 4:9b1c6b9dae1c 291
alpha_tango 4:9b1c6b9dae1c 292 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
alpha_tango 4:9b1c6b9dae1c 293
alpha_tango 4:9b1c6b9dae1c 294
alpha_tango 4:9b1c6b9dae1c 295
alpha_tango 7:1640572360de 296 //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing
alpha_tango 4:9b1c6b9dae1c 297 if(blue_value<10000 && red_value>10000){
alpha_tango 4:9b1c6b9dae1c 298 return(1);
alpha_tango 4:9b1c6b9dae1c 299 }
alpha_tango 4:9b1c6b9dae1c 300 else if(green_value>18000 && blue_value<30000){
alpha_tango 4:9b1c6b9dae1c 301 return(2);
alpha_tango 4:9b1c6b9dae1c 302 }
alpha_tango 4:9b1c6b9dae1c 303 else if(red_value<10000 && blue_value>15000){
alpha_tango 4:9b1c6b9dae1c 304 return(3);
alpha_tango 4:9b1c6b9dae1c 305 }
alpha_tango 4:9b1c6b9dae1c 306 else if(blue_value>30000 && red_value<20000){
alpha_tango 4:9b1c6b9dae1c 307 return(4);
alpha_tango 4:9b1c6b9dae1c 308 }
alpha_tango 4:9b1c6b9dae1c 309 else if(red_value>25000 && green_value<15000){
alpha_tango 4:9b1c6b9dae1c 310 return(5);
alpha_tango 4:9b1c6b9dae1c 311 }
alpha_tango 4:9b1c6b9dae1c 312 else if(red_value>50000){
alpha_tango 4:9b1c6b9dae1c 313 return(6);
alpha_tango 4:9b1c6b9dae1c 314 }
alpha_tango 4:9b1c6b9dae1c 315 else if(red_value<10000 && blue_value<10000){
alpha_tango 4:9b1c6b9dae1c 316 return(7);
alpha_tango 4:9b1c6b9dae1c 317 }
alpha_tango 4:9b1c6b9dae1c 318 else if(red_value==0){
alpha_tango 4:9b1c6b9dae1c 319 return(8);
alpha_tango 4:9b1c6b9dae1c 320 }
alpha_tango 7:1640572360de 321 else{
alpha_tango 7:1640572360de 322 return(9);
alpha_tango 7:1640572360de 323 }
alpha_tango 4:9b1c6b9dae1c 324 // print sensor readings
alpha_tango 4:9b1c6b9dae1c 325
alpha_tango 4:9b1c6b9dae1c 326 //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
alpha_tango 4:9b1c6b9dae1c 327 //wait(0.5);
alpha_tango 4:9b1c6b9dae1c 328 }
alpha_tango 8:a6080c27f8c5 329 void findPath(int color, int i)
alpha_tango 8:a6080c27f8c5 330 {
alpha_tango 8:a6080c27f8c5 331
alpha_tango 8:a6080c27f8c5 332
alpha_tango 8:a6080c27f8c5 333 }
alpha_tango 4:9b1c6b9dae1c 334 }