File content as of revision 10:ed6ecbb420b8:

#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();
void turnRight45();
void turnLeft();
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 returnToPrevious(int, int);
void positionIsBlue();      // uses these 6 function to return back to previous position
void positionIsRed();
void positionIsGreen();
void positionIsCyan();
void positionIsMagenta();
void positionIsYellow();


//    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; 
int color; 


//    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
    //Will need to be a variable timer based on round number
    
    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);
    
    while(true)
     { 
        for(int i = 0; i <=8; i++)
        {   
            leg = 0.762; //2.5 feet in meters 
            grabToken(); 
            color = findColor(); 
            if (color = 9)
            {
                break;
            }
            else 
            {
                findPath(color)
            }
            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);
        //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()
{ 
    //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 turnRight45()
{ 
    //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(2*(0.35343/stepSize)*(1/FREQUENCY));
    stepFL.period(0);
    stepFL.write(0);
}
void turnLeft()
{ 
    //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);
    }
}

//this function will use the iterator (i) to find the position it was just at and return there
//it will also use the color global variable to determine it's path there and the square num to determine 
//how many steps it will need to take in once it gets to its iterator position
// square num must = 0 for the outer square, 1 for next one in, 2 for following and 3 for innermost square. 

void returnToPrevious(int i, int square_num)
{
    rot180(); //gets robot back to normal orientation pointing in towards the middle
    if(color == 2 || color == 4 || color == 6 || color == 8)
    {
        move(0.2173224, 0); // moves the robot forward 0.7128 feet forward which is the diagonal from color square to back on playing field
        if(color == 2){
            positionIsBlue();
            turnLeft();
            move(square_num*0.1524, 0);
            }
        else if(color == 3){
            positionIsGreen();
            }
        else if(color == 4){
            positionIsRed();
            move(square_num*0.1524, 0);
            }
        else if(color == 6){
            positionIsCyan();
            move(square_num*0.1524, 0);
            }
        else if(color == 7){
            positionIsMagenta();
            }
        else if(color == 8){
            positionIsYellow();
            move(square_num*0.1524, 0);
            }       
    }
    
    else{
        move(0.1524, 0); // moves the robot forward half a foot, or the distance from edge of box to line
        if(color == 3){
            positionIsGreen();
            move(square_num*0.1524, 0);
            }
        else if(color == 7){
            positionIsMagenta();
            move(square_num*0.1524, 0);
            }
    }
}


void positionIsBlue()
{
    int pos;
    turnright45();
    for(int j = color; j=i; j--){
        pos = j % 8;
        move(leg, 0);
        if( pos%2 == 1){
            turnLeft();
        }
    }          
} 
void positionIsGreen()
{
    int pos;
    turnright();
    for(int j = color; j=i; j--){
        pos = j % 8;
        move(leg, 0);
        if( pos%2 == 1){
            turnLeft();
        }
    }          
} 
void positionIsRed()
{
    int pos;
    turnright45();
    for(int j = color; j=i; j--){
        pos = j % 8;
        move(leg, 0);
        if( pos%2 == 1){
            turnLeft();
        }
    }          
} 
void positionIsCyan()
{
    int pos;
    turnright45();
    for(int j = color; j=i; j--){
        pos = j % 8;
        move(leg, 0);
        if( pos%2 == 1){
            turnLeft();
        }
    }          
}  
void positionIsMagenta()
{
    turnright();
    for(int j = color; j=i; j++){
        j = j%8;
        move(leg, 0);
        if( j == 7 || j == 5 || j == 3 || j==1){
            turnLeft();
        }
    }          
}  

void positionIsYellow()
{
    int pos;
    turnright45();
    for(int j = color; j=i; j--){
        pos = j % 8;
        move(leg, 0);
        if( pos%2 == 1){
            turnLeft();
        }
    }          
}