Raj Patel / Mbed 2 deprecated skittleSorter

Skittle Sorter Running Code.

Dependencies:   Motor Servo mbed

main.cpp@0:2df76cd30860, 2016-04-29 (annotated)

Committer:
raj1995
Date:
Fri Apr 29 18:23:18 2016 +0000
Revision:
0:2df76cd30860
Skittle Sorter Code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raj1995 0:2df76cd30860 1 #include <math.h>
raj1995 0:2df76cd30860 2 #include <stdio.h>
raj1995 0:2df76cd30860 3 #include "mbed.h"
raj1995 0:2df76cd30860 4 #define commonAnode true
raj1995 0:2df76cd30860 5 #include <algorithm>
raj1995 0:2df76cd30860 6 #include "Servo.h"
raj1995 0:2df76cd30860 7
raj1995 0:2df76cd30860 8
raj1995 0:2df76cd30860 9 //Color Sensor
raj1995 0:2df76cd30860 10 I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
raj1995 0:2df76cd30860 11 int sensor_addr = 41 << 1;
raj1995 0:2df76cd30860 12 DigitalOut led(p11);
raj1995 0:2df76cd30860 13
raj1995 0:2df76cd30860 14 //IR
raj1995 0:2df76cd30860 15 Serial pc(USBTX, USBRX);
raj1995 0:2df76cd30860 16 Serial device(p13, p14); // tx, rx
raj1995 0:2df76cd30860 17 PwmOut IRLED(p21);
raj1995 0:2df76cd30860 18
raj1995 0:2df76cd30860 19 //Solenoid
raj1995 0:2df76cd30860 20 DigitalOut Ctrl(p8); //Solenoid output control bit
raj1995 0:2df76cd30860 21
raj1995 0:2df76cd30860 22
raj1995 0:2df76cd30860 23 //Motor A
raj1995 0:2df76cd30860 24 PwmOut speedA(p23); // PWM Speed pin for Motor A
raj1995 0:2df76cd30860 25 DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A
raj1995 0:2df76cd30860 26 DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A
raj1995 0:2df76cd30860 27
raj1995 0:2df76cd30860 28 //MotorB
raj1995 0:2df76cd30860 29 PwmOut speedB(p24); // PWM Speed pin for Motor B
raj1995 0:2df76cd30860 30 DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B
raj1995 0:2df76cd30860 31 DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B
raj1995 0:2df76cd30860 32
raj1995 0:2df76cd30860 33
raj1995 0:2df76cd30860 34 //Bottom Encoder
raj1995 0:2df76cd30860 35 DigitalIn Enc1(p16); //right motor
raj1995 0:2df76cd30860 36 int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
raj1995 0:2df76cd30860 37 int ticks = 0;
raj1995 0:2df76cd30860 38 int ticksPrime = 0;
raj1995 0:2df76cd30860 39 int dist2move = 0;
raj1995 0:2df76cd30860 40
raj1995 0:2df76cd30860 41 //Top Encoder
raj1995 0:2df76cd30860 42 DigitalIn Enc2(p15); //right motor
raj1995 0:2df76cd30860 43 int oldEnc2 = 0; //Was the encoder previously a 1 or zero?
raj1995 0:2df76cd30860 44 int ticks2 = 0;
raj1995 0:2df76cd30860 45
raj1995 0:2df76cd30860 46
raj1995 0:2df76cd30860 47 //Servo
raj1995 0:2df76cd30860 48 Servo servo(p25);
raj1995 0:2df76cd30860 49
raj1995 0:2df76cd30860 50 //Prototypes (headers)
raj1995 0:2df76cd30860 51 void irSetup();
raj1995 0:2df76cd30860 52 void colorSensorSetup();
raj1995 0:2df76cd30860 53 int readColor();
raj1995 0:2df76cd30860 54 int readIR();
raj1995 0:2df76cd30860 55 void solOn();
raj1995 0:2df76cd30860 56 void solOff();
raj1995 0:2df76cd30860 57 void motorA(int);
raj1995 0:2df76cd30860 58 void motorB(int);
raj1995 0:2df76cd30860 59 void encCount(int);
raj1995 0:2df76cd30860 60 void encCount2(int);
raj1995 0:2df76cd30860 61 void moveBottom(int);
raj1995 0:2df76cd30860 62 void moveTop(int);
raj1995 0:2df76cd30860 63 void pulseBack(int);
raj1995 0:2df76cd30860 64
raj1995 0:2df76cd30860 65 int redpos = 0;
raj1995 0:2df76cd30860 66 int greenpos = 0;
raj1995 0:2df76cd30860 67 int yellowpos = 0;
raj1995 0:2df76cd30860 68 int purplepos = 0;
raj1995 0:2df76cd30860 69 int randompos = 0;
raj1995 0:2df76cd30860 70
raj1995 0:2df76cd30860 71 int turnAmount = 75;
raj1995 0:2df76cd30860 72
raj1995 0:2df76cd30860 73 int greenList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
raj1995 0:2df76cd30860 74 int redList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
raj1995 0:2df76cd30860 75 int yellowList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
raj1995 0:2df76cd30860 76 int purpleList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
raj1995 0:2df76cd30860 77 int garbageList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
raj1995 0:2df76cd30860 78
raj1995 0:2df76cd30860 79 int currentColor = 4;
raj1995 0:2df76cd30860 80 int counter = 0;
raj1995 0:2df76cd30860 81
raj1995 0:2df76cd30860 82 int main() {
raj1995 0:2df76cd30860 83
raj1995 0:2df76cd30860 84
raj1995 0:2df76cd30860 85
raj1995 0:2df76cd30860 86 //Color Sensor setup
raj1995 0:2df76cd30860 87 colorSensorSetup();
raj1995 0:2df76cd30860 88
raj1995 0:2df76cd30860 89 //Bottom Encoder setup
raj1995 0:2df76cd30860 90 Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
raj1995 0:2df76cd30860 91 //Top Encoder setup
raj1995 0:2df76cd30860 92 Enc2.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
raj1995 0:2df76cd30860 93 servo = 0.05;
raj1995 0:2df76cd30860 94 bool succ = true;
raj1995 0:2df76cd30860 95 double servoClosed = -0.05;
raj1995 0:2df76cd30860 96 while(1) {
raj1995 0:2df76cd30860 97 //pc.printf("%d\r\n",ticks);
raj1995 0:2df76cd30860 98 /*
raj1995 0:2df76cd30860 99 readColor();
raj1995 0:2df76cd30860 100 wait(1);
raj1995 0:2df76cd30860 101
raj1995 0:2df76cd30860 102
raj1995 0:2df76cd30860 103
raj1995 0:2df76cd30860 104 motorA(1);
raj1995 0:2df76cd30860 105 wait(1);
raj1995 0:2df76cd30860 106 motorA(0);
raj1995 0:2df76cd30860 107 wait(1);
raj1995 0:2df76cd30860 108
raj1995 0:2df76cd30860 109
raj1995 0:2df76cd30860 110 motorB(-1);
raj1995 0:2df76cd30860 111 wait(1);
raj1995 0:2df76cd30860 112 motorB(0);
raj1995 0:2df76cd30860 113 wait(1);
raj1995 0:2df76cd30860 114 motorB(1);
raj1995 0:2df76cd30860 115 wait(1);
raj1995 0:2df76cd30860 116 motorB(0);
raj1995 0:2df76cd30860 117 wait(1);
raj1995 0:2df76cd30860 118 */
raj1995 0:2df76cd30860 119
raj1995 0:2df76cd30860 120 /*
raj1995 0:2df76cd30860 121 motorA(1);
raj1995 0:2df76cd30860 122 if (readIR == 1){
raj1995 0:2df76cd30860 123 motorA(0);
raj1995 0:2df76cd30860 124 solOn();
raj1995 0:2df76cd30860 125 wait(1);
raj1995 0:2df76cd30860 126 int color = readColor();
raj1995 0:2df76cd30860 127
raj1995 0:2df76cd30860 128 if (color == 0){
raj1995 0:2df76cd30860 129
raj1995 0:2df76cd30860 130 }else if (color == 1){
raj1995 0:2df76cd30860 131
raj1995 0:2df76cd30860 132 }else if (color == 2){
raj1995 0:2df76cd30860 133
raj1995 0:2df76cd30860 134 }else if (color == 3){
raj1995 0:2df76cd30860 135
raj1995 0:2df76cd30860 136 }else{
raj1995 0:2df76cd30860 137
raj1995 0:2df76cd30860 138
raj1995 0:2df76cd30860 139 }
raj1995 0:2df76cd30860 140
raj1995 0:2df76cd30860 141 solOff();
raj1995 0:2df76cd30860 142 wait(0.5);
raj1995 0:2df76cd30860 143 }
raj1995 0:2df76cd30860 144
raj1995 0:2df76cd30860 145 */
raj1995 0:2df76cd30860 146
raj1995 0:2df76cd30860 147 /*
raj1995 0:2df76cd30860 148 motorB(1);
raj1995 0:2df76cd30860 149 encCount(1);
raj1995 0:2df76cd30860 150 if (ticks==1000){
raj1995 0:2df76cd30860 151 motorB(0);
raj1995 0:2df76cd30860 152 break;
raj1995 0:2df76cd30860 153 }
raj1995 0:2df76cd30860 154 motorA(1);
raj1995 0:2df76cd30860 155 */
raj1995 0:2df76cd30860 156
raj1995 0:2df76cd30860 157 //succ = false;
raj1995 0:2df76cd30860 158 if (succ){
raj1995 0:2df76cd30860 159 moveTop(94);
raj1995 0:2df76cd30860 160 succ = false;
raj1995 0:2df76cd30860 161 wait(0.5);
raj1995 0:2df76cd30860 162 }
raj1995 0:2df76cd30860 163 encCount2(1);
raj1995 0:2df76cd30860 164 //pc.printf("%d\r\n",ticks2);
raj1995 0:2df76cd30860 165 wait(0.8);
raj1995 0:2df76cd30860 166 int nextColor = readColor();
raj1995 0:2df76cd30860 167 //pc.printf("%d,%d\n\r",currentColor,nextColor);
raj1995 0:2df76cd30860 168 switch (currentColor){
raj1995 0:2df76cd30860 169
raj1995 0:2df76cd30860 170 case 0:
raj1995 0:2df76cd30860 171
raj1995 0:2df76cd30860 172 switch (nextColor){
raj1995 0:2df76cd30860 173
raj1995 0:2df76cd30860 174 case 0:
raj1995 0:2df76cd30860 175
raj1995 0:2df76cd30860 176 moveBottom(greenList[0]);
raj1995 0:2df76cd30860 177 servo = 0.4;
raj1995 0:2df76cd30860 178 wait(1);
raj1995 0:2df76cd30860 179 servo = servoClosed;
raj1995 0:2df76cd30860 180
raj1995 0:2df76cd30860 181 break;
raj1995 0:2df76cd30860 182 case 1:
raj1995 0:2df76cd30860 183 moveBottom(greenList[1]);
raj1995 0:2df76cd30860 184 servo = 0.4;
raj1995 0:2df76cd30860 185 wait(1);
raj1995 0:2df76cd30860 186 servo = servoClosed;
raj1995 0:2df76cd30860 187 counter++;
raj1995 0:2df76cd30860 188 break;
raj1995 0:2df76cd30860 189 case 2:
raj1995 0:2df76cd30860 190 moveBottom(greenList[2]);
raj1995 0:2df76cd30860 191 servo = 0.4;
raj1995 0:2df76cd30860 192 wait(1);
raj1995 0:2df76cd30860 193 servo = servoClosed;
raj1995 0:2df76cd30860 194 counter++;
raj1995 0:2df76cd30860 195 break;
raj1995 0:2df76cd30860 196 case 3:
raj1995 0:2df76cd30860 197 moveBottom(greenList[3]);
raj1995 0:2df76cd30860 198 servo = 0.4;
raj1995 0:2df76cd30860 199 wait(1);
raj1995 0:2df76cd30860 200 servo = servoClosed;
raj1995 0:2df76cd30860 201 counter++;
raj1995 0:2df76cd30860 202 break;
raj1995 0:2df76cd30860 203 case 4:
raj1995 0:2df76cd30860 204 moveBottom(greenList[4]);
raj1995 0:2df76cd30860 205 servo = servoClosed;
raj1995 0:2df76cd30860 206 counter++;
raj1995 0:2df76cd30860 207 break;
raj1995 0:2df76cd30860 208 }
raj1995 0:2df76cd30860 209 break;
raj1995 0:2df76cd30860 210 case 1:
raj1995 0:2df76cd30860 211
raj1995 0:2df76cd30860 212 switch (nextColor){
raj1995 0:2df76cd30860 213
raj1995 0:2df76cd30860 214 case 0:
raj1995 0:2df76cd30860 215
raj1995 0:2df76cd30860 216 moveBottom(redList[4]);
raj1995 0:2df76cd30860 217 servo = 0.4;
raj1995 0:2df76cd30860 218 wait(1);
raj1995 0:2df76cd30860 219 servo = 0.05;
raj1995 0:2df76cd30860 220 counter++;
raj1995 0:2df76cd30860 221 break;
raj1995 0:2df76cd30860 222 case 1:
raj1995 0:2df76cd30860 223 moveBottom(redList[0]);
raj1995 0:2df76cd30860 224 servo = 0.4;
raj1995 0:2df76cd30860 225 wait(1);
raj1995 0:2df76cd30860 226 servo = servoClosed;
raj1995 0:2df76cd30860 227
raj1995 0:2df76cd30860 228 break;
raj1995 0:2df76cd30860 229 case 2:
raj1995 0:2df76cd30860 230 moveBottom(redList[1]);
raj1995 0:2df76cd30860 231 servo = 0.4;
raj1995 0:2df76cd30860 232 wait(1);
raj1995 0:2df76cd30860 233 servo = servoClosed;
raj1995 0:2df76cd30860 234 counter++;
raj1995 0:2df76cd30860 235 break;
raj1995 0:2df76cd30860 236 case 3:
raj1995 0:2df76cd30860 237 moveBottom(redList[2]);
raj1995 0:2df76cd30860 238 servo = 0.4;
raj1995 0:2df76cd30860 239 wait(1);
raj1995 0:2df76cd30860 240 servo = servoClosed;
raj1995 0:2df76cd30860 241 counter++;
raj1995 0:2df76cd30860 242 break;
raj1995 0:2df76cd30860 243 case 4:
raj1995 0:2df76cd30860 244 moveBottom(redList[3]);
raj1995 0:2df76cd30860 245 servo = servoClosed;
raj1995 0:2df76cd30860 246 counter++;
raj1995 0:2df76cd30860 247 break;
raj1995 0:2df76cd30860 248 }
raj1995 0:2df76cd30860 249 break;
raj1995 0:2df76cd30860 250 case 2:
raj1995 0:2df76cd30860 251
raj1995 0:2df76cd30860 252 switch (nextColor){
raj1995 0:2df76cd30860 253
raj1995 0:2df76cd30860 254 case 0:
raj1995 0:2df76cd30860 255
raj1995 0:2df76cd30860 256 moveBottom(yellowList[3]);
raj1995 0:2df76cd30860 257 servo = 0.4;
raj1995 0:2df76cd30860 258 wait(1);
raj1995 0:2df76cd30860 259 servo = servoClosed;
raj1995 0:2df76cd30860 260 counter++;
raj1995 0:2df76cd30860 261 break;
raj1995 0:2df76cd30860 262 case 1:
raj1995 0:2df76cd30860 263 moveBottom(yellowList[4]);
raj1995 0:2df76cd30860 264 servo = 0.4;
raj1995 0:2df76cd30860 265 wait(1);
raj1995 0:2df76cd30860 266 servo = servoClosed;
raj1995 0:2df76cd30860 267 counter++;
raj1995 0:2df76cd30860 268 break;
raj1995 0:2df76cd30860 269 case 2:
raj1995 0:2df76cd30860 270 moveBottom(yellowList[0]);
raj1995 0:2df76cd30860 271 servo = 0.4;
raj1995 0:2df76cd30860 272 wait(1);
raj1995 0:2df76cd30860 273 servo = servoClosed;
raj1995 0:2df76cd30860 274
raj1995 0:2df76cd30860 275 break;
raj1995 0:2df76cd30860 276 case 3:
raj1995 0:2df76cd30860 277 moveBottom(yellowList[1]);
raj1995 0:2df76cd30860 278 servo = 0.4;
raj1995 0:2df76cd30860 279 wait(1);
raj1995 0:2df76cd30860 280 servo = servoClosed;
raj1995 0:2df76cd30860 281 counter++;
raj1995 0:2df76cd30860 282 break;
raj1995 0:2df76cd30860 283 case 4:
raj1995 0:2df76cd30860 284 moveBottom(yellowList[2]);
raj1995 0:2df76cd30860 285 servo = servoClosed;
raj1995 0:2df76cd30860 286 counter++;
raj1995 0:2df76cd30860 287 break;
raj1995 0:2df76cd30860 288 }
raj1995 0:2df76cd30860 289 break;
raj1995 0:2df76cd30860 290 case 3:
raj1995 0:2df76cd30860 291
raj1995 0:2df76cd30860 292 switch (nextColor){
raj1995 0:2df76cd30860 293
raj1995 0:2df76cd30860 294 case 0:
raj1995 0:2df76cd30860 295
raj1995 0:2df76cd30860 296 moveBottom(purpleList[2]);
raj1995 0:2df76cd30860 297 servo = 0.4;
raj1995 0:2df76cd30860 298 wait(1);
raj1995 0:2df76cd30860 299 servo = servoClosed;
raj1995 0:2df76cd30860 300 counter++;
raj1995 0:2df76cd30860 301 break;
raj1995 0:2df76cd30860 302 case 1:
raj1995 0:2df76cd30860 303 moveBottom(purpleList[3]);
raj1995 0:2df76cd30860 304 servo = 0.4;
raj1995 0:2df76cd30860 305 wait(1);
raj1995 0:2df76cd30860 306 servo = servoClosed;
raj1995 0:2df76cd30860 307 counter++;
raj1995 0:2df76cd30860 308 break;
raj1995 0:2df76cd30860 309 case 2:
raj1995 0:2df76cd30860 310 moveBottom(purpleList[4]);
raj1995 0:2df76cd30860 311 servo = 0.4;
raj1995 0:2df76cd30860 312 wait(1);
raj1995 0:2df76cd30860 313 servo = servoClosed;
raj1995 0:2df76cd30860 314 counter++;
raj1995 0:2df76cd30860 315 break;
raj1995 0:2df76cd30860 316 case 3:
raj1995 0:2df76cd30860 317 moveBottom(purpleList[0]);
raj1995 0:2df76cd30860 318 servo = 0.4;
raj1995 0:2df76cd30860 319 wait(1);
raj1995 0:2df76cd30860 320 servo = servoClosed;
raj1995 0:2df76cd30860 321
raj1995 0:2df76cd30860 322 break;
raj1995 0:2df76cd30860 323 case 4:
raj1995 0:2df76cd30860 324 moveBottom(purpleList[1]);
raj1995 0:2df76cd30860 325 servo = servoClosed;
raj1995 0:2df76cd30860 326 counter++;
raj1995 0:2df76cd30860 327 break;
raj1995 0:2df76cd30860 328 }
raj1995 0:2df76cd30860 329 break;
raj1995 0:2df76cd30860 330 case 4:
raj1995 0:2df76cd30860 331
raj1995 0:2df76cd30860 332 switch (nextColor){
raj1995 0:2df76cd30860 333
raj1995 0:2df76cd30860 334 case 0:
raj1995 0:2df76cd30860 335 ticksPrime = ticks;
raj1995 0:2df76cd30860 336 dist2move = nextColor*75;
raj1995 0:2df76cd30860 337 moveBottom(garbageList[1]);
raj1995 0:2df76cd30860 338 servo = 0.4;
raj1995 0:2df76cd30860 339 wait(1);
raj1995 0:2df76cd30860 340 servo = servoClosed;
raj1995 0:2df76cd30860 341 if(ticks <= ticksPrime + dist2move){
raj1995 0:2df76cd30860 342 succ = true;
raj1995 0:2df76cd30860 343 }
raj1995 0:2df76cd30860 344 counter++;
raj1995 0:2df76cd30860 345 break;
raj1995 0:2df76cd30860 346 case 1:
raj1995 0:2df76cd30860 347 ticksPrime = ticks;
raj1995 0:2df76cd30860 348 dist2move = nextColor*75;
raj1995 0:2df76cd30860 349 moveBottom(garbageList[2]);
raj1995 0:2df76cd30860 350 servo = 0.4;
raj1995 0:2df76cd30860 351 wait(1);
raj1995 0:2df76cd30860 352 servo = servoClosed;
raj1995 0:2df76cd30860 353 if(ticks <= ticksPrime + dist2move){
raj1995 0:2df76cd30860 354 succ = true;
raj1995 0:2df76cd30860 355 }
raj1995 0:2df76cd30860 356 counter++;
raj1995 0:2df76cd30860 357 break;
raj1995 0:2df76cd30860 358 case 2:
raj1995 0:2df76cd30860 359 ticksPrime = ticks;
raj1995 0:2df76cd30860 360 dist2move = nextColor*75;
raj1995 0:2df76cd30860 361 moveBottom(garbageList[3]);
raj1995 0:2df76cd30860 362 servo = 0.4;
raj1995 0:2df76cd30860 363 wait(1);
raj1995 0:2df76cd30860 364 servo = servoClosed;
raj1995 0:2df76cd30860 365 if(ticks <= ticksPrime + dist2move){
raj1995 0:2df76cd30860 366 succ = true;
raj1995 0:2df76cd30860 367 }
raj1995 0:2df76cd30860 368 counter++;
raj1995 0:2df76cd30860 369 break;
raj1995 0:2df76cd30860 370 case 3:
raj1995 0:2df76cd30860 371 ticksPrime = ticks;
raj1995 0:2df76cd30860 372 dist2move = nextColor*75;
raj1995 0:2df76cd30860 373 moveBottom(garbageList[4]);
raj1995 0:2df76cd30860 374 servo = 0.4;
raj1995 0:2df76cd30860 375 wait(1);
raj1995 0:2df76cd30860 376 servo = servoClosed;
raj1995 0:2df76cd30860 377 if(ticks <= ticksPrime + dist2move){
raj1995 0:2df76cd30860 378 succ = true;
raj1995 0:2df76cd30860 379 }
raj1995 0:2df76cd30860 380 counter++;
raj1995 0:2df76cd30860 381 break;
raj1995 0:2df76cd30860 382 case 4:
raj1995 0:2df76cd30860 383 ticksPrime = ticks;
raj1995 0:2df76cd30860 384 dist2move = nextColor*75;
raj1995 0:2df76cd30860 385 moveBottom(garbageList[0]);
raj1995 0:2df76cd30860 386 servo = servoClosed;
raj1995 0:2df76cd30860 387 if(ticks <= ticksPrime + dist2move){
raj1995 0:2df76cd30860 388 succ = true;
raj1995 0:2df76cd30860 389 }
raj1995 0:2df76cd30860 390 break;
raj1995 0:2df76cd30860 391 }
raj1995 0:2df76cd30860 392 break;
raj1995 0:2df76cd30860 393 }
raj1995 0:2df76cd30860 394 encCount(1);
raj1995 0:2df76cd30860 395 currentColor = nextColor;
raj1995 0:2df76cd30860 396 if (counter==4){
raj1995 0:2df76cd30860 397 pulseBack(16);
raj1995 0:2df76cd30860 398 counter = 0;
raj1995 0:2df76cd30860 399 pc.printf("%d\r\n",counter);
raj1995 0:2df76cd30860 400 }
raj1995 0:2df76cd30860 401
raj1995 0:2df76cd30860 402 }
raj1995 0:2df76cd30860 403 }
raj1995 0:2df76cd30860 404
raj1995 0:2df76cd30860 405 int readColor(){
raj1995 0:2df76cd30860 406
raj1995 0:2df76cd30860 407
raj1995 0:2df76cd30860 408 // Initialize color sensor
raj1995 0:2df76cd30860 409 char clear_reg[1] = {148};
raj1995 0:2df76cd30860 410 char clear_data[2] = {0,0};
raj1995 0:2df76cd30860 411 i2c.write(sensor_addr,clear_reg,1, true);
raj1995 0:2df76cd30860 412 i2c.read(sensor_addr,clear_data,2, false);
raj1995 0:2df76cd30860 413
raj1995 0:2df76cd30860 414 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
raj1995 0:2df76cd30860 415
raj1995 0:2df76cd30860 416 char red_reg[1] = {150};
raj1995 0:2df76cd30860 417 char red_data[2] = {0,0};
raj1995 0:2df76cd30860 418 i2c.write(sensor_addr,red_reg,1, true);
raj1995 0:2df76cd30860 419 i2c.read(sensor_addr,red_data,2, false);
raj1995 0:2df76cd30860 420
raj1995 0:2df76cd30860 421 int red_value = ((int)red_data[1] << 8) | red_data[0];
raj1995 0:2df76cd30860 422
raj1995 0:2df76cd30860 423 char green_reg[1] = {152};
raj1995 0:2df76cd30860 424 char green_data[2] = {0,0};
raj1995 0:2df76cd30860 425 i2c.write(sensor_addr,green_reg,1, true);
raj1995 0:2df76cd30860 426 i2c.read(sensor_addr,green_data,2, false);
raj1995 0:2df76cd30860 427
raj1995 0:2df76cd30860 428 int green_value = ((int)green_data[1] << 8) | green_data[0];
raj1995 0:2df76cd30860 429
raj1995 0:2df76cd30860 430 char blue_reg[1] = {154};
raj1995 0:2df76cd30860 431 char blue_data[2] = {0,0};
raj1995 0:2df76cd30860 432 i2c.write(sensor_addr,blue_reg,1, true);
raj1995 0:2df76cd30860 433 i2c.read(sensor_addr,blue_data,2, false);
raj1995 0:2df76cd30860 434
raj1995 0:2df76cd30860 435 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
raj1995 0:2df76cd30860 436
raj1995 0:2df76cd30860 437 // print sensor readings
raj1995 0:2df76cd30860 438
raj1995 0:2df76cd30860 439
raj1995 0:2df76cd30860 440 //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n\r", clear_value, red_value, green_value, blue_value);
raj1995 0:2df76cd30860 441 //pc.printf("Red (%d), Green (%d), Blue (%d)\n\r", red_value, green_value, blue_value);
raj1995 0:2df76cd30860 442
raj1995 0:2df76cd30860 443
raj1995 0:2df76cd30860 444 float red_in = (float)red_value;
raj1995 0:2df76cd30860 445 float green_in = (float)green_value;
raj1995 0:2df76cd30860 446 float blue_in = (float)blue_value;
raj1995 0:2df76cd30860 447
raj1995 0:2df76cd30860 448 float redSkittle[] = {4656.25,588.25,826};
raj1995 0:2df76cd30860 449 float greenSkittle[] = {2968.25,3898.5,1346.5};
raj1995 0:2df76cd30860 450 float yellowSkittle[] = {15387.75,9977.5,3173.75};
raj1995 0:2df76cd30860 451 //float purpleSkittle[] = {1088,342.25,340.5};
raj1995 0:2df76cd30860 452 float purpleSkittle[] = {1215,1773,2541};
raj1995 0:2df76cd30860 453 float closed[] = {1094, 1149, 987};
raj1995 0:2df76cd30860 454
raj1995 0:2df76cd30860 455 float distRed = sqrt(pow(red_in-redSkittle[0],2)+pow(green_in-redSkittle[1],2)+pow(blue_in-redSkittle[2],2));
raj1995 0:2df76cd30860 456 float distGreen = sqrt(pow(red_in-greenSkittle[0],2)+pow(green_in-greenSkittle[1],2)+pow(blue_in-greenSkittle[2],2));
raj1995 0:2df76cd30860 457 float distYellow = sqrt(pow(red_in-yellowSkittle[0],2)+pow(green_in-yellowSkittle[1],2)+pow(blue_in-yellowSkittle[2],2));
raj1995 0:2df76cd30860 458 float distPurple = sqrt(pow(red_in-purpleSkittle[0],2)+pow(green_in-purpleSkittle[1],2)+pow(blue_in-purpleSkittle[2],2));
raj1995 0:2df76cd30860 459 float distClosed = sqrt(pow(red_in-closed[0],2)+pow(green_in-closed[1],2)+pow(blue_in-closed[2],2));
raj1995 0:2df76cd30860 460
raj1995 0:2df76cd30860 461 float choices[] = {distRed, distGreen, distYellow, distPurple, distClosed};
raj1995 0:2df76cd30860 462
raj1995 0:2df76cd30860 463 sort(choices,choices+5);
raj1995 0:2df76cd30860 464
raj1995 0:2df76cd30860 465 float closest = choices[0];
raj1995 0:2df76cd30860 466
raj1995 0:2df76cd30860 467
raj1995 0:2df76cd30860 468 if (closest==distRed){
raj1995 0:2df76cd30860 469 //pc.printf("Red\n\r");
raj1995 0:2df76cd30860 470 return 1;
raj1995 0:2df76cd30860 471 }else if(closest==distGreen){
raj1995 0:2df76cd30860 472 //pc.printf("Green\n\r");
raj1995 0:2df76cd30860 473 return 0;
raj1995 0:2df76cd30860 474 }else if(closest==distYellow){
raj1995 0:2df76cd30860 475 //pc.printf("Yellow\n\r");
raj1995 0:2df76cd30860 476 return 2;
raj1995 0:2df76cd30860 477 }else if(closest==distPurple){
raj1995 0:2df76cd30860 478 //pc.printf("Purple\n\r");
raj1995 0:2df76cd30860 479 return 3;
raj1995 0:2df76cd30860 480 }else //pc.printf("Dave messed up.\n\r");
raj1995 0:2df76cd30860 481
raj1995 0:2df76cd30860 482 return 4;
raj1995 0:2df76cd30860 483
raj1995 0:2df76cd30860 484
raj1995 0:2df76cd30860 485 }
raj1995 0:2df76cd30860 486
raj1995 0:2df76cd30860 487 int readIR(){
raj1995 0:2df76cd30860 488
raj1995 0:2df76cd30860 489 if(pc.readable()) {
raj1995 0:2df76cd30860 490 device.putc(pc.getc());
raj1995 0:2df76cd30860 491 }
raj1995 0:2df76cd30860 492 //IR Receive code
raj1995 0:2df76cd30860 493 if(device.readable()) {
raj1995 0:2df76cd30860 494 pc.putc(device.getc());
raj1995 0:2df76cd30860 495 }
raj1995 0:2df76cd30860 496 return 1;
raj1995 0:2df76cd30860 497 }
raj1995 0:2df76cd30860 498
raj1995 0:2df76cd30860 499 void solOn(){
raj1995 0:2df76cd30860 500 Ctrl = 1; //ON
raj1995 0:2df76cd30860 501 }
raj1995 0:2df76cd30860 502
raj1995 0:2df76cd30860 503 void solOff(){
raj1995 0:2df76cd30860 504 Ctrl = 0; //ON
raj1995 0:2df76cd30860 505 }
raj1995 0:2df76cd30860 506
raj1995 0:2df76cd30860 507 void motorA(int state){
raj1995 0:2df76cd30860 508 fwdA = 1;
raj1995 0:2df76cd30860 509 revA = 0;
raj1995 0:2df76cd30860 510 speedA = (float)state*0.15;
raj1995 0:2df76cd30860 511 }
raj1995 0:2df76cd30860 512
raj1995 0:2df76cd30860 513 void motorB(int dir){
raj1995 0:2df76cd30860 514 if (dir > 0){
raj1995 0:2df76cd30860 515 fwdB = 1;
raj1995 0:2df76cd30860 516 revB = 0;
raj1995 0:2df76cd30860 517 }else{
raj1995 0:2df76cd30860 518 fwdB = 0;
raj1995 0:2df76cd30860 519 revB = 1;
raj1995 0:2df76cd30860 520 dir = -dir;
raj1995 0:2df76cd30860 521 }
raj1995 0:2df76cd30860 522 speedB = (float)dir*.3;
raj1995 0:2df76cd30860 523 }
raj1995 0:2df76cd30860 524 void pulseBack(int dir) {
raj1995 0:2df76cd30860 525 //int newticks = ticks - dir;
raj1995 0:2df76cd30860 526 motorB(1);
raj1995 0:2df76cd30860 527 /*
raj1995 0:2df76cd30860 528 while (ticks > newticks){
raj1995 0:2df76cd30860 529 encCount(-1);
raj1995 0:2df76cd30860 530 //pc.printf("%d,%d\n\r",newticks,ticks2);
raj1995 0:2df76cd30860 531 }
raj1995 0:2df76cd30860 532 */
raj1995 0:2df76cd30860 533 wait(0.1);
raj1995 0:2df76cd30860 534 motorB(0);
raj1995 0:2df76cd30860 535 return;
raj1995 0:2df76cd30860 536 }
raj1995 0:2df76cd30860 537 void irSetup(){
raj1995 0:2df76cd30860 538 //IR Transmit setup code
raj1995 0:2df76cd30860 539 IRLED.period(1.0/38000.0);
raj1995 0:2df76cd30860 540 IRLED = 0.5;
raj1995 0:2df76cd30860 541 //Drive IR LED data pin with 38Khz PWM Carrier
raj1995 0:2df76cd30860 542 //Drive IR LED gnd pin with serial TX
raj1995 0:2df76cd30860 543 device.baud(2400);
raj1995 0:2df76cd30860 544 }
raj1995 0:2df76cd30860 545
raj1995 0:2df76cd30860 546 void colorSensorSetup(){
raj1995 0:2df76cd30860 547 pc.baud(9600);
raj1995 0:2df76cd30860 548
raj1995 0:2df76cd30860 549 led = 1; // off
raj1995 0:2df76cd30860 550 // Connect to the Color sensor and verify whether we connected to the correct sensor.
raj1995 0:2df76cd30860 551
raj1995 0:2df76cd30860 552 i2c.frequency(200000);
raj1995 0:2df76cd30860 553
raj1995 0:2df76cd30860 554 char id_regval[1] = {146};
raj1995 0:2df76cd30860 555 char data[1] = {0};
raj1995 0:2df76cd30860 556 i2c.write(sensor_addr,id_regval,1, true);
raj1995 0:2df76cd30860 557 i2c.read(sensor_addr,data,1,false);
raj1995 0:2df76cd30860 558
raj1995 0:2df76cd30860 559 if (data[0]==68) {
raj1995 0:2df76cd30860 560 //green = 0;
raj1995 0:2df76cd30860 561 wait (2);
raj1995 0:2df76cd30860 562 //green = 1;
raj1995 0:2df76cd30860 563 } else {
raj1995 0:2df76cd30860 564 //green = 1;
raj1995 0:2df76cd30860 565 }
raj1995 0:2df76cd30860 566
raj1995 0:2df76cd30860 567 char timing_register[2] = {129,0};
raj1995 0:2df76cd30860 568 i2c.write(sensor_addr,timing_register,2,false);
raj1995 0:2df76cd30860 569
raj1995 0:2df76cd30860 570 char control_register[2] = {143,0};
raj1995 0:2df76cd30860 571 i2c.write(sensor_addr,control_register,2,false);
raj1995 0:2df76cd30860 572
raj1995 0:2df76cd30860 573 char enable_register[2] = {128,3};
raj1995 0:2df76cd30860 574 i2c.write(sensor_addr,enable_register,2,false);
raj1995 0:2df76cd30860 575
raj1995 0:2df76cd30860 576
raj1995 0:2df76cd30860 577 }
raj1995 0:2df76cd30860 578
raj1995 0:2df76cd30860 579 void encCount(int dir){
raj1995 0:2df76cd30860 580 if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched.
raj1995 0:2df76cd30860 581 if (dir>0) ticks++;
raj1995 0:2df76cd30860 582 else ticks--;
raj1995 0:2df76cd30860 583 oldEnc1 = Enc1;
raj1995 0:2df76cd30860 584 //pc.printf("%d\r\n",ticks);
raj1995 0:2df76cd30860 585
raj1995 0:2df76cd30860 586 }
raj1995 0:2df76cd30860 587
raj1995 0:2df76cd30860 588 }
raj1995 0:2df76cd30860 589
raj1995 0:2df76cd30860 590
raj1995 0:2df76cd30860 591 void encCount2(int dir){
raj1995 0:2df76cd30860 592 if(Enc2 != oldEnc2) { // Increment ticks every time the state has switched.
raj1995 0:2df76cd30860 593 if (dir>0) ticks2++;
raj1995 0:2df76cd30860 594 else ticks2--;
raj1995 0:2df76cd30860 595 oldEnc2 = Enc2;
raj1995 0:2df76cd30860 596 //pc.printf("%d\r\n",ticks2);
raj1995 0:2df76cd30860 597
raj1995 0:2df76cd30860 598 }
raj1995 0:2df76cd30860 599
raj1995 0:2df76cd30860 600 }
raj1995 0:2df76cd30860 601
raj1995 0:2df76cd30860 602 void moveBottom(int amt){
raj1995 0:2df76cd30860 603 int newticks = ticks + amt;
raj1995 0:2df76cd30860 604 motorB(-1);
raj1995 0:2df76cd30860 605 while (ticks < newticks-5){
raj1995 0:2df76cd30860 606 encCount(1);
raj1995 0:2df76cd30860 607 //pc.printf("%d,%d\n\r",newticks,ticks);
raj1995 0:2df76cd30860 608 }
raj1995 0:2df76cd30860 609 motorB(0);
raj1995 0:2df76cd30860 610 return;
raj1995 0:2df76cd30860 611 }
raj1995 0:2df76cd30860 612 void moveTop(int amt){
raj1995 0:2df76cd30860 613 int newticks = ticks2 + amt;
raj1995 0:2df76cd30860 614 motorA(1);
raj1995 0:2df76cd30860 615 while (ticks2 < newticks-5){
raj1995 0:2df76cd30860 616 encCount2(1);
raj1995 0:2df76cd30860 617 //pc.printf("%d,%d\n\r",newticks,ticks2);
raj1995 0:2df76cd30860 618 }
raj1995 0:2df76cd30860 619 motorA(0);
raj1995 0:2df76cd30860 620 return;
raj1995 0:2df76cd30860 621 }