File content as of revision 2:5f29bc7daa49:

#include "mbed.h"

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

//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
*/
/*
    int findColor(); //Figures out what color the disk is and makes a decision on where to take the disk
    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
    void rot90(); //Rotates the robot 90 degrees to the left
    void rot180(); //Turns the robot around
    void readToken(); //Gets the color data from the RGB sensor
*/

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


*/

void move(int dist, bool direction); 
void grabToken();//Picks up the token for reading
void dropToken();//Drops the token off
void kill(); 
void turnLeft(int, bool);
//void rot90(); //Rotates the robot 90 degrees to the left
//void rot180(); //Turns the robot around

    const int FORWARD = 0;
    const int BACKWARD = 1;
    const float stepSize = 0.001212; //in meters
    const float FREQUENCY = 500; //steps per second




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
    
    */

     highH = 1; //This is always high for the H-Bridge
     enableH = 0; //Making sure the H-Bridge starts low and off
     while(true) 
     {
         if (Start == 0)
         break;
     }
    
    while(true)
    
     {
        killAll.rise(&kill); 
        grabToken(); 
        move(1,FORWARD);
        wait(2); 
        dropToken(); 
        wait(2);
        move(1, BACKWARD); 
        
     }
}
//Distance is in meters
void move(int dist, bool direction)
{
        FLdirection = direction;
        FRdirection = !direction; 
        
        stepFL.period(1/FREQUENCY); 
        stepFR.period(1/FREQUENCY);
        stepFL.write(0.5); 
        stepFR.write(0.5);
        //dist/stepSize is the number of steps
        //1/FREQUENCY is the time per step
        wait((dist/stepSize)*(1/FREQUENCY));
        stepFL.period(0);
        stepFR.period(0);
        stepFL.write(0);
        stepFR.write(0);
       
}
void grabToken()
{
        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); 
        enableH = 0; 
        magArm.period(0); 
        magArm.write(0);    
}
void turnLeft(int dist, bool direction)
{ 
    //Get rid of all FR occurences which will turn right motor off
    FLdirection = direction;    //to turn left 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((dist/stepSize)*(1/FREQUENCY));
    stepFL.period(0);
    stepFL.write(0);
}
void rot180()
{
     
}
void kill() 
{
    exit(0);    
}