main.cpp

00001 #include <math.h>
00002 #include <stdio.h>
00003 #include "mbed.h"
00004 #define commonAnode true
00005 #include <algorithm>
00006 #include "Servo.h"
00007 
00008 
00009 //Color Sensor
00010 I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
00011 int sensor_addr = 41 << 1;
00012 DigitalOut led(p11);
00013 
00014 //IR
00015 Serial pc(USBTX, USBRX);
00016 Serial device(p13, p14);  // tx, rx
00017 PwmOut IRLED(p21);
00018 
00019 //Solenoid
00020 DigitalOut Ctrl(p8); //Solenoid output control bit
00021 
00022 
00023 //Motor A
00024 PwmOut speedA(p23);        // PWM Speed pin for Motor A
00025 DigitalOut fwdA(p17);      // Digital out signal for forward direction for Motor A
00026 DigitalOut revA(p18);      // Digital out signal for reverse direction for Motor A
00027 
00028 //MotorB
00029 PwmOut speedB(p24);        // PWM Speed pin for Motor B
00030 DigitalOut fwdB(p20);      // Digital out signal for forward direction for Motor B
00031 DigitalOut revB(p19);      // Digital out signal for reverse direction for Motor B
00032 
00033 
00034 //Bottom Encoder
00035 DigitalIn Enc1(p16); //right motor
00036 int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
00037 int ticks = 0;
00038 int ticksPrime = 0;
00039 int dist2move = 0;
00040 
00041 //Top Encoder
00042 DigitalIn Enc2(p15); //right motor
00043 int oldEnc2 = 0; //Was the encoder previously a 1 or zero?
00044 int ticks2 = 0;
00045 
00046 
00047 //Servo
00048 Servo servo(p25);
00049 
00050 //Prototypes (headers)
00051 void irSetup();
00052 void colorSensorSetup();
00053 int readColor();
00054 int readIR();
00055 void solOn();
00056 void solOff();
00057 void motorA(int);
00058 void motorB(int);
00059 void encCount(int);
00060 void encCount2(int);
00061 void moveBottom(int);
00062 void moveTop(int);
00063 void pulseBack(int);
00064 
00065 int redpos = 0;
00066 int greenpos = 0;
00067 int yellowpos = 0;
00068 int purplepos = 0;
00069 int randompos = 0;
00070 
00071 int turnAmount = 75;
00072 
00073 int greenList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
00074 int redList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
00075 int yellowList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
00076 int purpleList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
00077 int garbageList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
00078 
00079 int currentColor = 4;
00080 int counter = 0;
00081 
00082 int main() {
00083     
00084 
00085     
00086     //Color Sensor setup
00087     colorSensorSetup();
00088     
00089     //Bottom Encoder setup
00090     Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
00091     //Top Encoder setup
00092     Enc2.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
00093     servo = 0.05;
00094     bool succ = true;
00095     double servoClosed = -0.05;
00096     while(1) {
00097         //pc.printf("%d\r\n",ticks);
00098         /*
00099         readColor();
00100         wait(1);
00101         
00102                 
00103         
00104         motorA(1);
00105         wait(1);
00106         motorA(0);
00107         wait(1);
00108         
00109         
00110         motorB(-1);
00111         wait(1);
00112         motorB(0);
00113         wait(1);
00114         motorB(1);
00115         wait(1);
00116         motorB(0);
00117         wait(1);
00118         */
00119         
00120         /*
00121         motorA(1);
00122         if (readIR == 1){
00123             motorA(0);
00124             solOn();
00125             wait(1);
00126             int color = readColor();
00127             
00128             if (color == 0){
00129                 
00130             }else if (color == 1){
00131                 
00132             }else if (color == 2){
00133                 
00134             }else if (color == 3){
00135             
00136             }else{
00137                 
00138                 
00139             }
00140             
00141             solOff();
00142             wait(0.5);
00143         }
00144         
00145         */
00146         
00147         /*
00148         motorB(1);
00149         encCount(1);
00150         if (ticks==1000){
00151             motorB(0);
00152             break;
00153         }
00154         motorA(1);
00155         */
00156         
00157       //succ = false;
00158         if (succ){
00159             moveTop(94);
00160             succ = false;
00161             wait(0.5);
00162         }
00163         encCount2(1);
00164         //pc.printf("%d\r\n",ticks2);
00165         wait(0.8);
00166         int nextColor = readColor();
00167         //pc.printf("%d,%d\n\r",currentColor,nextColor);
00168         switch (currentColor){
00169             
00170         case 0:
00171          
00172             switch (nextColor){
00173             
00174             case 0:
00175                 
00176                 moveBottom(greenList[0]);
00177                 servo = 0.4;
00178                 wait(1);
00179                 servo = servoClosed;
00180                 
00181                 break;
00182             case 1:
00183                 moveBottom(greenList[1]);
00184                 servo = 0.4;
00185                 wait(1);
00186                 servo = servoClosed;
00187                 counter++;
00188                 break;
00189             case 2:
00190                 moveBottom(greenList[2]);
00191                 servo = 0.4;
00192                 wait(1);
00193                 servo = servoClosed;
00194                 counter++;
00195                 break;
00196             case 3:
00197                 moveBottom(greenList[3]);
00198                 servo = 0.4;
00199                 wait(1);
00200                 servo = servoClosed;
00201                 counter++;
00202                 break;
00203             case 4:
00204                 moveBottom(greenList[4]);
00205                 servo = servoClosed;
00206                 counter++;
00207                 break;
00208             }
00209             break;
00210         case 1:
00211          
00212             switch (nextColor){
00213             
00214             case 0:
00215                 
00216                 moveBottom(redList[4]);
00217                 servo = 0.4;
00218                 wait(1);
00219                 servo = 0.05;
00220                 counter++;
00221                 break;
00222             case 1:
00223                 moveBottom(redList[0]);
00224                 servo = 0.4;
00225                 wait(1);
00226                 servo = servoClosed;
00227                 
00228                 break;
00229             case 2:
00230                 moveBottom(redList[1]);
00231                 servo = 0.4;
00232                 wait(1);
00233                 servo = servoClosed;
00234                 counter++;
00235                 break;
00236             case 3:
00237                 moveBottom(redList[2]);
00238                 servo = 0.4;
00239                 wait(1);
00240                 servo = servoClosed;
00241                 counter++;
00242                 break;
00243             case 4:
00244                 moveBottom(redList[3]);
00245                 servo = servoClosed;
00246                 counter++;
00247                 break;
00248             }
00249             break;
00250         case 2:
00251          
00252             switch (nextColor){
00253             
00254             case 0:
00255                 
00256                 moveBottom(yellowList[3]);
00257                 servo = 0.4;
00258                 wait(1);
00259                 servo = servoClosed;
00260                 counter++;
00261                 break;
00262             case 1:
00263                 moveBottom(yellowList[4]);
00264                 servo = 0.4;
00265                 wait(1);
00266                 servo = servoClosed;
00267                 counter++;
00268                 break;
00269             case 2:
00270                 moveBottom(yellowList[0]);
00271                 servo = 0.4;
00272                 wait(1);
00273                 servo = servoClosed;
00274                 
00275                 break;
00276             case 3:
00277                 moveBottom(yellowList[1]);
00278                 servo = 0.4;
00279                 wait(1);
00280                 servo = servoClosed;
00281                 counter++;
00282                 break;
00283             case 4:
00284                 moveBottom(yellowList[2]);
00285                 servo = servoClosed;
00286                 counter++;
00287                 break;
00288             }
00289             break;
00290         case 3:
00291          
00292             switch (nextColor){
00293             
00294             case 0:
00295                 
00296                 moveBottom(purpleList[2]);
00297                 servo = 0.4;
00298                 wait(1);
00299                 servo = servoClosed;
00300                 counter++;
00301                 break;
00302             case 1:
00303                 moveBottom(purpleList[3]);
00304                 servo = 0.4;
00305                 wait(1);
00306                 servo = servoClosed;
00307                 counter++;
00308                 break;
00309             case 2:
00310                 moveBottom(purpleList[4]);
00311                 servo = 0.4;
00312                 wait(1);
00313                 servo = servoClosed;
00314                 counter++;
00315                 break;
00316             case 3:
00317                 moveBottom(purpleList[0]);
00318                 servo = 0.4;
00319                 wait(1);
00320                 servo = servoClosed;
00321                 
00322                 break;
00323             case 4:
00324                 moveBottom(purpleList[1]);
00325                 servo = servoClosed;
00326                 counter++;
00327                 break;
00328             }
00329             break;    
00330         case 4:
00331          
00332             switch (nextColor){
00333             
00334             case 0:
00335                 ticksPrime = ticks;
00336                 dist2move = nextColor*75;
00337                 moveBottom(garbageList[1]);
00338                 servo = 0.4;
00339                 wait(1);
00340                 servo = servoClosed;
00341                 if(ticks <= ticksPrime + dist2move){
00342                     succ = true;
00343                 }
00344                 counter++;
00345                 break;
00346             case 1:
00347                 ticksPrime = ticks;
00348                 dist2move = nextColor*75;
00349                 moveBottom(garbageList[2]);
00350                 servo = 0.4;
00351                 wait(1);
00352                 servo = servoClosed;
00353                 if(ticks <= ticksPrime + dist2move){
00354                     succ = true;
00355                 }
00356                 counter++;
00357                 break;
00358             case 2:
00359                 ticksPrime = ticks;
00360                 dist2move = nextColor*75;
00361                 moveBottom(garbageList[3]);
00362                 servo = 0.4;
00363                 wait(1);
00364                 servo = servoClosed;
00365                 if(ticks <= ticksPrime + dist2move){
00366                     succ = true;
00367                 }
00368                 counter++;
00369                 break;
00370             case 3:
00371                 ticksPrime = ticks;
00372                 dist2move = nextColor*75;
00373                 moveBottom(garbageList[4]);
00374                 servo = 0.4;
00375                 wait(1);
00376                 servo = servoClosed;
00377                 if(ticks <= ticksPrime + dist2move){
00378                     succ = true;
00379                 }
00380                 counter++;
00381                 break;
00382             case 4:
00383                 ticksPrime = ticks;
00384                 dist2move = nextColor*75;
00385                 moveBottom(garbageList[0]);
00386                 servo = servoClosed;
00387                 if(ticks <= ticksPrime + dist2move){
00388                     succ = true;
00389                 }
00390                 break;
00391             }
00392             break;    
00393         }
00394         encCount(1);
00395         currentColor = nextColor;
00396         if (counter==4){
00397             pulseBack(16);
00398             counter = 0;  
00399             pc.printf("%d\r\n",counter);  
00400         }        
00401         
00402     }
00403 }
00404 
00405 int readColor(){
00406 
00407     
00408     // Initialize color sensor
00409         char clear_reg[1] = {148};
00410         char clear_data[2] = {0,0};
00411         i2c.write(sensor_addr,clear_reg,1, true);
00412         i2c.read(sensor_addr,clear_data,2, false);
00413         
00414         int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
00415         
00416         char red_reg[1] = {150};
00417         char red_data[2] = {0,0};
00418         i2c.write(sensor_addr,red_reg,1, true);
00419         i2c.read(sensor_addr,red_data,2, false);
00420         
00421         int red_value = ((int)red_data[1] << 8) | red_data[0];
00422         
00423         char green_reg[1] = {152};
00424         char green_data[2] = {0,0};
00425         i2c.write(sensor_addr,green_reg,1, true);
00426         i2c.read(sensor_addr,green_data,2, false);
00427         
00428         int green_value = ((int)green_data[1] << 8) | green_data[0];
00429         
00430         char blue_reg[1] = {154};
00431         char blue_data[2] = {0,0};
00432         i2c.write(sensor_addr,blue_reg,1, true);
00433         i2c.read(sensor_addr,blue_data,2, false);
00434         
00435         int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
00436         
00437         // print sensor readings
00438         
00439         
00440         //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n\r", clear_value, red_value, green_value, blue_value);
00441         //pc.printf("Red (%d), Green (%d), Blue (%d)\n\r", red_value, green_value, blue_value);
00442         
00443         
00444         float red_in = (float)red_value;
00445         float green_in = (float)green_value;
00446         float blue_in = (float)blue_value;
00447         
00448         float redSkittle[] = {4656.25,588.25,826};
00449         float greenSkittle[] = {2968.25,3898.5,1346.5};
00450         float yellowSkittle[] = {15387.75,9977.5,3173.75};
00451         //float purpleSkittle[] = {1088,342.25,340.5};
00452         float purpleSkittle[] = {1215,1773,2541};
00453         float closed[] = {1094, 1149, 987};
00454         
00455         float distRed = sqrt(pow(red_in-redSkittle[0],2)+pow(green_in-redSkittle[1],2)+pow(blue_in-redSkittle[2],2));
00456         float distGreen = sqrt(pow(red_in-greenSkittle[0],2)+pow(green_in-greenSkittle[1],2)+pow(blue_in-greenSkittle[2],2));
00457         float distYellow = sqrt(pow(red_in-yellowSkittle[0],2)+pow(green_in-yellowSkittle[1],2)+pow(blue_in-yellowSkittle[2],2));
00458         float distPurple = sqrt(pow(red_in-purpleSkittle[0],2)+pow(green_in-purpleSkittle[1],2)+pow(blue_in-purpleSkittle[2],2));
00459         float distClosed = sqrt(pow(red_in-closed[0],2)+pow(green_in-closed[1],2)+pow(blue_in-closed[2],2));
00460         
00461         float choices[] = {distRed, distGreen, distYellow, distPurple, distClosed};
00462         
00463         sort(choices,choices+5);
00464         
00465         float closest = choices[0];
00466         
00467 
00468         if (closest==distRed){
00469             //pc.printf("Red\n\r");
00470             return 1;
00471         }else if(closest==distGreen){
00472             //pc.printf("Green\n\r");
00473             return 0;
00474         }else if(closest==distYellow){
00475             //pc.printf("Yellow\n\r");
00476             return 2;
00477         }else if(closest==distPurple){
00478             //pc.printf("Purple\n\r");
00479             return 3;
00480         }else //pc.printf("Dave messed up.\n\r");
00481         
00482         return 4;
00483         
00484         
00485 }
00486 
00487 int readIR(){
00488     
00489     if(pc.readable()) {
00490         device.putc(pc.getc());
00491     }
00492     //IR Receive code
00493     if(device.readable()) {
00494         pc.putc(device.getc());
00495     }
00496     return 1;
00497 }
00498 
00499 void solOn(){
00500     Ctrl = 1; //ON   
00501 }
00502 
00503 void solOff(){
00504     Ctrl = 0; //ON   
00505 }
00506 
00507 void motorA(int state){
00508     fwdA = 1;
00509     revA = 0;
00510     speedA = (float)state*0.15;
00511 }
00512 
00513 void motorB(int dir){
00514     if (dir > 0){
00515         fwdB = 1;
00516         revB = 0;
00517     }else{
00518         fwdB = 0;
00519         revB = 1;
00520         dir = -dir;
00521     }
00522     speedB = (float)dir*.3;   
00523 }
00524 void pulseBack(int dir) {
00525     //int newticks = ticks - dir;
00526     motorB(1);
00527     /*
00528     while (ticks > newticks){
00529         encCount(-1);
00530         //pc.printf("%d,%d\n\r",newticks,ticks2);
00531     }   
00532     */
00533     wait(0.1);
00534     motorB(0);
00535     return;    
00536 }
00537 void irSetup(){
00538     //IR Transmit setup code
00539     IRLED.period(1.0/38000.0);
00540     IRLED = 0.5;
00541     //Drive IR LED data pin with 38Khz PWM Carrier
00542     //Drive IR LED gnd pin with serial TX
00543     device.baud(2400);    
00544 }
00545 
00546 void colorSensorSetup(){
00547     pc.baud(9600);
00548     
00549     led = 1; // off
00550     // Connect to the Color sensor and verify whether we connected to the correct sensor. 
00551     
00552     i2c.frequency(200000);
00553     
00554     char id_regval[1] = {146};
00555     char data[1] = {0};
00556     i2c.write(sensor_addr,id_regval,1, true);
00557     i2c.read(sensor_addr,data,1,false);
00558     
00559     if (data[0]==68) {
00560         //green = 0;
00561         wait (2); 
00562         //green = 1;
00563         } else {
00564         //green = 1; 
00565     }
00566     
00567     char timing_register[2] = {129,0};
00568     i2c.write(sensor_addr,timing_register,2,false);
00569     
00570     char control_register[2] = {143,0};
00571     i2c.write(sensor_addr,control_register,2,false);
00572     
00573     char enable_register[2] = {128,3};
00574     i2c.write(sensor_addr,enable_register,2,false);
00575     
00576 
00577 }
00578 
00579 void encCount(int dir){
00580     if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched. 
00581         if (dir>0) ticks++;
00582         else ticks--;
00583         oldEnc1 = Enc1;
00584         //pc.printf("%d\r\n",ticks);
00585 
00586     }
00587     
00588 }
00589 
00590 
00591 void encCount2(int dir){
00592     if(Enc2 != oldEnc2) { // Increment ticks every time the state has switched. 
00593         if (dir>0) ticks2++;
00594         else ticks2--;
00595         oldEnc2 = Enc2;
00596         //pc.printf("%d\r\n",ticks2);
00597 
00598     }
00599     
00600 }
00601 
00602 void moveBottom(int amt){
00603     int newticks = ticks + amt;
00604     motorB(-1);
00605     while (ticks < newticks-5){
00606         encCount(1);
00607         //pc.printf("%d,%d\n\r",newticks,ticks);
00608     }   
00609     motorB(0);
00610     return;
00611 }
00612 void moveTop(int amt){
00613     int newticks = ticks2 + amt;
00614     motorA(1);
00615     while (ticks2 < newticks-5){
00616         encCount2(1);
00617         //pc.printf("%d,%d\n\r",newticks,ticks2);
00618     }   
00619     motorA(0);
00620     return;
00621 }