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.
main.cpp@11:93c035708249, 2018-04-01 (annotated)
- Committer:
- alpha_tango
- Date:
- Sun Apr 01 00:50:47 2018 +0000
- Revision:
- 11:93c035708249
- Parent:
- 10:ed6ecbb420b8
- Child:
- 12:92ba52394c85
Fully mapped out course for R, G, & B sorting
Who changed what in which revision?
| User | Revision | Line number | New 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 | 11:93c035708249 | 22 | Ticker timer; // attach this to return home function, set according to round time |
| alpha_tango | 11:93c035708249 | 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 | 10:ed6ecbb420b8 | 30 | void turnRight(); |
| alpha_tango | 10:ed6ecbb420b8 | 31 | void turnLeft(); |
| 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 | 11:93c035708249 | 34 | 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 |
| alpha_tango | 11:93c035708249 | 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 | 11:93c035708249 | 44 | float scale; |
| alpha_tango | 10:ed6ecbb420b8 | 45 | int color; |
| alpha_tango | 4:9b1c6b9dae1c | 46 | |
| alpha_tango | 4:9b1c6b9dae1c | 47 | |
| alpha_tango | 11:93c035708249 | 48 | |
| alpha_tango | 11:93c035708249 | 49 | |
| alpha_tango | 11:93c035708249 | 50 | |
| alpha_tango | 11:93c035708249 | 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 | 11:93c035708249 | 79 | //timer.attach(&returnHome, TIME); |
| 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 | 11:93c035708249 | 129 | move(0.762, FORWARD); |
| alpha_tango | 11:93c035708249 | 130 | |
| alpha_tango | 0:7e54f0e2be67 | 131 | |
| alpha_tango | 0:7e54f0e2be67 | 132 | while(true) |
| alpha_tango | 4:9b1c6b9dae1c | 133 | { |
| alpha_tango | 11:93c035708249 | 134 | 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 | 135 | { |
| alpha_tango | 7:1640572360de | 136 | leg = 0.762; //2.5 feet in meters |
| alpha_tango | 11:93c035708249 | 137 | scale = 1; |
| alpha_tango | 11:93c035708249 | 138 | if(i % 2 == 0) |
| alpha_tango | 11:93c035708249 | 139 | { |
| alpha_tango | 11:93c035708249 | 140 | turnRight(); |
| alpha_tango | 11:93c035708249 | 141 | } |
| alpha_tango | 7:1640572360de | 142 | grabToken(); |
| alpha_tango | 7:1640572360de | 143 | color = findColor(); |
| alpha_tango | 11:93c035708249 | 144 | if (color == 9) |
| alpha_tango | 7:1640572360de | 145 | { |
| alpha_tango | 11:93c035708249 | 146 | return(0); |
| alpha_tango | 7:1640572360de | 147 | } |
| alpha_tango | 7:1640572360de | 148 | else |
| alpha_tango | 7:1640572360de | 149 | { |
| alpha_tango | 11:93c035708249 | 150 | findPathReturn(color, i, scale, leg); |
| alpha_tango | 11:93c035708249 | 151 | dropToken(); |
| alpha_tango | 11:93c035708249 | 152 | //returnPrevious(); |
| alpha_tango | 7:1640572360de | 153 | } |
| alpha_tango | 7:1640572360de | 154 | move(leg, FORWARD); |
| alpha_tango | 7:1640572360de | 155 | |
| alpha_tango | 7:1640572360de | 156 | } |
| alpha_tango | 11:93c035708249 | 157 | |
| alpha_tango | 0:7e54f0e2be67 | 158 | } |
| alpha_tango | 0:7e54f0e2be67 | 159 | } |
| alpha_tango | 4:9b1c6b9dae1c | 160 | |
| alpha_tango | 0:7e54f0e2be67 | 161 | //Distance is in meters |
| alpha_tango | 4:9b1c6b9dae1c | 162 | void move(float dist, bool direction) |
| alpha_tango | 0:7e54f0e2be67 | 163 | { |
| alpha_tango | 0:7e54f0e2be67 | 164 | FLdirection = direction; |
| alpha_tango | 0:7e54f0e2be67 | 165 | FRdirection = !direction; |
| alpha_tango | 0:7e54f0e2be67 | 166 | |
| alpha_tango | 4:9b1c6b9dae1c | 167 | stepFL.period(1.0/FREQUENCY); |
| alpha_tango | 0:7e54f0e2be67 | 168 | stepFR.period(1/FREQUENCY); |
| alpha_tango | 4:9b1c6b9dae1c | 169 | stepFL.write(0.5f); |
| alpha_tango | 4:9b1c6b9dae1c | 170 | stepFR.write(0.5f); |
| alpha_tango | 11:93c035708249 | 171 | |
| alpha_tango | 11:93c035708249 | 172 | wait(4*(dist/stepSize)*(1/FREQUENCY)); //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step |
| alpha_tango | 4:9b1c6b9dae1c | 173 | stepFL.period(0.0f); |
| alpha_tango | 4:9b1c6b9dae1c | 174 | stepFR.period(0.0f); |
| alpha_tango | 4:9b1c6b9dae1c | 175 | stepFL.write(0.0f); |
| alpha_tango | 4:9b1c6b9dae1c | 176 | stepFR.write(0.0f); |
| alpha_tango | 0:7e54f0e2be67 | 177 | |
| alpha_tango | 0:7e54f0e2be67 | 178 | } |
| alpha_tango | 4:9b1c6b9dae1c | 179 | |
| alpha_tango | 0:7e54f0e2be67 | 180 | void grabToken() |
| alpha_tango | 0:7e54f0e2be67 | 181 | { |
| alpha_tango | 4:9b1c6b9dae1c | 182 | highL = 0; |
| alpha_tango | 4:9b1c6b9dae1c | 183 | highH = 1; |
| alpha_tango | 0:7e54f0e2be67 | 184 | enableH = 1; |
| alpha_tango | 0:7e54f0e2be67 | 185 | wait(1); |
| alpha_tango | 0:7e54f0e2be67 | 186 | magDirection = 1; |
| alpha_tango | 0:7e54f0e2be67 | 187 | magArm.period(0.002); |
| alpha_tango | 0:7e54f0e2be67 | 188 | magArm.write(0.5); |
| alpha_tango | 0:7e54f0e2be67 | 189 | wait(0.65); |
| alpha_tango | 0:7e54f0e2be67 | 190 | magArm.period(0); |
| alpha_tango | 0:7e54f0e2be67 | 191 | magArm.write(0); |
| alpha_tango | 0:7e54f0e2be67 | 192 | |
| alpha_tango | 0:7e54f0e2be67 | 193 | } |
| alpha_tango | 4:9b1c6b9dae1c | 194 | |
| alpha_tango | 0:7e54f0e2be67 | 195 | void dropToken() |
| alpha_tango | 0:7e54f0e2be67 | 196 | { |
| alpha_tango | 0:7e54f0e2be67 | 197 | magDirection = 0; |
| alpha_tango | 0:7e54f0e2be67 | 198 | magArm.period(0.002); |
| alpha_tango | 0:7e54f0e2be67 | 199 | magArm.write(0.5); |
| alpha_tango | 0:7e54f0e2be67 | 200 | wait(0.65); |
| alpha_tango | 0:7e54f0e2be67 | 201 | magArm.period(0); |
| alpha_tango | 4:9b1c6b9dae1c | 202 | magArm.write(0); |
| alpha_tango | 4:9b1c6b9dae1c | 203 | highL = 1; |
| alpha_tango | 4:9b1c6b9dae1c | 204 | highH = 0; |
| alpha_tango | 4:9b1c6b9dae1c | 205 | wait(2); |
| alpha_tango | 4:9b1c6b9dae1c | 206 | enableH = 0; |
| alpha_tango | 0:7e54f0e2be67 | 207 | } |
| alpha_tango | 4:9b1c6b9dae1c | 208 | |
| alpha_tango | 11:93c035708249 | 209 | void turnRight(float dist, bool direction) |
| alpha_tango | 2:5f29bc7daa49 | 210 | { |
| alpha_tango | 2:5f29bc7daa49 | 211 | //Get rid of all FR occurences which will turn right motor off |
| alpha_tango | 5:17a8d8395a50 | 212 | FLdirection = 0; //to turn right we want this going FORWARD so a 0; |
| alpha_tango | 2:5f29bc7daa49 | 213 | |
| alpha_tango | 2:5f29bc7daa49 | 214 | |
| alpha_tango | 2:5f29bc7daa49 | 215 | stepFL.period(1/FREQUENCY); |
| alpha_tango | 2:5f29bc7daa49 | 216 | stepFL.write(0.5); |
| alpha_tango | 2:5f29bc7daa49 | 217 | |
| alpha_tango | 2:5f29bc7daa49 | 218 | //dist/stepSize is the number of steps |
| alpha_tango | 2:5f29bc7daa49 | 219 | //1/FREQUENCY is the time per step |
| alpha_tango | 5:17a8d8395a50 | 220 | wait(4*(0.35343/stepSize)*(1/FREQUENCY)); |
| alpha_tango | 2:5f29bc7daa49 | 221 | stepFL.period(0); |
| alpha_tango | 2:5f29bc7daa49 | 222 | stepFL.write(0); |
| alpha_tango | 0:7e54f0e2be67 | 223 | } |
| alpha_tango | 11:93c035708249 | 224 | void turnLeft(float dist, bool direction) |
| alpha_tango | 3:d3264a6f7a62 | 225 | { |
| alpha_tango | 3:d3264a6f7a62 | 226 | //Get rid of all FL occurences which will turn left motor off |
| alpha_tango | 5:17a8d8395a50 | 227 | FRdirection = 1; //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0 |
| alpha_tango | 3:d3264a6f7a62 | 228 | |
| alpha_tango | 3:d3264a6f7a62 | 229 | |
| alpha_tango | 3:d3264a6f7a62 | 230 | stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator. |
| alpha_tango | 3:d3264a6f7a62 | 231 | stepFR.write(0.5); |
| alpha_tango | 3:d3264a6f7a62 | 232 | |
| alpha_tango | 3:d3264a6f7a62 | 233 | //dist/stepSize is the number of steps |
| alpha_tango | 3:d3264a6f7a62 | 234 | //1/FREQUENCY is the time per step |
| alpha_tango | 5:17a8d8395a50 | 235 | wait(4*(0.35343/stepSize)*(1/FREQUENCY)); |
| alpha_tango | 3:d3264a6f7a62 | 236 | stepFR.period(0); |
| alpha_tango | 3:d3264a6f7a62 | 237 | stepFR.write(0); |
| alpha_tango | 3:d3264a6f7a62 | 238 | } |
| alpha_tango | 0:7e54f0e2be67 | 239 | void rot180() |
| alpha_tango | 0:7e54f0e2be67 | 240 | { |
| alpha_tango | 4:9b1c6b9dae1c | 241 | //Get rid of all FR occurences which will turn right motor off |
| alpha_tango | 11:93c035708249 | 242 | FLdirection = 0; //to turn right we want this going FORWARD so a 0; |
| alpha_tango | 4:9b1c6b9dae1c | 243 | |
| alpha_tango | 4:9b1c6b9dae1c | 244 | |
| alpha_tango | 4:9b1c6b9dae1c | 245 | stepFL.period(1/FREQUENCY); |
| alpha_tango | 4:9b1c6b9dae1c | 246 | stepFL.write(0.5); |
| alpha_tango | 4:9b1c6b9dae1c | 247 | |
| alpha_tango | 4:9b1c6b9dae1c | 248 | //dist/stepSize is the number of steps |
| alpha_tango | 4:9b1c6b9dae1c | 249 | //1/FREQUENCY is the time per step |
| alpha_tango | 4:9b1c6b9dae1c | 250 | wait(2*4*(0.35343/stepSize)*(1/FREQUENCY)); |
| alpha_tango | 4:9b1c6b9dae1c | 251 | stepFL.period(0); |
| alpha_tango | 4:9b1c6b9dae1c | 252 | stepFL.write(0); |
| alpha_tango | 0:7e54f0e2be67 | 253 | } |
| alpha_tango | 0:7e54f0e2be67 | 254 | void kill() |
| alpha_tango | 0:7e54f0e2be67 | 255 | { |
| alpha_tango | 0:7e54f0e2be67 | 256 | exit(0); |
| alpha_tango | 0:7e54f0e2be67 | 257 | } |
| alpha_tango | 4:9b1c6b9dae1c | 258 | |
| alpha_tango | 4:9b1c6b9dae1c | 259 | int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk |
| alpha_tango | 4:9b1c6b9dae1c | 260 | |
| alpha_tango | 4:9b1c6b9dae1c | 261 | while (true) { |
| alpha_tango | 4:9b1c6b9dae1c | 262 | wait(1); |
| alpha_tango | 4:9b1c6b9dae1c | 263 | char clear_reg[1] = {148}; |
| alpha_tango | 4:9b1c6b9dae1c | 264 | char clear_data[2] = {0,0}; |
| alpha_tango | 4:9b1c6b9dae1c | 265 | i2c.write(sensor_addr,clear_reg,1, true); |
| alpha_tango | 4:9b1c6b9dae1c | 266 | i2c.read(sensor_addr,clear_data,2, false); |
| alpha_tango | 4:9b1c6b9dae1c | 267 | |
| alpha_tango | 4:9b1c6b9dae1c | 268 | int clear_value = ((int)clear_data[1] << 8) | clear_data[0]; |
| alpha_tango | 4:9b1c6b9dae1c | 269 | |
| alpha_tango | 4:9b1c6b9dae1c | 270 | char red_reg[1] = {150}; |
| alpha_tango | 4:9b1c6b9dae1c | 271 | char red_data[2] = {0,0}; |
| alpha_tango | 4:9b1c6b9dae1c | 272 | i2c.write(sensor_addr,red_reg,1, true); |
| alpha_tango | 4:9b1c6b9dae1c | 273 | i2c.read(sensor_addr,red_data,2, false); |
| alpha_tango | 4:9b1c6b9dae1c | 274 | |
| alpha_tango | 4:9b1c6b9dae1c | 275 | int red_value = ((int)red_data[1] << 8) | red_data[0]; |
| alpha_tango | 4:9b1c6b9dae1c | 276 | |
| alpha_tango | 4:9b1c6b9dae1c | 277 | char green_reg[1] = {152}; |
| alpha_tango | 4:9b1c6b9dae1c | 278 | char green_data[2] = {0,0}; |
| alpha_tango | 4:9b1c6b9dae1c | 279 | i2c.write(sensor_addr,green_reg,1, true); |
| alpha_tango | 4:9b1c6b9dae1c | 280 | i2c.read(sensor_addr,green_data,2, false); |
| alpha_tango | 4:9b1c6b9dae1c | 281 | |
| alpha_tango | 4:9b1c6b9dae1c | 282 | int green_value = ((int)green_data[1] << 8) | green_data[0]; |
| alpha_tango | 4:9b1c6b9dae1c | 283 | |
| alpha_tango | 4:9b1c6b9dae1c | 284 | char blue_reg[1] = {154}; |
| alpha_tango | 4:9b1c6b9dae1c | 285 | char blue_data[2] = {0,0}; |
| alpha_tango | 4:9b1c6b9dae1c | 286 | i2c.write(sensor_addr,blue_reg,1, true); |
| alpha_tango | 4:9b1c6b9dae1c | 287 | i2c.read(sensor_addr,blue_data,2, false); |
| alpha_tango | 4:9b1c6b9dae1c | 288 | |
| alpha_tango | 4:9b1c6b9dae1c | 289 | int blue_value = ((int)blue_data[1] << 8) | blue_data[0]; |
| alpha_tango | 4:9b1c6b9dae1c | 290 | |
| alpha_tango | 4:9b1c6b9dae1c | 291 | |
| alpha_tango | 4:9b1c6b9dae1c | 292 | |
| alpha_tango | 7:1640572360de | 293 | //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error, 9=nothing |
| alpha_tango | 4:9b1c6b9dae1c | 294 | if(blue_value<10000 && red_value>10000){ |
| alpha_tango | 4:9b1c6b9dae1c | 295 | return(1); |
| alpha_tango | 4:9b1c6b9dae1c | 296 | } |
| alpha_tango | 4:9b1c6b9dae1c | 297 | else if(green_value>18000 && blue_value<30000){ |
| alpha_tango | 4:9b1c6b9dae1c | 298 | return(2); |
| alpha_tango | 4:9b1c6b9dae1c | 299 | } |
| alpha_tango | 4:9b1c6b9dae1c | 300 | else if(red_value<10000 && blue_value>15000){ |
| alpha_tango | 4:9b1c6b9dae1c | 301 | return(3); |
| alpha_tango | 4:9b1c6b9dae1c | 302 | } |
| alpha_tango | 4:9b1c6b9dae1c | 303 | else if(blue_value>30000 && red_value<20000){ |
| alpha_tango | 4:9b1c6b9dae1c | 304 | return(4); |
| alpha_tango | 4:9b1c6b9dae1c | 305 | } |
| alpha_tango | 4:9b1c6b9dae1c | 306 | else if(red_value>25000 && green_value<15000){ |
| alpha_tango | 4:9b1c6b9dae1c | 307 | return(5); |
| alpha_tango | 4:9b1c6b9dae1c | 308 | } |
| alpha_tango | 4:9b1c6b9dae1c | 309 | else if(red_value>50000){ |
| alpha_tango | 4:9b1c6b9dae1c | 310 | return(6); |
| alpha_tango | 4:9b1c6b9dae1c | 311 | } |
| alpha_tango | 4:9b1c6b9dae1c | 312 | else if(red_value<10000 && blue_value<10000){ |
| alpha_tango | 4:9b1c6b9dae1c | 313 | return(7); |
| alpha_tango | 4:9b1c6b9dae1c | 314 | } |
| alpha_tango | 4:9b1c6b9dae1c | 315 | else if(red_value==0){ |
| alpha_tango | 4:9b1c6b9dae1c | 316 | return(8); |
| alpha_tango | 4:9b1c6b9dae1c | 317 | } |
| alpha_tango | 7:1640572360de | 318 | else{ |
| alpha_tango | 7:1640572360de | 319 | return(9); |
| alpha_tango | 7:1640572360de | 320 | } |
| alpha_tango | 4:9b1c6b9dae1c | 321 | // print sensor readings |
| alpha_tango | 4:9b1c6b9dae1c | 322 | |
| alpha_tango | 4:9b1c6b9dae1c | 323 | //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value); |
| alpha_tango | 4:9b1c6b9dae1c | 324 | //wait(0.5); |
| alpha_tango | 4:9b1c6b9dae1c | 325 | } |
| alpha_tango | 11:93c035708249 | 326 | //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 | 11:93c035708249 | 327 | // 3 |
| alpha_tango | 11:93c035708249 | 328 | //2*--------*--------*4 |
| alpha_tango | 11:93c035708249 | 329 | // | | |
| alpha_tango | 11:93c035708249 | 330 | // | | |
| alpha_tango | 11:93c035708249 | 331 | // | | |
| alpha_tango | 11:93c035708249 | 332 | // | | |
| alpha_tango | 11:93c035708249 | 333 | //1* *5 |
| alpha_tango | 11:93c035708249 | 334 | // | | |
| alpha_tango | 11:93c035708249 | 335 | // | | |
| alpha_tango | 11:93c035708249 | 336 | // | | |
| alpha_tango | 11:93c035708249 | 337 | // | | |
| alpha_tango | 11:93c035708249 | 338 | // *--------*--------*6 |
| alpha_tango | 11:93c035708249 | 339 | // 0 7 |
| alpha_tango | 11:93c035708249 | 340 | void findPathReturn(int color, int i, float scale, float leg); |
| alpha_tango | 8:a6080c27f8c5 | 341 | { |
| alpha_tango | 11:93c035708249 | 342 | /*******************************************************************///Red |
| alpha_tango | 11:93c035708249 | 343 | if( i == 0 && color == 1 )//condition for return red at index 0 |
| alpha_tango | 11:93c035708249 | 344 | { |
| alpha_tango | 11:93c035708249 | 345 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 346 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 347 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 348 | dropToken(); |
| alpha_tango | 11:93c035708249 | 349 | rot180(); |
| alpha_tango | 11:93c035708249 | 350 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 351 | turnRight(); |
| alpha_tango | 11:93c035708249 | 352 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 353 | rot180(); |
| alpha_tango | 11:93c035708249 | 354 | |
| alpha_tango | 11:93c035708249 | 355 | } |
| alpha_tango | 11:93c035708249 | 356 | if( i == 1 && color == 1 )//condition for return red at index 1 |
| alpha_tango | 11:93c035708249 | 357 | { |
| alpha_tango | 11:93c035708249 | 358 | move((leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 359 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 360 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 361 | dropToken(); |
| alpha_tango | 11:93c035708249 | 362 | rot180(); |
| alpha_tango | 11:93c035708249 | 363 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 364 | turnRight(); |
| alpha_tango | 11:93c035708249 | 365 | move((leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 366 | rot180(); |
| alpha_tango | 11:93c035708249 | 367 | |
| alpha_tango | 11:93c035708249 | 368 | |
| alpha_tango | 11:93c035708249 | 369 | } |
| alpha_tango | 11:93c035708249 | 370 | if( i == 2 && color == 1 )//condition for return red at index 2 |
| alpha_tango | 11:93c035708249 | 371 | { |
| alpha_tango | 11:93c035708249 | 372 | rot180(); |
| alpha_tango | 11:93c035708249 | 373 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 374 | turnRight(); |
| alpha_tango | 11:93c035708249 | 375 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 376 | dropToken(); |
| alpha_tango | 11:93c035708249 | 377 | rot180(); |
| alpha_tango | 11:93c035708249 | 378 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 379 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 380 | move((0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 381 | |
| alpha_tango | 11:93c035708249 | 382 | |
| alpha_tango | 11:93c035708249 | 383 | } |
| alpha_tango | 11:93c035708249 | 384 | if( i == 4 && color == 1 )//condition for return red at index 4 |
| alpha_tango | 9:a5a6d3a48145 | 385 | { |
| alpha_tango | 11:93c035708249 | 386 | turnRight(); |
| alpha_tango | 11:93c035708249 | 387 | move(2*leg + 0.3048*scale, FOWARD); |
| alpha_tango | 11:93c035708249 | 388 | turnRight(); |
| alpha_tango | 11:93c035708249 | 389 | move(0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 390 | dropToken(); |
| alpha_tango | 11:93c035708249 | 391 | rot180(); |
| alpha_tango | 11:93c035708249 | 392 | move(0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 393 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 394 | move(2*leg + 0.3048*scale, FOWARD); |
| alpha_tango | 11:93c035708249 | 395 | turnRight(); |
| alpha_tango | 11:93c035708249 | 396 | |
| alpha_tango | 11:93c035708249 | 397 | } |
| alpha_tango | 11:93c035708249 | 398 | if( i == 5 && color == 1 ) |
| alpha_tango | 11:93c035708249 | 399 | { |
| alpha_tango | 11:93c035708249 | 400 | |
| alpha_tango | 11:93c035708249 | 401 | rot180(); |
| alpha_tango | 11:93c035708249 | 402 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 403 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 404 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 405 | dropToken(); |
| alpha_tango | 11:93c035708249 | 406 | rot180(); |
| alpha_tango | 11:93c035708249 | 407 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 408 | turnRight(); |
| alpha_tango | 11:93c035708249 | 409 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 410 | } |
| alpha_tango | 11:93c035708249 | 411 | if( i == 6 && color == 1 ) //condition for return red at index 5 |
| alpha_tango | 11:93c035708249 | 412 | { |
| alpha_tango | 11:93c035708249 | 413 | turnRight(); |
| alpha_tango | 11:93c035708249 | 414 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 415 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 416 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 417 | dropToken(); |
| alpha_tango | 11:93c035708249 | 418 | rot180(); |
| alpha_tango | 11:93c035708249 | 419 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 420 | turnRight(); |
| alpha_tango | 11:93c035708249 | 421 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 422 | |
| alpha_tango | 11:93c035708249 | 423 | } |
| alpha_tango | 11:93c035708249 | 424 | |
| alpha_tango | 11:93c035708249 | 425 | /*******************************************************************///Green |
| alpha_tango | 11:93c035708249 | 426 | if( i == 0 && color == 2 )//condition for return Green at index 0 |
| alpha_tango | 11:93c035708249 | 427 | { |
| alpha_tango | 11:93c035708249 | 428 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 429 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 430 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 431 | dropToken(); |
| alpha_tango | 11:93c035708249 | 432 | rot180(); |
| alpha_tango | 11:93c035708249 | 433 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 434 | turnRight(); |
| alpha_tango | 11:93c035708249 | 435 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 436 | rot180(); |
| alpha_tango | 11:93c035708249 | 437 | |
| alpha_tango | 11:93c035708249 | 438 | } |
| alpha_tango | 11:93c035708249 | 439 | if( i == 1 && color == 2 )//condition for return green at index 1 |
| alpha_tango | 11:93c035708249 | 440 | { |
| alpha_tango | 11:93c035708249 | 441 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 442 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 443 | dropToken(); |
| alpha_tango | 11:93c035708249 | 444 | rot180(); |
| alpha_tango | 11:93c035708249 | 445 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 446 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 447 | |
| alpha_tango | 11:93c035708249 | 448 | } |
| alpha_tango | 11:93c035708249 | 449 | if( i == 2 && color == 2 )//condition for return green at index 2 |
| alpha_tango | 11:93c035708249 | 450 | { |
| alpha_tango | 11:93c035708249 | 451 | turnRight(); |
| alpha_tango | 11:93c035708249 | 452 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 453 | turnRight(); |
| alpha_tango | 11:93c035708249 | 454 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 455 | dropToken(); |
| alpha_tango | 11:93c035708249 | 456 | rot180(); |
| alpha_tango | 11:93c035708249 | 457 | move(0.3046*scale); |
| alpha_tango | 11:93c035708249 | 458 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 459 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 460 | turnRight(); |
| alpha_tango | 11:93c035708249 | 461 | |
| alpha_tango | 9:a5a6d3a48145 | 462 | } |
| alpha_tango | 8:a6080c27f8c5 | 463 | |
| alpha_tango | 11:93c035708249 | 464 | if( i == 4 && color == 2 ) //condition for return green at index 4 |
| alpha_tango | 11:93c035708249 | 465 | { |
| alpha_tango | 11:93c035708249 | 466 | turnRight(); |
| alpha_tango | 11:93c035708249 | 467 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 468 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 469 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 470 | dropToken(); |
| alpha_tango | 11:93c035708249 | 471 | rot180(); |
| alpha_tango | 11:93c035708249 | 472 | move(leg, FORWARD); |
| alpha_tango | 11:93c035708249 | 473 | turnRight(); |
| alpha_tango | 11:93c035708249 | 474 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 475 | turnRight(); |
| alpha_tango | 11:93c035708249 | 476 | |
| alpha_tango | 11:93c035708249 | 477 | |
| alpha_tango | 11:93c035708249 | 478 | |
| alpha_tango | 11:93c035708249 | 479 | } |
| alpha_tango | 11:93c035708249 | 480 | if( i == 5 && color == 2 )//condition for return green at index 5 |
| alpha_tango | 11:93c035708249 | 481 | { |
| alpha_tango | 11:93c035708249 | 482 | turnRight(); |
| alpha_tango | 11:93c035708249 | 483 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 484 | dropToken(); |
| alpha_tango | 11:93c035708249 | 485 | rot180(); |
| alpha_tango | 11:93c035708249 | 486 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 487 | turnRight(); |
| alpha_tango | 11:93c035708249 | 488 | |
| alpha_tango | 11:93c035708249 | 489 | } |
| alpha_tango | 11:93c035708249 | 490 | if( i == 6 && color == 2 )//condition for return green at index 6 |
| alpha_tango | 11:93c035708249 | 491 | { |
| alpha_tango | 11:93c035708249 | 492 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 493 | turnRight(); |
| alpha_tango | 11:93c035708249 | 494 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 495 | dropToken(); |
| alpha_tango | 11:93c035708249 | 496 | rot180(); |
| alpha_tango | 11:93c035708249 | 497 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 498 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 499 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 500 | rot180(); |
| alpha_tango | 11:93c035708249 | 501 | } |
| alpha_tango | 11:93c035708249 | 502 | /*******************************************************************///Blue |
| alpha_tango | 11:93c035708249 | 503 | if( i == 0 && color == 3 )//condition for return Blue at index 0 :: |
| alpha_tango | 11:93c035708249 | 504 | { |
| alpha_tango | 11:93c035708249 | 505 | rot180(); |
| alpha_tango | 11:93c035708249 | 506 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 507 | turnRight(); |
| alpha_tango | 11:93c035708249 | 508 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 509 | dropToken(); |
| alpha_tango | 11:93c035708249 | 510 | rot180(); |
| alpha_tango | 11:93c035708249 | 511 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 512 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 513 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 514 | |
| alpha_tango | 9:a5a6d3a48145 | 515 | } |
| alpha_tango | 11:93c035708249 | 516 | if( i == 1 && color == 3 ) //condition for return blue at index 1 |
| alpha_tango | 11:93c035708249 | 517 | { |
| alpha_tango | 11:93c035708249 | 518 | rot180(); |
| alpha_tango | 11:93c035708249 | 519 | move((leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 520 | turnRight(); |
| alpha_tango | 11:93c035708249 | 521 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 522 | dropToken(); |
| alpha_tango | 11:93c035708249 | 523 | rot180(); |
| alpha_tango | 11:93c035708249 | 524 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 525 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 526 | move((leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 527 | |
| alpha_tango | 11:93c035708249 | 528 | } |
| alpha_tango | 11:93c035708249 | 529 | if( i == 2 && color == 3 ) //condition for return blue at index 2 |
| alpha_tango | 11:93c035708249 | 530 | { |
| alpha_tango | 11:93c035708249 | 531 | turnRight(); |
| alpha_tango | 11:93c035708249 | 532 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 533 | turnRight(); |
| alpha_tango | 11:93c035708249 | 534 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 535 | dropToken(); |
| alpha_tango | 11:93c035708249 | 536 | rot180(); |
| alpha_tango | 11:93c035708249 | 537 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 538 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 539 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 540 | turnRight(); |
| alpha_tango | 11:93c035708249 | 541 | |
| alpha_tango | 11:93c035708249 | 542 | |
| alpha_tango | 11:93c035708249 | 543 | } |
| alpha_tango | 11:93c035708249 | 544 | if( i == 4 && color == 3 ) //condition for return blue at index 4 |
| alpha_tango | 11:93c035708249 | 545 | { |
| alpha_tango | 11:93c035708249 | 546 | turnRight(); |
| alpha_tango | 11:93c035708249 | 547 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 548 | turnRight(); |
| alpha_tango | 11:93c035708249 | 549 | move((2*leg) + (0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 550 | dropToken(); |
| alpha_tango | 11:93c035708249 | 551 | rot180(); |
| alpha_tango | 11:93c035708249 | 552 | move((2*leg + 0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 553 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 554 | move((2*leg) + (0.3048*scale), FORWARD); |
| alpha_tango | 11:93c035708249 | 555 | turnRight(); |
| alpha_tango | 10:ed6ecbb420b8 | 556 | |
| alpha_tango | 11:93c035708249 | 557 | |
| alpha_tango | 11:93c035708249 | 558 | } |
| alpha_tango | 11:93c035708249 | 559 | if( i == 5 && color == 3 ) //condition for return blue at index 5 |
| alpha_tango | 11:93c035708249 | 560 | { |
| alpha_tango | 11:93c035708249 | 561 | |
| alpha_tango | 11:93c035708249 | 562 | move(leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 563 | turnRight(); |
| alpha_tango | 11:93c035708249 | 564 | move(2*leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 565 | dropToken(); |
| alpha_tango | 11:93c035708249 | 566 | rot180(); |
| alpha_tango | 11:93c035708249 | 567 | move(2*leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 568 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 569 | move(leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 570 | rot180(); |
| alpha_tango | 11:93c035708249 | 571 | |
| alpha_tango | 11:93c035708249 | 572 | |
| alpha_tango | 10:ed6ecbb420b8 | 573 | |
| alpha_tango | 11:93c035708249 | 574 | |
| alpha_tango | 11:93c035708249 | 575 | } |
| alpha_tango | 11:93c035708249 | 576 | if( i == 6 && color == 3 ) //condition for return blue at index 5 |
| alpha_tango | 11:93c035708249 | 577 | { |
| alpha_tango | 11:93c035708249 | 578 | move(2*leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 579 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 580 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 581 | dropToken(); |
| alpha_tango | 11:93c035708249 | 582 | rot180(); |
| alpha_tango | 11:93c035708249 | 583 | move(0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 584 | turnRight(); |
| alpha_tango | 11:93c035708249 | 585 | move(2*leg + 0.3048*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 586 | rot180(); |
| alpha_tango | 11:93c035708249 | 587 | |
| alpha_tango | 11:93c035708249 | 588 | } |
| alpha_tango | 11:93c035708249 | 589 | /*******************************************************************///Cyan |
| alpha_tango | 11:93c035708249 | 590 | if( i == 0 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 591 | { |
| alpha_tango | 11:93c035708249 | 592 | //condition for return Cyan at index 0 |
| alpha_tango | 11:93c035708249 | 593 | |
| alpha_tango | 11:93c035708249 | 594 | } |
| alpha_tango | 11:93c035708249 | 595 | if( i == 1 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 596 | { |
| alpha_tango | 11:93c035708249 | 597 | //condition for return Cyan at index 1 |
| alpha_tango | 11:93c035708249 | 598 | |
| alpha_tango | 11:93c035708249 | 599 | } |
| alpha_tango | 11:93c035708249 | 600 | if( i == 2 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 601 | { |
| alpha_tango | 11:93c035708249 | 602 | //condition for return Cyan at index 2 |
| alpha_tango | 11:93c035708249 | 603 | |
| alpha_tango | 11:93c035708249 | 604 | } |
| alpha_tango | 11:93c035708249 | 605 | if( i == 4 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 606 | { |
| alpha_tango | 11:93c035708249 | 607 | //condition for return Cyan at index 4 |
| alpha_tango | 11:93c035708249 | 608 | |
| alpha_tango | 11:93c035708249 | 609 | } |
| alpha_tango | 11:93c035708249 | 610 | if( i == 5 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 611 | { |
| alpha_tango | 11:93c035708249 | 612 | //condition for return Cyan at index 5 |
| alpha_tango | 11:93c035708249 | 613 | |
| alpha_tango | 11:93c035708249 | 614 | } |
| alpha_tango | 11:93c035708249 | 615 | if( i == 6 && color == 4 ) |
| alpha_tango | 11:93c035708249 | 616 | { |
| alpha_tango | 11:93c035708249 | 617 | //condition for return Cyan at index 6 |
| alpha_tango | 11:93c035708249 | 618 | |
| alpha_tango | 11:93c035708249 | 619 | } |
| alpha_tango | 11:93c035708249 | 620 | /*******************************************************************///Magenta |
| alpha_tango | 11:93c035708249 | 621 | if( i == 0 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 622 | { |
| alpha_tango | 11:93c035708249 | 623 | //condition for return Magenta at index 0 |
| alpha_tango | 11:93c035708249 | 624 | |
| alpha_tango | 11:93c035708249 | 625 | } |
| alpha_tango | 11:93c035708249 | 626 | if( i == 1 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 627 | { |
| alpha_tango | 11:93c035708249 | 628 | //condition for return Magenta at index 1 |
| alpha_tango | 11:93c035708249 | 629 | |
| alpha_tango | 11:93c035708249 | 630 | } |
| alpha_tango | 11:93c035708249 | 631 | if( i == 2 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 632 | { |
| alpha_tango | 11:93c035708249 | 633 | //condition for return Magenta at index 2 |
| alpha_tango | 11:93c035708249 | 634 | |
| alpha_tango | 11:93c035708249 | 635 | } |
| alpha_tango | 11:93c035708249 | 636 | if( i == 4 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 637 | { |
| alpha_tango | 11:93c035708249 | 638 | //condition for return Magenta at index 4 |
| alpha_tango | 11:93c035708249 | 639 | |
| alpha_tango | 11:93c035708249 | 640 | } |
| alpha_tango | 11:93c035708249 | 641 | if( i == 5 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 642 | { |
| alpha_tango | 11:93c035708249 | 643 | //condition for return Magenta at index 5 |
| alpha_tango | 11:93c035708249 | 644 | |
| alpha_tango | 11:93c035708249 | 645 | } |
| alpha_tango | 11:93c035708249 | 646 | if( i == 6 && color == 5 ) |
| alpha_tango | 11:93c035708249 | 647 | { |
| alpha_tango | 11:93c035708249 | 648 | //condition for return Magenta at index 6 |
| alpha_tango | 11:93c035708249 | 649 | |
| alpha_tango | 11:93c035708249 | 650 | } |
| alpha_tango | 11:93c035708249 | 651 | /*******************************************************************///Yellow |
| alpha_tango | 11:93c035708249 | 652 | if( i == 0 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 653 | { |
| alpha_tango | 11:93c035708249 | 654 | //condition for return Yellow at index 0 |
| alpha_tango | 11:93c035708249 | 655 | |
| alpha_tango | 11:93c035708249 | 656 | } |
| alpha_tango | 11:93c035708249 | 657 | if( i == 1 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 658 | { |
| alpha_tango | 11:93c035708249 | 659 | //condition for return Yellow at index 1 |
| alpha_tango | 11:93c035708249 | 660 | |
| alpha_tango | 11:93c035708249 | 661 | } |
| alpha_tango | 11:93c035708249 | 662 | if( i == 2 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 663 | { |
| alpha_tango | 11:93c035708249 | 664 | //condition for return Yellow at index 2 |
| alpha_tango | 11:93c035708249 | 665 | |
| alpha_tango | 11:93c035708249 | 666 | } |
| alpha_tango | 11:93c035708249 | 667 | if( i == 4 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 668 | { |
| alpha_tango | 11:93c035708249 | 669 | //condition for return Yellow at index 4 |
| alpha_tango | 11:93c035708249 | 670 | |
| alpha_tango | 11:93c035708249 | 671 | } |
| alpha_tango | 11:93c035708249 | 672 | if( i == 5 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 673 | { |
| alpha_tango | 11:93c035708249 | 674 | //condition for return Yellow at index 5 |
| alpha_tango | 11:93c035708249 | 675 | |
| alpha_tango | 11:93c035708249 | 676 | } |
| alpha_tango | 11:93c035708249 | 677 | if( i == 6 && color == 6 ) |
| alpha_tango | 11:93c035708249 | 678 | { |
| alpha_tango | 11:93c035708249 | 679 | //condition for return Yellow at index 6 |
| alpha_tango | 11:93c035708249 | 680 | |
| alpha_tango | 11:93c035708249 | 681 | } |
| alpha_tango | 11:93c035708249 | 682 | /*******************************************************************///Gray |
| alpha_tango | 11:93c035708249 | 683 | if( i == 0 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 684 | { |
| alpha_tango | 11:93c035708249 | 685 | //condition for return Gray at index 0 |
| alpha_tango | 11:93c035708249 | 686 | |
| alpha_tango | 11:93c035708249 | 687 | } |
| alpha_tango | 11:93c035708249 | 688 | if( i == 1 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 689 | { |
| alpha_tango | 11:93c035708249 | 690 | //condition for return Gray at index 1 |
| alpha_tango | 11:93c035708249 | 691 | |
| alpha_tango | 11:93c035708249 | 692 | } |
| alpha_tango | 11:93c035708249 | 693 | if( i == 2 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 694 | { |
| alpha_tango | 11:93c035708249 | 695 | //condition for return Gray at index 2 |
| alpha_tango | 11:93c035708249 | 696 | |
| alpha_tango | 11:93c035708249 | 697 | } |
| alpha_tango | 11:93c035708249 | 698 | if( i == 4 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 699 | { |
| alpha_tango | 11:93c035708249 | 700 | //condition for return Gray at index 4 |
| alpha_tango | 11:93c035708249 | 701 | |
| alpha_tango | 11:93c035708249 | 702 | } |
| alpha_tango | 11:93c035708249 | 703 | if( i == 5 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 704 | { |
| alpha_tango | 11:93c035708249 | 705 | //condition for return Gray at index 5 |
| alpha_tango | 11:93c035708249 | 706 | |
| alpha_tango | 11:93c035708249 | 707 | } |
| alpha_tango | 11:93c035708249 | 708 | if( i == 6 && color == 7 ) |
| alpha_tango | 11:93c035708249 | 709 | { |
| alpha_tango | 11:93c035708249 | 710 | //condition for return Gray at index 6 |
| alpha_tango | 11:93c035708249 | 711 | |
| alpha_tango | 11:93c035708249 | 712 | } |
| alpha_tango | 11:93c035708249 | 713 | else |
| alpha_tango | 11:93c035708249 | 714 | {} |
| alpha_tango | 11:93c035708249 | 715 | /*******************************************************************/ |
| alpha_tango | 11:93c035708249 | 716 | |
| alpha_tango | 11:93c035708249 | 717 | } |
| alpha_tango | 11:93c035708249 | 718 | } |