Revision 0:2df76cd30860, committed 2016-04-29

Comitter:

raj1995

Date:

Fri Apr 29 18:23:18 2016 +0000

Commit message:

Skittle Sorter Code.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Apr 29 18:23:18 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/Motor/#f265e441bcd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Fri Apr 29 18:23:18 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 29 18:23:18 2016 +0000
@@ -0,0 +1,621 @@
+#include <math.h>
+#include <stdio.h>
+#include "mbed.h"
+#define commonAnode true
+#include <algorithm>
+#include "Servo.h"
+
+
+//Color Sensor
+I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
+int sensor_addr = 41 << 1;
+DigitalOut led(p11);
+
+//IR
+Serial pc(USBTX, USBRX);
+Serial device(p13, p14);  // tx, rx
+PwmOut IRLED(p21);
+
+//Solenoid
+DigitalOut Ctrl(p8); //Solenoid output control bit
+
+
+//Motor A
+PwmOut speedA(p23);        // PWM Speed pin for Motor A
+DigitalOut fwdA(p17);      // Digital out signal for forward direction for Motor A
+DigitalOut revA(p18);      // Digital out signal for reverse direction for Motor A
+
+//MotorB
+PwmOut speedB(p24);        // PWM Speed pin for Motor B
+DigitalOut fwdB(p20);      // Digital out signal for forward direction for Motor B
+DigitalOut revB(p19);      // Digital out signal for reverse direction for Motor B
+
+
+//Bottom Encoder
+DigitalIn Enc1(p16); //right motor
+int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
+int ticks = 0;
+int ticksPrime = 0;
+int dist2move = 0;
+
+//Top Encoder
+DigitalIn Enc2(p15); //right motor
+int oldEnc2 = 0; //Was the encoder previously a 1 or zero?
+int ticks2 = 0;
+
+
+//Servo
+Servo servo(p25);
+
+//Prototypes (headers)
+void irSetup();
+void colorSensorSetup();
+int readColor();
+int readIR();
+void solOn();
+void solOff();
+void motorA(int);
+void motorB(int);
+void encCount(int);
+void encCount2(int);
+void moveBottom(int);
+void moveTop(int);
+void pulseBack(int);
+
+int redpos = 0;
+int greenpos = 0;
+int yellowpos = 0;
+int purplepos = 0;
+int randompos = 0;
+
+int turnAmount = 75;
+
+int greenList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int redList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int yellowList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int purpleList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int garbageList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+
+int currentColor = 4;
+int counter = 0;
+
+int main() {
+    
+
+    
+    //Color Sensor setup
+    colorSensorSetup();
+    
+    //Bottom Encoder setup
+    Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+    //Top Encoder setup
+    Enc2.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+    servo = 0.05;
+    bool succ = true;
+    double servoClosed = -0.05;
+    while(1) {
+        //pc.printf("%d\r\n",ticks);
+        /*
+        readColor();
+        wait(1);
+        
+                
+        
+        motorA(1);
+        wait(1);
+        motorA(0);
+        wait(1);
+        
+        
+        motorB(-1);
+        wait(1);
+        motorB(0);
+        wait(1);
+        motorB(1);
+        wait(1);
+        motorB(0);
+        wait(1);
+        */
+        
+        /*
+        motorA(1);
+        if (readIR == 1){
+            motorA(0);
+            solOn();
+            wait(1);
+            int color = readColor();
+            
+            if (color == 0){
+                
+            }else if (color == 1){
+                
+            }else if (color == 2){
+                
+            }else if (color == 3){
+            
+            }else{
+                
+                
+            }
+            
+            solOff();
+            wait(0.5);
+        }
+        
+        */
+        
+        /*
+        motorB(1);
+        encCount(1);
+        if (ticks==1000){
+            motorB(0);
+            break;
+        }
+        motorA(1);
+        */
+        
+      //succ = false;
+        if (succ){
+            moveTop(94);
+            succ = false;
+            wait(0.5);
+        }
+        encCount2(1);
+        //pc.printf("%d\r\n",ticks2);
+        wait(0.8);
+        int nextColor = readColor();
+        //pc.printf("%d,%d\n\r",currentColor,nextColor);
+        switch (currentColor){
+            
+        case 0:
+         
+            switch (nextColor){
+            
+            case 0:
+                
+                moveBottom(greenList[0]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                
+                break;
+            case 1:
+                moveBottom(greenList[1]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 2:
+                moveBottom(greenList[2]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 3:
+                moveBottom(greenList[3]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 4:
+                moveBottom(greenList[4]);
+                servo = servoClosed;
+                counter++;
+                break;
+            }
+            break;
+        case 1:
+         
+            switch (nextColor){
+            
+            case 0:
+                
+                moveBottom(redList[4]);
+                servo = 0.4;
+                wait(1);
+                servo = 0.05;
+                counter++;
+                break;
+            case 1:
+                moveBottom(redList[0]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                
+                break;
+            case 2:
+                moveBottom(redList[1]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 3:
+                moveBottom(redList[2]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 4:
+                moveBottom(redList[3]);
+                servo = servoClosed;
+                counter++;
+                break;
+            }
+            break;
+        case 2:
+         
+            switch (nextColor){
+            
+            case 0:
+                
+                moveBottom(yellowList[3]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 1:
+                moveBottom(yellowList[4]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 2:
+                moveBottom(yellowList[0]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                
+                break;
+            case 3:
+                moveBottom(yellowList[1]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 4:
+                moveBottom(yellowList[2]);
+                servo = servoClosed;
+                counter++;
+                break;
+            }
+            break;
+        case 3:
+         
+            switch (nextColor){
+            
+            case 0:
+                
+                moveBottom(purpleList[2]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 1:
+                moveBottom(purpleList[3]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 2:
+                moveBottom(purpleList[4]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                counter++;
+                break;
+            case 3:
+                moveBottom(purpleList[0]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                
+                break;
+            case 4:
+                moveBottom(purpleList[1]);
+                servo = servoClosed;
+                counter++;
+                break;
+            }
+            break;    
+        case 4:
+         
+            switch (nextColor){
+            
+            case 0:
+                ticksPrime = ticks;
+                dist2move = nextColor*75;
+                moveBottom(garbageList[1]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                if(ticks <= ticksPrime + dist2move){
+                    succ = true;
+                }
+                counter++;
+                break;
+            case 1:
+                ticksPrime = ticks;
+                dist2move = nextColor*75;
+                moveBottom(garbageList[2]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                if(ticks <= ticksPrime + dist2move){
+                    succ = true;
+                }
+                counter++;
+                break;
+            case 2:
+                ticksPrime = ticks;
+                dist2move = nextColor*75;
+                moveBottom(garbageList[3]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                if(ticks <= ticksPrime + dist2move){
+                    succ = true;
+                }
+                counter++;
+                break;
+            case 3:
+                ticksPrime = ticks;
+                dist2move = nextColor*75;
+                moveBottom(garbageList[4]);
+                servo = 0.4;
+                wait(1);
+                servo = servoClosed;
+                if(ticks <= ticksPrime + dist2move){
+                    succ = true;
+                }
+                counter++;
+                break;
+            case 4:
+                ticksPrime = ticks;
+                dist2move = nextColor*75;
+                moveBottom(garbageList[0]);
+                servo = servoClosed;
+                if(ticks <= ticksPrime + dist2move){
+                    succ = true;
+                }
+                break;
+            }
+            break;    
+        }
+        encCount(1);
+        currentColor = nextColor;
+        if (counter==4){
+            pulseBack(16);
+            counter = 0;  
+            pc.printf("%d\r\n",counter);  
+        }        
+        
+    }
+}
+
+int readColor(){
+
+    
+    // Initialize color sensor
+        char clear_reg[1] = {148};
+        char clear_data[2] = {0,0};
+        i2c.write(sensor_addr,clear_reg,1, true);
+        i2c.read(sensor_addr,clear_data,2, false);
+        
+        int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
+        
+        char red_reg[1] = {150};
+        char red_data[2] = {0,0};
+        i2c.write(sensor_addr,red_reg,1, true);
+        i2c.read(sensor_addr,red_data,2, false);
+        
+        int red_value = ((int)red_data[1] << 8) | red_data[0];
+        
+        char green_reg[1] = {152};
+        char green_data[2] = {0,0};
+        i2c.write(sensor_addr,green_reg,1, true);
+        i2c.read(sensor_addr,green_data,2, false);
+        
+        int green_value = ((int)green_data[1] << 8) | green_data[0];
+        
+        char blue_reg[1] = {154};
+        char blue_data[2] = {0,0};
+        i2c.write(sensor_addr,blue_reg,1, true);
+        i2c.read(sensor_addr,blue_data,2, false);
+        
+        int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
+        
+        // print sensor readings
+        
+        
+        //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n\r", clear_value, red_value, green_value, blue_value);
+        //pc.printf("Red (%d), Green (%d), Blue (%d)\n\r", red_value, green_value, blue_value);
+        
+        
+        float red_in = (float)red_value;
+        float green_in = (float)green_value;
+        float blue_in = (float)blue_value;
+        
+        float redSkittle[] = {4656.25,588.25,826};
+        float greenSkittle[] = {2968.25,3898.5,1346.5};
+        float yellowSkittle[] = {15387.75,9977.5,3173.75};
+        //float purpleSkittle[] = {1088,342.25,340.5};
+        float purpleSkittle[] = {1215,1773,2541};
+        float closed[] = {1094, 1149, 987};
+        
+        float distRed = sqrt(pow(red_in-redSkittle[0],2)+pow(green_in-redSkittle[1],2)+pow(blue_in-redSkittle[2],2));
+        float distGreen = sqrt(pow(red_in-greenSkittle[0],2)+pow(green_in-greenSkittle[1],2)+pow(blue_in-greenSkittle[2],2));
+        float distYellow = sqrt(pow(red_in-yellowSkittle[0],2)+pow(green_in-yellowSkittle[1],2)+pow(blue_in-yellowSkittle[2],2));
+        float distPurple = sqrt(pow(red_in-purpleSkittle[0],2)+pow(green_in-purpleSkittle[1],2)+pow(blue_in-purpleSkittle[2],2));
+        float distClosed = sqrt(pow(red_in-closed[0],2)+pow(green_in-closed[1],2)+pow(blue_in-closed[2],2));
+        
+        float choices[] = {distRed, distGreen, distYellow, distPurple, distClosed};
+        
+        sort(choices,choices+5);
+        
+        float closest = choices[0];
+        
+
+        if (closest==distRed){
+            //pc.printf("Red\n\r");
+            return 1;
+        }else if(closest==distGreen){
+            //pc.printf("Green\n\r");
+            return 0;
+        }else if(closest==distYellow){
+            //pc.printf("Yellow\n\r");
+            return 2;
+        }else if(closest==distPurple){
+            //pc.printf("Purple\n\r");
+            return 3;
+        }else //pc.printf("Dave messed up.\n\r");
+        
+        return 4;
+        
+        
+}
+
+int readIR(){
+    
+    if(pc.readable()) {
+        device.putc(pc.getc());
+    }
+    //IR Receive code
+    if(device.readable()) {
+        pc.putc(device.getc());
+    }
+    return 1;
+}
+
+void solOn(){
+    Ctrl = 1; //ON   
+}
+
+void solOff(){
+    Ctrl = 0; //ON   
+}
+
+void motorA(int state){
+    fwdA = 1;
+    revA = 0;
+    speedA = (float)state*0.15;
+}
+
+void motorB(int dir){
+    if (dir > 0){
+        fwdB = 1;
+        revB = 0;
+    }else{
+        fwdB = 0;
+        revB = 1;
+        dir = -dir;
+    }
+    speedB = (float)dir*.3;   
+}
+void pulseBack(int dir) {
+    //int newticks = ticks - dir;
+    motorB(1);
+    /*
+    while (ticks > newticks){
+        encCount(-1);
+        //pc.printf("%d,%d\n\r",newticks,ticks2);
+    }   
+    */
+    wait(0.1);
+    motorB(0);
+    return;    
+}
+void irSetup(){
+    //IR Transmit setup code
+    IRLED.period(1.0/38000.0);
+    IRLED = 0.5;
+    //Drive IR LED data pin with 38Khz PWM Carrier
+    //Drive IR LED gnd pin with serial TX
+    device.baud(2400);    
+}
+
+void colorSensorSetup(){
+    pc.baud(9600);
+    
+    led = 1; // off
+    // Connect to the Color sensor and verify whether we connected to the correct sensor. 
+    
+    i2c.frequency(200000);
+    
+    char id_regval[1] = {146};
+    char data[1] = {0};
+    i2c.write(sensor_addr,id_regval,1, true);
+    i2c.read(sensor_addr,data,1,false);
+    
+    if (data[0]==68) {
+        //green = 0;
+        wait (2); 
+        //green = 1;
+        } else {
+        //green = 1; 
+    }
+    
+    char timing_register[2] = {129,0};
+    i2c.write(sensor_addr,timing_register,2,false);
+    
+    char control_register[2] = {143,0};
+    i2c.write(sensor_addr,control_register,2,false);
+    
+    char enable_register[2] = {128,3};
+    i2c.write(sensor_addr,enable_register,2,false);
+    
+
+}
+
+void encCount(int dir){
+    if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched. 
+        if (dir>0) ticks++;
+        else ticks--;
+        oldEnc1 = Enc1;
+        //pc.printf("%d\r\n",ticks);
+
+    }
+    
+}
+
+
+void encCount2(int dir){
+    if(Enc2 != oldEnc2) { // Increment ticks every time the state has switched. 
+        if (dir>0) ticks2++;
+        else ticks2--;
+        oldEnc2 = Enc2;
+        //pc.printf("%d\r\n",ticks2);
+
+    }
+    
+}
+
+void moveBottom(int amt){
+    int newticks = ticks + amt;
+    motorB(-1);
+    while (ticks < newticks-5){
+        encCount(1);
+        //pc.printf("%d,%d\n\r",newticks,ticks);
+    }   
+    motorB(0);
+    return;
+}
+void moveTop(int amt){
+    int newticks = ticks2 + amt;
+    motorA(1);
+    while (ticks2 < newticks-5){
+        encCount2(1);
+        //pc.printf("%d,%d\n\r",newticks,ticks2);
+    }   
+    motorA(0);
+    return;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Apr 29 18:23:18 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/082adc85693f
\ No newline at end of file