File content as of revision 8:a6080c27f8c5:

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

Ticker time; // attach this to return home function, set according to round time


//   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
int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
void findPath(int color, int i); //Determines where to take the token based on the color and its relative location
 

//    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;
float leg; 


 


//    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()
{
    //Start a timer
    time.attach(&returnHome, TIME); 
    
    float radDistance = 0.5;
    float posDistance = 0.5;
    float armDistance = 0.5;


     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
    
     
    //   RGB Sensor Settings     
     pc.baud(115200);
     green = 1; // off  
     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; 
     }
    
    //    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);
    
    //  Initialize the robot position
    move((0.6096-radDistance+posDistance+armDistance),FORWARD);
    turnLeft();
    move(radDistance,BACKWARD);
    move(0.762, FORWARD); 
    
    
    while(true)
     { 
        for(int i = 0; i <=8; i++) //begin motion around Nth perimeter
        {   
            leg = 0.762; //2.5 feet in meters 
            if(i % 2 == 0)
            {
                turnRight(); 
            }
            grabToken(); 
            color = findColor(); 
            if (color = 9)
            {
                break;
            }
            else 
            {
                findPath(color, i);
                dropToken(); 
                //returnPrevious(); 
            }
            move(leg, FORWARD); 
               
        }
       /* grabToken(); 
        move(1,FORWARD);
        turnLeft();
        wait(0.5);
        turnLeft();
        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);
       
        wait(4*(dist/stepSize)*(1/FREQUENCY));  //(dist/stepSize) is the number of steps; 1/FREQUENCY is the time per step
        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 = 0;    //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*(0.35343/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 = 1;    //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*(0.35343/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, 9=nothing
        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);
        }
        else{
            return(9);
        }
        // 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);
    }
void findPath(int color, int i)
{
    
    
}
}