File content as of revision 4:9b1c6b9dae1c:

#include "mbed.h"


//   PIN DECLARATIONS
DigitalOut FLdirection(PTB18); 
DigitalOut FRdirection(PTA4);
DigitalOut magDirection(PTB19);
PwmOut stepFL(PTD3); 
PwmOut stepFR(PTA5);
PwmOut magArm(PTA12);
InterruptIn killAll(PTC3); 
DigitalIn Start(PTC12); 
DigitalOut enableH(PTC11);
DigitalOut highH(PTC10);
DigitalOut enableL(PTC11);
DigitalOut highL(PTC7);
I2C i2c(PTC9, PTC8); //pins for I2C communication (SDA, SCL)
Serial pc(USBTX, USBRX);
DigitalOut LED(PTC4);
DigitalOut green(LED_GREEN);


//   PROTOTYPE FUNCTION DECLARATIONS
void move(float dist, bool direction); 
void grabToken();//Picks up the token for reading
void dropToken();//Drops the token off
void kill(); 
void turnRight(float, bool);
void turnLeft(float, bool);
void rot180(); //Turns the robot around

//    GLOBAL VARIABLES
const int FORWARD = 0;
const int BACKWARD = 1;
const float stepSize = 0.001212; //in meters
const float FREQUENCY = 500; //steps per second
int sensor_addr = 41 << 1;


//    NOTES
/*
    -36 prewriten functions for the drop off decision
    -Possibly use another RGB sensor as a line follower
    -Decides function based on color
    -findPath
        -starting at bottomLeft
        -travel up one leg
        -turn right if rgb
        -turn left if cmy
        -turn right, left, or stay based on color choice

    void findPath(); //Figures out the path to take to take the disk to its drop off position
    void returnHome(); //Returns to the home white square
    void returnPrevPos(); //Does the opposite of findPath() to return to the previous position

//Variables

    -boxSizes, 2x2, 3x3, 4x4, etc
    -legSize, 1 foot, 1.5 feet, 2 feet, etc.
    -direction choices for findPath
*/


int main()
{
    //Wait for a button press
    //Can use an interupt
    
    //Start a timer
    //Will need to be a variable timer based on round number
    
    //Start Sequence - Get to the lines to start reading tokens
    /*
        -Travel 2 feet forward minus the radDistance plus the posDistance plus the armDistance
        -Turn 90 degrees to the left with the left wheel stationary
        -Back up the radDistance
    
    */

     enableH = 0; //Making sure the H-Bridge starts low and off
     highH = 0; //This starts high for the H-Bridge
     highL = 1; //This starts low for the H-Bridge
     while(true) //The start button
     {
         if (Start == 0)
         break;
     }
     
     killAll.rise(&kill); //The kill interupt
     
     pc.baud(115200);
     green = 1; // off
    
     // Connect to the Color sensor and verify whether we connected to the correct sensor. 
    
    i2c.frequency(200000);
    
    //   RGB Sensor Settings
    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; 
    }
    
    //    Initialize color sensor
    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);
    
    
    
    while(true)
     { 
        grabToken(); 
        move(1,FORWARD);
        turnLeft(0.35343, FORWARD);
        wait(0.5);
        turnLeft(0.35343, FORWARD);
        wait(2); 
        dropToken(); 
        wait(2);
     }
}

//Distance is in meters
void move(float dist, bool direction)
{
        FLdirection = direction;
        FRdirection = !direction; 
        
        stepFL.period(1.0/FREQUENCY); 
        stepFR.period(1/FREQUENCY);
        stepFL.write(0.5f); 
        stepFR.write(0.5f);
        //dist/stepSize is the number of steps
        //1/FREQUENCY is the time per step
        wait(4*(dist/stepSize)*(1/FREQUENCY));
        stepFL.period(0.0f);
        stepFR.period(0.0f);
        stepFL.write(0.0f);
        stepFR.write(0.0f);
       
}

void grabToken()
{
        highL = 0;
        highH = 1;
        enableH = 1;
        wait(1);
        magDirection = 1; 
        magArm.period(0.002);
        magArm.write(0.5);
        wait(0.65);
        magArm.period(0);
        magArm.write(0);
    
}

void dropToken()
{  
        magDirection = 0; 
        magArm.period(0.002); 
        magArm.write(0.5); 
        wait(0.65); 
        magArm.period(0); 
        magArm.write(0);
        highL = 1;
        highH = 0;
        wait(2);
        enableH = 0;    
}

void turnRight(float dist, bool direction)
{ 
    //Get rid of all FR occurences which will turn right motor off
    FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
                                
    
    stepFL.period(1/FREQUENCY); 
    stepFL.write(0.5); 
    
    //dist/stepSize is the number of steps
    //1/FREQUENCY is the time per step
    wait(4*(dist/stepSize)*(1/FREQUENCY));
    stepFL.period(0);
    stepFL.write(0);
}
void turnLeft(float dist, bool direction)
{ 
    //Get rid of all FL occurences which will turn left motor off
    FRdirection = !direction;    //to turn right we want this going FORWARD, since FORWARD = 0, it must be !0
                                
    
    stepFR.period(1/FREQUENCY); // We could slow motor down by subtracting from denominator.
    stepFR.write(0.5); 
    
    //dist/stepSize is the number of steps
    //1/FREQUENCY is the time per step
    wait(4*(dist/stepSize)*(1/FREQUENCY));
    stepFR.period(0);
    stepFR.write(0);
}
void rot180()
{
    //Get rid of all FR occurences which will turn right motor off
    FLdirection = direction;    //to turn right we want this going FORWARD so a 0;
                                
    
    stepFL.period(1/FREQUENCY); 
    stepFL.write(0.5); 
    
    //dist/stepSize is the number of steps
    //1/FREQUENCY is the time per step
    wait(2*4*(0.35343/stepSize)*(1/FREQUENCY));
    stepFL.period(0);
    stepFL.write(0);
}
void kill() 
{
    exit(0);    
}

int findColor(){ //Figures out what color the disk is and makes a decision on where to take the disk
        
    while (true) { 
        wait(1);
        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];
        
        
        
        //1=red,2=green,3=blue,4=cyan,5=magenta,6=yellow,7=gray,8=error
        if(blue_value<10000 && red_value>10000){
            return(1);
        }
        else if(green_value>18000 && blue_value<30000){
            return(2);   
        }
        else if(red_value<10000 && blue_value>15000){
            return(3);   
        }
        else if(blue_value>30000 && red_value<20000){
            return(4);   
        }
        else if(red_value>25000 && green_value<15000){
            return(5);   
        }
        else if(red_value>50000){
            return(6);   
        }
        else if(red_value<10000 && blue_value<10000){
            return(7);   
        }
        else if(red_value==0){
            return(8);
        }
        
        // print sensor readings
        
        //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n", clear_value, red_value, green_value, blue_value);
        //wait(0.5);
    }
}