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

Committer:
alpha_tango
Date:
Sat Mar 31 23:57:23 2018 +0000
Revision:
10:ed6ecbb420b8
Parent:
9:a5a6d3a48145
Child:
11:93c035708249
returnToPrevious

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 4:9b1c6b9dae1c 22
alpha_tango 4:9b1c6b9dae1c 23 // PROTOTYPE FUNCTION DECLARATIONS
alpha_tango 4:9b1c6b9dae1c 24 void move(float dist, bool direction);
alpha_tango 4:9b1c6b9dae1c 25 void grabToken();//Picks up the token for reading
alpha_tango 4:9b1c6b9dae1c 26 void dropToken();//Drops the token off
alpha_tango 4:9b1c6b9dae1c 27 void kill();
alpha_tango 10:ed6ecbb420b8 28 void turnRight();
alpha_tango 10:ed6ecbb420b8 29 void turnRight45();
alpha_tango 10:ed6ecbb420b8 30 void turnLeft();
alpha_tango 4:9b1c6b9dae1c 31 void rot180(); //Turns the robot around
alpha_tango 5:17a8d8395a50 32 int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 10:ed6ecbb420b8 33 void returnToPrevious(int, int);
alpha_tango 10:ed6ecbb420b8 34 void positionIsBlue(); // uses these 6 function to return back to previous position
alpha_tango 10:ed6ecbb420b8 35 void positionIsRed();
alpha_tango 10:ed6ecbb420b8 36 void positionIsGreen();
alpha_tango 10:ed6ecbb420b8 37 void positionIsCyan();
alpha_tango 10:ed6ecbb420b8 38 void positionIsMagenta();
alpha_tango 10:ed6ecbb420b8 39 void positionIsYellow();
alpha_tango 10:ed6ecbb420b8 40
alpha_tango 4:9b1c6b9dae1c 41
alpha_tango 4:9b1c6b9dae1c 42 // GLOBAL VARIABLES
alpha_tango 4:9b1c6b9dae1c 43 const int FORWARD = 0;
alpha_tango 4:9b1c6b9dae1c 44 const int BACKWARD = 1;
alpha_tango 4:9b1c6b9dae1c 45 const float stepSize = 0.001212; //in meters
alpha_tango 4:9b1c6b9dae1c 46 const float FREQUENCY = 500; //steps per second
alpha_tango 4:9b1c6b9dae1c 47 int sensor_addr = 41 << 1;
alpha_tango 7:1640572360de 48 float leg;
alpha_tango 10:ed6ecbb420b8 49 int color;
alpha_tango 4:9b1c6b9dae1c 50
alpha_tango 4:9b1c6b9dae1c 51
alpha_tango 4:9b1c6b9dae1c 52 // NOTES
alpha_tango 0:7e54f0e2be67 53 /*
alpha_tango 0:7e54f0e2be67 54 -36 prewriten functions for the drop off decision
alpha_tango 0:7e54f0e2be67 55 -Possibly use another RGB sensor as a line follower
alpha_tango 0:7e54f0e2be67 56 -Decides function based on color
alpha_tango 0:7e54f0e2be67 57 -findPath
alpha_tango 0:7e54f0e2be67 58 -starting at bottomLeft
alpha_tango 0:7e54f0e2be67 59 -travel up one leg
alpha_tango 0:7e54f0e2be67 60 -turn right if rgb
alpha_tango 0:7e54f0e2be67 61 -turn left if cmy
alpha_tango 0:7e54f0e2be67 62 -turn right, left, or stay based on color choice
alpha_tango 4:9b1c6b9dae1c 63
alpha_tango 0:7e54f0e2be67 64 void findPath(); //Figures out the path to take to take the disk to its drop off position
alpha_tango 0:7e54f0e2be67 65 void returnHome(); //Returns to the home white square
alpha_tango 0:7e54f0e2be67 66 void returnPrevPos(); //Does the opposite of findPath() to return to the previous position
alpha_tango 0:7e54f0e2be67 67
alpha_tango 0:7e54f0e2be67 68 //Variables
alpha_tango 4:9b1c6b9dae1c 69
alpha_tango 0:7e54f0e2be67 70 -boxSizes, 2x2, 3x3, 4x4, etc
alpha_tango 0:7e54f0e2be67 71 -legSize, 1 foot, 1.5 feet, 2 feet, etc.
alpha_tango 0:7e54f0e2be67 72 -direction choices for findPath
alpha_tango 0:7e54f0e2be67 73 */
alpha_tango 0:7e54f0e2be67 74
alpha_tango 0:7e54f0e2be67 75
alpha_tango 0:7e54f0e2be67 76 int main()
alpha_tango 0:7e54f0e2be67 77 {
alpha_tango 0:7e54f0e2be67 78 //Start a timer
alpha_tango 10:ed6ecbb420b8 79 //Will need to be a variable timer based on round number
alpha_tango 0:7e54f0e2be67 80
alpha_tango 5:17a8d8395a50 81 float radDistance = 0.5;
alpha_tango 5:17a8d8395a50 82 float posDistance = 0.5;
alpha_tango 5:17a8d8395a50 83 float armDistance = 0.5;
alpha_tango 5:17a8d8395a50 84
alpha_tango 0:7e54f0e2be67 85
alpha_tango 0:7e54f0e2be67 86 enableH = 0; //Making sure the H-Bridge starts low and off
alpha_tango 4:9b1c6b9dae1c 87 highH = 0; //This starts high for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 88 highL = 1; //This starts low for the H-Bridge
alpha_tango 4:9b1c6b9dae1c 89 while(true) //The start button
alpha_tango 1:4ab886b72870 90 {
alpha_tango 1:4ab886b72870 91 if (Start == 0)
alpha_tango 1:4ab886b72870 92 break;
alpha_tango 1:4ab886b72870 93 }
alpha_tango 4:9b1c6b9dae1c 94
alpha_tango 4:9b1c6b9dae1c 95 killAll.rise(&kill); //The kill interupt
alpha_tango 5:17a8d8395a50 96
alpha_tango 4:9b1c6b9dae1c 97
alpha_tango 5:17a8d8395a50 98 // RGB Sensor Settings
alpha_tango 4:9b1c6b9dae1c 99 pc.baud(115200);
alpha_tango 5:17a8d8395a50 100 green = 1; // off
alpha_tango 7:1640572360de 101 i2c.frequency(200000);
alpha_tango 7:1640572360de 102 char id_regval[1] = {146};
alpha_tango 7:1640572360de 103 char data[1] = {0};
alpha_tango 7:1640572360de 104 i2c.write(sensor_addr,id_regval,1, true);
alpha_tango 7:1640572360de 105 i2c.read(sensor_addr,data,1,false);
alpha_tango 7:1640572360de 106 if (data[0]==68)
alpha_tango 7:1640572360de 107 {
alpha_tango 4:9b1c6b9dae1c 108 green = 0;
alpha_tango 4:9b1c6b9dae1c 109 wait (2);
alpha_tango 4:9b1c6b9dae1c 110 green = 1;
alpha_tango 4:9b1c6b9dae1c 111 } else {
alpha_tango 4:9b1c6b9dae1c 112 green = 1;
alpha_tango 7:1640572360de 113 }
alpha_tango 4:9b1c6b9dae1c 114
alpha_tango 4:9b1c6b9dae1c 115 // Initialize color sensor
alpha_tango 4:9b1c6b9dae1c 116 char timing_register[2] = {129,0};
alpha_tango 4:9b1c6b9dae1c 117 i2c.write(sensor_addr,timing_register,2,false);
alpha_tango 4:9b1c6b9dae1c 118
alpha_tango 4:9b1c6b9dae1c 119 char control_register[2] = {143,0};
alpha_tango 4:9b1c6b9dae1c 120 i2c.write(sensor_addr,control_register,2,false);
alpha_tango 4:9b1c6b9dae1c 121
alpha_tango 4:9b1c6b9dae1c 122 char enable_register[2] = {128,3};
alpha_tango 4:9b1c6b9dae1c 123 i2c.write(sensor_addr,enable_register,2,false);
alpha_tango 4:9b1c6b9dae1c 124
alpha_tango 5:17a8d8395a50 125 // Initialize the robot position
alpha_tango 5:17a8d8395a50 126 move((0.6096-radDistance+posDistance+armDistance),FORWARD);
alpha_tango 5:17a8d8395a50 127 turnLeft();
alpha_tango 5:17a8d8395a50 128 move(radDistance,BACKWARD);
alpha_tango 0:7e54f0e2be67 129
alpha_tango 0:7e54f0e2be67 130 while(true)
alpha_tango 4:9b1c6b9dae1c 131 {
alpha_tango 10:ed6ecbb420b8 132 for(int i = 0; i <=8; i++)
alpha_tango 7:1640572360de 133 {
alpha_tango 7:1640572360de 134 leg = 0.762; //2.5 feet in meters
alpha_tango 7:1640572360de 135 grabToken();
alpha_tango 7:1640572360de 136 color = findColor();
alpha_tango 7:1640572360de 137 if (color = 9)
alpha_tango 7:1640572360de 138 {
alpha_tango 7:1640572360de 139 break;
alpha_tango 7:1640572360de 140 }
alpha_tango 7:1640572360de 141 else
alpha_tango 7:1640572360de 142 {
alpha_tango 10:ed6ecbb420b8 143 findPath(color)
alpha_tango 7:1640572360de 144 }
alpha_tango 7:1640572360de 145 move(leg, FORWARD);
alpha_tango 7:1640572360de 146
alpha_tango 7:1640572360de 147 }
alpha_tango 10:ed6ecbb420b8 148 grabToken();
alpha_tango 10:ed6ecbb420b8 149 move(1,FORWARD);
alpha_tango 10:ed6ecbb420b8 150 turnLeft();
alpha_tango 10:ed6ecbb420b8 151 wait(0.5);
alpha_tango 10:ed6ecbb420b8 152 turnLeft();
alpha_tango 10:ed6ecbb420b8 153 wait(2);
alpha_tango 10:ed6ecbb420b8 154 dropToken();
alpha_tango 10:ed6ecbb420b8 155 wait(2);
alpha_tango 0:7e54f0e2be67 156 }
alpha_tango 0:7e54f0e2be67 157 }
alpha_tango 4:9b1c6b9dae1c 158
alpha_tango 0:7e54f0e2be67 159 //Distance is in meters
alpha_tango 4:9b1c6b9dae1c 160 void move(float dist, bool direction)
alpha_tango 0:7e54f0e2be67 161 {
alpha_tango 0:7e54f0e2be67 162 FLdirection = direction;
alpha_tango 0:7e54f0e2be67 163 FRdirection = !direction;
alpha_tango 0:7e54f0e2be67 164
alpha_tango 4:9b1c6b9dae1c 165 stepFL.period(1.0/FREQUENCY);
alpha_tango 0:7e54f0e2be67 166 stepFR.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 167 stepFL.write(0.5f);
alpha_tango 4:9b1c6b9dae1c 168 stepFR.write(0.5f);
alpha_tango 10:ed6ecbb420b8 169 //dist/stepSize is the number of steps
alpha_tango 10:ed6ecbb420b8 170 //1/FREQUENCY is the time per step
alpha_tango 10:ed6ecbb420b8 171 wait(4*(dist/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 172 stepFL.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 173 stepFR.period(0.0f);
alpha_tango 4:9b1c6b9dae1c 174 stepFL.write(0.0f);
alpha_tango 4:9b1c6b9dae1c 175 stepFR.write(0.0f);
alpha_tango 0:7e54f0e2be67 176
alpha_tango 0:7e54f0e2be67 177 }
alpha_tango 4:9b1c6b9dae1c 178
alpha_tango 0:7e54f0e2be67 179 void grabToken()
alpha_tango 0:7e54f0e2be67 180 {
alpha_tango 4:9b1c6b9dae1c 181 highL = 0;
alpha_tango 4:9b1c6b9dae1c 182 highH = 1;
alpha_tango 0:7e54f0e2be67 183 enableH = 1;
alpha_tango 0:7e54f0e2be67 184 wait(1);
alpha_tango 0:7e54f0e2be67 185 magDirection = 1;
alpha_tango 0:7e54f0e2be67 186 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 187 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 188 wait(0.65);
alpha_tango 0:7e54f0e2be67 189 magArm.period(0);
alpha_tango 0:7e54f0e2be67 190 magArm.write(0);
alpha_tango 0:7e54f0e2be67 191
alpha_tango 0:7e54f0e2be67 192 }
alpha_tango 4:9b1c6b9dae1c 193
alpha_tango 0:7e54f0e2be67 194 void dropToken()
alpha_tango 0:7e54f0e2be67 195 {
alpha_tango 0:7e54f0e2be67 196 magDirection = 0;
alpha_tango 0:7e54f0e2be67 197 magArm.period(0.002);
alpha_tango 0:7e54f0e2be67 198 magArm.write(0.5);
alpha_tango 0:7e54f0e2be67 199 wait(0.65);
alpha_tango 0:7e54f0e2be67 200 magArm.period(0);
alpha_tango 4:9b1c6b9dae1c 201 magArm.write(0);
alpha_tango 4:9b1c6b9dae1c 202 highL = 1;
alpha_tango 4:9b1c6b9dae1c 203 highH = 0;
alpha_tango 4:9b1c6b9dae1c 204 wait(2);
alpha_tango 4:9b1c6b9dae1c 205 enableH = 0;
alpha_tango 0:7e54f0e2be67 206 }
alpha_tango 4:9b1c6b9dae1c 207
alpha_tango 10:ed6ecbb420b8 208 void turnRight()
alpha_tango 2:5f29bc7daa49 209 {
alpha_tango 2:5f29bc7daa49 210 //Get rid of all FR occurences which will turn right motor off
alpha_tango 5:17a8d8395a50 211 FLdirection = 0; //to turn right we want this going FORWARD so a 0;
alpha_tango 2:5f29bc7daa49 212
alpha_tango 2:5f29bc7daa49 213
alpha_tango 2:5f29bc7daa49 214 stepFL.period(1/FREQUENCY);
alpha_tango 2:5f29bc7daa49 215 stepFL.write(0.5);
alpha_tango 2:5f29bc7daa49 216
alpha_tango 2:5f29bc7daa49 217 //dist/stepSize is the number of steps
alpha_tango 2:5f29bc7daa49 218 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 219 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 2:5f29bc7daa49 220 stepFL.period(0);
alpha_tango 2:5f29bc7daa49 221 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 222 }
alpha_tango 10:ed6ecbb420b8 223 void turnRight45()
alpha_tango 10:ed6ecbb420b8 224 {
alpha_tango 10:ed6ecbb420b8 225 //Get rid of all FR occurences which will turn right motor off
alpha_tango 10:ed6ecbb420b8 226 FLdirection = 0; //to turn right we want this going FORWARD so a 0;
alpha_tango 10:ed6ecbb420b8 227
alpha_tango 10:ed6ecbb420b8 228
alpha_tango 10:ed6ecbb420b8 229 stepFL.period(1/FREQUENCY);
alpha_tango 10:ed6ecbb420b8 230 stepFL.write(0.5);
alpha_tango 10:ed6ecbb420b8 231
alpha_tango 10:ed6ecbb420b8 232 //dist/stepSize is the number of steps
alpha_tango 10:ed6ecbb420b8 233 //1/FREQUENCY is the time per step
alpha_tango 10:ed6ecbb420b8 234 wait(2*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 10:ed6ecbb420b8 235 stepFL.period(0);
alpha_tango 10:ed6ecbb420b8 236 stepFL.write(0);
alpha_tango 10:ed6ecbb420b8 237 }
alpha_tango 10:ed6ecbb420b8 238 void turnLeft()
alpha_tango 3:d3264a6f7a62 239 {
alpha_tango 3:d3264a6f7a62 240 //Get rid of all FL occurences which will turn left motor off
alpha_tango 5:17a8d8395a50 241 FRdirection = 1; //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
alpha_tango 3:d3264a6f7a62 242
alpha_tango 3:d3264a6f7a62 243
alpha_tango 3:d3264a6f7a62 244 stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
alpha_tango 3:d3264a6f7a62 245 stepFR.write(0.5);
alpha_tango 3:d3264a6f7a62 246
alpha_tango 3:d3264a6f7a62 247 //dist/stepSize is the number of steps
alpha_tango 3:d3264a6f7a62 248 //1/FREQUENCY is the time per step
alpha_tango 5:17a8d8395a50 249 wait(4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 3:d3264a6f7a62 250 stepFR.period(0);
alpha_tango 3:d3264a6f7a62 251 stepFR.write(0);
alpha_tango 3:d3264a6f7a62 252 }
alpha_tango 10:ed6ecbb420b8 253
alpha_tango 0:7e54f0e2be67 254 void rot180()
alpha_tango 0:7e54f0e2be67 255 {
alpha_tango 4:9b1c6b9dae1c 256 //Get rid of all FR occurences which will turn right motor off
alpha_tango 4:9b1c6b9dae1c 257 FLdirection = direction; //to turn right we want this going FORWARD so a 0;
alpha_tango 4:9b1c6b9dae1c 258
alpha_tango 4:9b1c6b9dae1c 259
alpha_tango 4:9b1c6b9dae1c 260 stepFL.period(1/FREQUENCY);
alpha_tango 4:9b1c6b9dae1c 261 stepFL.write(0.5);
alpha_tango 4:9b1c6b9dae1c 262
alpha_tango 4:9b1c6b9dae1c 263 //dist/stepSize is the number of steps
alpha_tango 4:9b1c6b9dae1c 264 //1/FREQUENCY is the time per step
alpha_tango 4:9b1c6b9dae1c 265 wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
alpha_tango 4:9b1c6b9dae1c 266 stepFL.period(0);
alpha_tango 4:9b1c6b9dae1c 267 stepFL.write(0);
alpha_tango 0:7e54f0e2be67 268 }
alpha_tango 0:7e54f0e2be67 269 void kill()
alpha_tango 0:7e54f0e2be67 270 {
alpha_tango 0:7e54f0e2be67 271 exit(0);
alpha_tango 0:7e54f0e2be67 272 }
alpha_tango 4:9b1c6b9dae1c 273
alpha_tango 4:9b1c6b9dae1c 274 int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
alpha_tango 4:9b1c6b9dae1c 275
alpha_tango 4:9b1c6b9dae1c 276 while (true) {
alpha_tango 4:9b1c6b9dae1c 277 wait(1);
alpha_tango 4:9b1c6b9dae1c 278 char clear_reg[1] = {148};
alpha_tango 4:9b1c6b9dae1c 279 char clear_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 280 i2c.write(sensor_addr,clear_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 281 i2c.read(sensor_addr,clear_data,2, false);
alpha_tango 4:9b1c6b9dae1c 282
alpha_tango 4:9b1c6b9dae1c 283 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
alpha_tango 4:9b1c6b9dae1c 284
alpha_tango 4:9b1c6b9dae1c 285 char red_reg[1] = {150};
alpha_tango 4:9b1c6b9dae1c 286 char red_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 287 i2c.write(sensor_addr,red_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 288 i2c.read(sensor_addr,red_data,2, false);
alpha_tango 4:9b1c6b9dae1c 289
alpha_tango 4:9b1c6b9dae1c 290 int red_value = ((int)red_data[1] << 8) | red_data[0];
alpha_tango 4:9b1c6b9dae1c 291
alpha_tango 4:9b1c6b9dae1c 292 char green_reg[1] = {152};
alpha_tango 4:9b1c6b9dae1c 293 char green_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 294 i2c.write(sensor_addr,green_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 295 i2c.read(sensor_addr,green_data,2, false);
alpha_tango 4:9b1c6b9dae1c 296
alpha_tango 4:9b1c6b9dae1c 297 int green_value = ((int)green_data[1] << 8) | green_data[0];
alpha_tango 4:9b1c6b9dae1c 298
alpha_tango 4:9b1c6b9dae1c 299 char blue_reg[1] = {154};
alpha_tango 4:9b1c6b9dae1c 300 char blue_data[2] = {0,0};
alpha_tango 4:9b1c6b9dae1c 301 i2c.write(sensor_addr,blue_reg,1, true);
alpha_tango 4:9b1c6b9dae1c 302 i2c.read(sensor_addr,blue_data,2, false);
alpha_tango 4:9b1c6b9dae1c 303
alpha_tango 4:9b1c6b9dae1c 304 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
alpha_tango 4:9b1c6b9dae1c 305
alpha_tango 4:9b1c6b9dae1c 306
alpha_tango 4:9b1c6b9dae1c 307
alpha_tango 7:1640572360de 308 //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing
alpha_tango 4:9b1c6b9dae1c 309 if(blue_value<10000 && red_value>10000){
alpha_tango 4:9b1c6b9dae1c 310 return(1);
alpha_tango 4:9b1c6b9dae1c 311 }
alpha_tango 4:9b1c6b9dae1c 312 else if(green_value>18000 && blue_value<30000){
alpha_tango 4:9b1c6b9dae1c 313 return(2);
alpha_tango 4:9b1c6b9dae1c 314 }
alpha_tango 4:9b1c6b9dae1c 315 else if(red_value<10000 && blue_value>15000){
alpha_tango 4:9b1c6b9dae1c 316 return(3);
alpha_tango 4:9b1c6b9dae1c 317 }
alpha_tango 4:9b1c6b9dae1c 318 else if(blue_value>30000 && red_value<20000){
alpha_tango 4:9b1c6b9dae1c 319 return(4);
alpha_tango 4:9b1c6b9dae1c 320 }
alpha_tango 4:9b1c6b9dae1c 321 else if(red_value>25000 && green_value<15000){
alpha_tango 4:9b1c6b9dae1c 322 return(5);
alpha_tango 4:9b1c6b9dae1c 323 }
alpha_tango 4:9b1c6b9dae1c 324 else if(red_value>50000){
alpha_tango 4:9b1c6b9dae1c 325 return(6);
alpha_tango 4:9b1c6b9dae1c 326 }
alpha_tango 4:9b1c6b9dae1c 327 else if(red_value<10000 && blue_value<10000){
alpha_tango 4:9b1c6b9dae1c 328 return(7);
alpha_tango 4:9b1c6b9dae1c 329 }
alpha_tango 4:9b1c6b9dae1c 330 else if(red_value==0){
alpha_tango 4:9b1c6b9dae1c 331 return(8);
alpha_tango 4:9b1c6b9dae1c 332 }
alpha_tango 7:1640572360de 333 else{
alpha_tango 7:1640572360de 334 return(9);
alpha_tango 7:1640572360de 335 }
alpha_tango 4:9b1c6b9dae1c 336 // print sensor readings
alpha_tango 4:9b1c6b9dae1c 337
alpha_tango 4:9b1c6b9dae1c 338 //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
alpha_tango 4:9b1c6b9dae1c 339 //wait(0.5);
alpha_tango 4:9b1c6b9dae1c 340 }
alpha_tango 10:ed6ecbb420b8 341 }
alpha_tango 10:ed6ecbb420b8 342
alpha_tango 10:ed6ecbb420b8 343 //this function will use the iterator (i) to find the position it was just at and return there
alpha_tango 10:ed6ecbb420b8 344 //it will also use the color global variable to determine it's path there and the square num to determine
alpha_tango 10:ed6ecbb420b8 345 //how many steps it will need to take in once it gets to its iterator position
alpha_tango 10:ed6ecbb420b8 346 // square num must = 0 for the outer square, 1 for next one in, 2 for following and 3 for innermost square.
alpha_tango 10:ed6ecbb420b8 347
alpha_tango 10:ed6ecbb420b8 348 void returnToPrevious(int i, int square_num)
alpha_tango 8:a6080c27f8c5 349 {
alpha_tango 10:ed6ecbb420b8 350 rot180(); //gets robot back to normal orientation pointing in towards the middle
alpha_tango 10:ed6ecbb420b8 351 if(color == 2 || color == 4 || color == 6 || color == 8)
alpha_tango 9:a5a6d3a48145 352 {
alpha_tango 10:ed6ecbb420b8 353 move(0.2173224, 0); // moves the robot forward 0.7128 feet forward which is the diagonal from color square to back on playing field
alpha_tango 10:ed6ecbb420b8 354 if(color == 2){
alpha_tango 10:ed6ecbb420b8 355 positionIsBlue();
alpha_tango 10:ed6ecbb420b8 356 turnLeft();
alpha_tango 10:ed6ecbb420b8 357 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 358 }
alpha_tango 10:ed6ecbb420b8 359 else if(color == 3){
alpha_tango 10:ed6ecbb420b8 360 positionIsGreen();
alpha_tango 10:ed6ecbb420b8 361 }
alpha_tango 10:ed6ecbb420b8 362 else if(color == 4){
alpha_tango 10:ed6ecbb420b8 363 positionIsRed();
alpha_tango 10:ed6ecbb420b8 364 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 365 }
alpha_tango 10:ed6ecbb420b8 366 else if(color == 6){
alpha_tango 10:ed6ecbb420b8 367 positionIsCyan();
alpha_tango 10:ed6ecbb420b8 368 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 369 }
alpha_tango 10:ed6ecbb420b8 370 else if(color == 7){
alpha_tango 10:ed6ecbb420b8 371 positionIsMagenta();
alpha_tango 10:ed6ecbb420b8 372 }
alpha_tango 10:ed6ecbb420b8 373 else if(color == 8){
alpha_tango 10:ed6ecbb420b8 374 positionIsYellow();
alpha_tango 10:ed6ecbb420b8 375 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 376 }
alpha_tango 9:a5a6d3a48145 377 }
alpha_tango 8:a6080c27f8c5 378
alpha_tango 10:ed6ecbb420b8 379 else{
alpha_tango 10:ed6ecbb420b8 380 move(0.1524, 0); // moves the robot forward half a foot, or the distance from edge of box to line
alpha_tango 10:ed6ecbb420b8 381 if(color == 3){
alpha_tango 10:ed6ecbb420b8 382 positionIsGreen();
alpha_tango 10:ed6ecbb420b8 383 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 384 }
alpha_tango 10:ed6ecbb420b8 385 else if(color == 7){
alpha_tango 10:ed6ecbb420b8 386 positionIsMagenta();
alpha_tango 10:ed6ecbb420b8 387 move(square_num*0.1524, 0);
alpha_tango 10:ed6ecbb420b8 388 }
alpha_tango 9:a5a6d3a48145 389 }
alpha_tango 8:a6080c27f8c5 390 }
alpha_tango 10:ed6ecbb420b8 391
alpha_tango 10:ed6ecbb420b8 392
alpha_tango 10:ed6ecbb420b8 393 void positionIsBlue()
alpha_tango 10:ed6ecbb420b8 394 {
alpha_tango 10:ed6ecbb420b8 395 int pos;
alpha_tango 10:ed6ecbb420b8 396 turnright45();
alpha_tango 10:ed6ecbb420b8 397 for(int j = color; j=i; j--){
alpha_tango 10:ed6ecbb420b8 398 pos = j % 8;
alpha_tango 10:ed6ecbb420b8 399 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 400 if( pos%2 == 1){
alpha_tango 10:ed6ecbb420b8 401 turnLeft();
alpha_tango 10:ed6ecbb420b8 402 }
alpha_tango 10:ed6ecbb420b8 403 }
alpha_tango 10:ed6ecbb420b8 404 }
alpha_tango 10:ed6ecbb420b8 405 void positionIsGreen()
alpha_tango 10:ed6ecbb420b8 406 {
alpha_tango 10:ed6ecbb420b8 407 int pos;
alpha_tango 10:ed6ecbb420b8 408 turnright();
alpha_tango 10:ed6ecbb420b8 409 for(int j = color; j=i; j--){
alpha_tango 10:ed6ecbb420b8 410 pos = j % 8;
alpha_tango 10:ed6ecbb420b8 411 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 412 if( pos%2 == 1){
alpha_tango 10:ed6ecbb420b8 413 turnLeft();
alpha_tango 10:ed6ecbb420b8 414 }
alpha_tango 10:ed6ecbb420b8 415 }
alpha_tango 10:ed6ecbb420b8 416 }
alpha_tango 10:ed6ecbb420b8 417 void positionIsRed()
alpha_tango 10:ed6ecbb420b8 418 {
alpha_tango 10:ed6ecbb420b8 419 int pos;
alpha_tango 10:ed6ecbb420b8 420 turnright45();
alpha_tango 10:ed6ecbb420b8 421 for(int j = color; j=i; j--){
alpha_tango 10:ed6ecbb420b8 422 pos = j % 8;
alpha_tango 10:ed6ecbb420b8 423 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 424 if( pos%2 == 1){
alpha_tango 10:ed6ecbb420b8 425 turnLeft();
alpha_tango 10:ed6ecbb420b8 426 }
alpha_tango 10:ed6ecbb420b8 427 }
alpha_tango 10:ed6ecbb420b8 428 }
alpha_tango 10:ed6ecbb420b8 429 void positionIsCyan()
alpha_tango 10:ed6ecbb420b8 430 {
alpha_tango 10:ed6ecbb420b8 431 int pos;
alpha_tango 10:ed6ecbb420b8 432 turnright45();
alpha_tango 10:ed6ecbb420b8 433 for(int j = color; j=i; j--){
alpha_tango 10:ed6ecbb420b8 434 pos = j % 8;
alpha_tango 10:ed6ecbb420b8 435 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 436 if( pos%2 == 1){
alpha_tango 10:ed6ecbb420b8 437 turnLeft();
alpha_tango 10:ed6ecbb420b8 438 }
alpha_tango 10:ed6ecbb420b8 439 }
alpha_tango 10:ed6ecbb420b8 440 }
alpha_tango 10:ed6ecbb420b8 441 void positionIsMagenta()
alpha_tango 10:ed6ecbb420b8 442 {
alpha_tango 10:ed6ecbb420b8 443 turnright();
alpha_tango 10:ed6ecbb420b8 444 for(int j = color; j=i; j++){
alpha_tango 10:ed6ecbb420b8 445 j = j%8;
alpha_tango 10:ed6ecbb420b8 446 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 447 if( j == 7 || j == 5 || j == 3 || j==1){
alpha_tango 10:ed6ecbb420b8 448 turnLeft();
alpha_tango 10:ed6ecbb420b8 449 }
alpha_tango 10:ed6ecbb420b8 450 }
alpha_tango 10:ed6ecbb420b8 451 }
alpha_tango 10:ed6ecbb420b8 452
alpha_tango 10:ed6ecbb420b8 453 void positionIsYellow()
alpha_tango 10:ed6ecbb420b8 454 {
alpha_tango 10:ed6ecbb420b8 455 int pos;
alpha_tango 10:ed6ecbb420b8 456 turnright45();
alpha_tango 10:ed6ecbb420b8 457 for(int j = color; j=i; j--){
alpha_tango 10:ed6ecbb420b8 458 pos = j % 8;
alpha_tango 10:ed6ecbb420b8 459 move(leg, 0);
alpha_tango 10:ed6ecbb420b8 460 if( pos%2 == 1){
alpha_tango 10:ed6ecbb420b8 461 turnLeft();
alpha_tango 10:ed6ecbb420b8 462 }
alpha_tango 10:ed6ecbb420b8 463 }
alpha_tango 10:ed6ecbb420b8 464 }
alpha_tango 10:ed6ecbb420b8 465