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@12:92ba52394c85, 2018-04-01 (annotated)
- Committer:
- alpha_tango
- Date:
- Sun Apr 01 01:46:03 2018 +0000
- Revision:
- 12:92ba52394c85
- Parent:
- 11:93c035708249
- Child:
- 13:2c8f8e77b549
Completed find and return path ;
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 | 12:92ba52394c85 | 590 | if( i == 0 && color == 4 )//condition for return Cyan at index 0 |
| alpha_tango | 11:93c035708249 | 591 | { |
| alpha_tango | 12:92ba52394c85 | 592 | move(2*leg + 0.3046*scale, FORWARD) |
| alpha_tango | 12:92ba52394c85 | 593 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 594 | move(2*leg + 0.3046*scale, FORWARD) |
| alpha_tango | 12:92ba52394c85 | 595 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 596 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 597 | move(2*leg + 0.3046*scale, FORWARD) |
| alpha_tango | 12:92ba52394c85 | 598 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 599 | move(2*leg + 0.3046*scale, FORWARD) |
| alpha_tango | 12:92ba52394c85 | 600 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 601 | } |
| alpha_tango | 12:92ba52394c85 | 602 | if( i == 1 && color == 4 )//condition for return Cyan at index 1 |
| alpha_tango | 12:92ba52394c85 | 603 | { |
| alpha_tango | 12:92ba52394c85 | 604 | move(leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 605 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 606 | move(2*leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 607 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 608 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 609 | move(2*leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 610 | turnleft(); |
| alpha_tango | 12:92ba52394c85 | 611 | move(leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 612 | rot180(); |
| alpha_tango | 11:93c035708249 | 613 | |
| alpha_tango | 11:93c035708249 | 614 | } |
| alpha_tango | 12:92ba52394c85 | 615 | if( i == 2 && color == 4 )//condition for return Cyan at index 2 |
| alpha_tango | 11:93c035708249 | 616 | { |
| alpha_tango | 12:92ba52394c85 | 617 | move(2*leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 618 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 619 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 620 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 621 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 622 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 623 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 624 | move(2*leg + 0.3046*scale, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 625 | rot180(); |
| alpha_tango | 11:93c035708249 | 626 | |
| alpha_tango | 11:93c035708249 | 627 | } |
| alpha_tango | 12:92ba52394c85 | 628 | if( i == 4 && color == 4 )//condition for return Cyan at index 4 |
| alpha_tango | 11:93c035708249 | 629 | { |
| alpha_tango | 12:92ba52394c85 | 630 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 631 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 632 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 633 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 634 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 635 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 636 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 637 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 638 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 639 | turnleft(); |
| alpha_tango | 12:92ba52394c85 | 640 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 641 | turnRight(); |
| alpha_tango | 11:93c035708249 | 642 | |
| alpha_tango | 11:93c035708249 | 643 | } |
| alpha_tango | 12:92ba52394c85 | 644 | if( i == 5 && color == 4 )//condition for return Cyan at index 5 |
| alpha_tango | 11:93c035708249 | 645 | { |
| alpha_tango | 12:92ba52394c85 | 646 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 647 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 648 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 649 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 650 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 651 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 652 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 653 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 654 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 655 | |
| alpha_tango | 11:93c035708249 | 656 | } |
| alpha_tango | 12:92ba52394c85 | 657 | if( i == 6 && color == 4 )//condition for return Cyan at index 6 |
| alpha_tango | 11:93c035708249 | 658 | { |
| alpha_tango | 12:92ba52394c85 | 659 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 660 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 661 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 662 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 663 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 664 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 665 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 666 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 667 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 668 | turnRight(); |
| alpha_tango | 11:93c035708249 | 669 | |
| alpha_tango | 11:93c035708249 | 670 | } |
| alpha_tango | 11:93c035708249 | 671 | /*******************************************************************///Magenta |
| alpha_tango | 12:92ba52394c85 | 672 | if( i == 0 && color == 5 )//condition for return Magenta at index 0 |
| alpha_tango | 11:93c035708249 | 673 | { |
| alpha_tango | 12:92ba52394c85 | 674 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 675 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 676 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 677 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 678 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 679 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 680 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 681 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 682 | rot180(); |
| alpha_tango | 11:93c035708249 | 683 | |
| alpha_tango | 11:93c035708249 | 684 | } |
| alpha_tango | 12:92ba52394c85 | 685 | if( i == 1 && color == 5 )//condition for return Magenta at index 1 |
| alpha_tango | 11:93c035708249 | 686 | { |
| alpha_tango | 12:92ba52394c85 | 687 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 688 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 689 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 690 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 691 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 692 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 693 | |
| alpha_tango | 12:92ba52394c85 | 694 | } |
| alpha_tango | 12:92ba52394c85 | 695 | if( i == 2 && color == 5 )//condition for return Magenta at index 2 |
| alpha_tango | 12:92ba52394c85 | 696 | { |
| alpha_tango | 12:92ba52394c85 | 697 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 698 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 699 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 700 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 701 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 702 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 703 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 704 | move(2*leg + 0.3046*scalem FORWARD); |
| alpha_tango | 12:92ba52394c85 | 705 | rot180(); |
| alpha_tango | 11:93c035708249 | 706 | |
| alpha_tango | 11:93c035708249 | 707 | } |
| alpha_tango | 12:92ba52394c85 | 708 | if( i == 4 && color == 5 )//condition for return Magenta at index 4 |
| alpha_tango | 11:93c035708249 | 709 | { |
| alpha_tango | 12:92ba52394c85 | 710 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 711 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 712 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 713 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 714 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 715 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 716 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 717 | Move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 718 | rot180(); |
| alpha_tango | 11:93c035708249 | 719 | |
| alpha_tango | 11:93c035708249 | 720 | } |
| alpha_tango | 12:92ba52394c85 | 721 | if( i == 5 && color == 5 )//condition for return Magenta at index 5 |
| alpha_tango | 11:93c035708249 | 722 | { |
| alpha_tango | 12:92ba52394c85 | 723 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 724 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 725 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 726 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 727 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 728 | turnLeft(); |
| alpha_tango | 11:93c035708249 | 729 | |
| alpha_tango | 11:93c035708249 | 730 | } |
| alpha_tango | 12:92ba52394c85 | 731 | if( i == 6 && color == 5 )//condition for return Magenta at index 6 |
| alpha_tango | 11:93c035708249 | 732 | { |
| alpha_tango | 12:92ba52394c85 | 733 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 734 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 735 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 736 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 737 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 738 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 739 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 740 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 741 | move(lef, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 742 | turnRight(); |
| alpha_tango | 11:93c035708249 | 743 | |
| alpha_tango | 11:93c035708249 | 744 | } |
| alpha_tango | 11:93c035708249 | 745 | /*******************************************************************///Yellow |
| alpha_tango | 12:92ba52394c85 | 746 | if( i == 0 && color == 6 )//condition for return Yellow at index 0 |
| alpha_tango | 11:93c035708249 | 747 | { |
| alpha_tango | 12:92ba52394c85 | 748 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 749 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 750 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 751 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 752 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 753 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 754 | move(0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 755 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 756 | move(2*leg + 0.3046*scale); |
| alpha_tango | 12:92ba52394c85 | 757 | turnRight(); |
| alpha_tango | 11:93c035708249 | 758 | |
| alpha_tango | 11:93c035708249 | 759 | } |
| alpha_tango | 12:92ba52394c85 | 760 | if( i == 1 && color == 6 )//condition for return Yellow at index 1 |
| alpha_tango | 11:93c035708249 | 761 | { |
| alpha_tango | 12:92ba52394c85 | 762 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 763 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 764 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 765 | move(2*leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 766 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 767 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 768 | move(2*leg, FOWARD); |
| alpha_tango | 12:92ba52394c85 | 769 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 770 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 771 | |
| alpha_tango | 11:93c035708249 | 772 | |
| alpha_tango | 11:93c035708249 | 773 | } |
| alpha_tango | 12:92ba52394c85 | 774 | if( i == 2 && color == 6 )//condition for return Yellow at index 2 |
| alpha_tango | 11:93c035708249 | 775 | { |
| alpha_tango | 12:92ba52394c85 | 776 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 777 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 778 | move(2*leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 779 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 780 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 781 | move(2*leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 782 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 783 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 784 | rot180(); |
| alpha_tango | 11:93c035708249 | 785 | |
| alpha_tango | 11:93c035708249 | 786 | } |
| alpha_tango | 12:92ba52394c85 | 787 | if( i == 4 && color == 6 )//condition for return Yellow at index 4 |
| alpha_tango | 11:93c035708249 | 788 | { |
| alpha_tango | 12:92ba52394c85 | 789 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 790 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 791 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 792 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 793 | rot180(0); |
| alpha_tango | 12:92ba52394c85 | 794 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 795 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 796 | move(2*leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 797 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 798 | |
| alpha_tango | 11:93c035708249 | 799 | |
| alpha_tango | 11:93c035708249 | 800 | } |
| alpha_tango | 12:92ba52394c85 | 801 | if( i == 5 && color == 6 )//condition for return Yellow at index 5 |
| alpha_tango | 11:93c035708249 | 802 | { |
| alpha_tango | 12:92ba52394c85 | 803 | move(leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 804 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 805 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 806 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 807 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 808 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 809 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 810 | move(Leg + 0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 811 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 812 | |
| alpha_tango | 11:93c035708249 | 813 | |
| alpha_tango | 11:93c035708249 | 814 | } |
| alpha_tango | 12:92ba52394c85 | 815 | if( i == 6 && color == 6 )//condition for return Yellow at index 6 |
| alpha_tango | 11:93c035708249 | 816 | { |
| alpha_tango | 12:92ba52394c85 | 817 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 818 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 819 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 820 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 821 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 822 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 823 | move(0.3046*scale, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 824 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 825 | move(0.3046*scale, FORWARD); |
| alpha_tango | 11:93c035708249 | 826 | |
| alpha_tango | 11:93c035708249 | 827 | } |
| alpha_tango | 11:93c035708249 | 828 | /*******************************************************************///Gray |
| alpha_tango | 12:92ba52394c85 | 829 | if( i == 0 && color == 7 )//condition for return Gray at index 0 |
| alpha_tango | 11:93c035708249 | 830 | { |
| alpha_tango | 12:92ba52394c85 | 831 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 832 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 833 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 834 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 835 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 836 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 837 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 838 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 839 | |
| alpha_tango | 11:93c035708249 | 840 | |
| alpha_tango | 11:93c035708249 | 841 | } |
| alpha_tango | 12:92ba52394c85 | 842 | if( i == 1 && color == 7 )//condition for return Gray at index 1 |
| alpha_tango | 12:92ba52394c85 | 843 | { |
| alpha_tango | 12:92ba52394c85 | 844 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 845 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 846 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 847 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 848 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 849 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 850 | } |
| alpha_tango | 12:92ba52394c85 | 851 | if( i == 2 && color == 7 ) //condition for return Gray at index 2 |
| alpha_tango | 11:93c035708249 | 852 | { |
| alpha_tango | 12:92ba52394c85 | 853 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 854 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 855 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 856 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 857 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 858 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 859 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 860 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 861 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 862 | turnRight(); |
| alpha_tango | 11:93c035708249 | 863 | |
| alpha_tango | 11:93c035708249 | 864 | } |
| alpha_tango | 12:92ba52394c85 | 865 | if( i == 4 && color == 7 ) //condition for return Gray at index 4 |
| alpha_tango | 11:93c035708249 | 866 | { |
| alpha_tango | 12:92ba52394c85 | 867 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 868 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 869 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 870 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 871 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 872 | move(leg, FORARD); |
| alpha_tango | 12:92ba52394c85 | 873 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 874 | move(lef, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 875 | rot180(); |
| alpha_tango | 11:93c035708249 | 876 | |
| alpha_tango | 11:93c035708249 | 877 | } |
| alpha_tango | 12:92ba52394c85 | 878 | if( i == 5 && color == 7 )//condition for return Gray at index 5 |
| alpha_tango | 11:93c035708249 | 879 | { |
| alpha_tango | 12:92ba52394c85 | 880 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 881 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 882 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 883 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 884 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 885 | turnRight(); |
| alpha_tango | 11:93c035708249 | 886 | |
| alpha_tango | 11:93c035708249 | 887 | } |
| alpha_tango | 12:92ba52394c85 | 888 | if( i == 6 && color == 7 )//condition for return Gray at index 6 |
| alpha_tango | 11:93c035708249 | 889 | { |
| alpha_tango | 12:92ba52394c85 | 890 | turnRight(); |
| alpha_tango | 12:92ba52394c85 | 891 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 892 | turnLeft(); |
| alpha_tango | 12:92ba52394c85 | 893 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 894 | dropToken(); |
| alpha_tango | 12:92ba52394c85 | 895 | rot180(); |
| alpha_tango | 12:92ba52394c85 | 896 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 897 | rightRight(); |
| alpha_tango | 12:92ba52394c85 | 898 | move(leg, FORWARD); |
| alpha_tango | 12:92ba52394c85 | 899 | turnRight(); |
| alpha_tango | 11:93c035708249 | 900 | |
| alpha_tango | 11:93c035708249 | 901 | } |
| alpha_tango | 11:93c035708249 | 902 | else |
| alpha_tango | 11:93c035708249 | 903 | {} |
| alpha_tango | 11:93c035708249 | 904 | /*******************************************************************/ |
| alpha_tango | 11:93c035708249 | 905 | |
| alpha_tango | 11:93c035708249 | 906 | } |
| alpha_tango | 11:93c035708249 | 907 | } |