File content as of revision 0:2df76cd30860:

#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;
}