File content as of revision 0:178a07cd3e39:

#include "mbed.h"
#include "bees.h"

#define DEBUG 1
#define V_PER_IN 0.0098 // mV/in

#ifdef DEBUG
Serial pc(USBTX, USBRX); // tx, rx
#endif

DigitalOut myled(LED1);

AnalogIn srf(p20);
Serial xbee(p9, p10);
DigitalOut xbee_reset(p11);


///
AnalogIn r_encoder(p15);
AnalogIn l_encoder(p16);

DigitalOut r_charge(p5);
DigitalOut l_charge(p6);

Motor r_motor(p21,p30,p29);
Motor l_motor(p22,p7,p8);

volatile int prev_r_enc = 0;
volatile int prev_l_enc = 0;

volatile int r_tics = 0;
volatile int l_tics = 0;

volatile int r_goal = 0;
volatile int l_goal = 0;

volatile float r_C = -1;
volatile float l_C = -1;

Ticker charge_period;
Ticker discharge_period;

int directions[2*DIRECTIONS+1];

MovementState move_state = Stopped;
///
//DoType dotype;


int main() {
    Bee_Type type;
    Operation_Mode mode;
    float distance;
    
    QTIsensor_init();
    charge_period.attach(&QTIsensor_charge,0.02);
    wait_ms(0.5);
    discharge_period.attach(&QTIsensor_discharge,0.02);
    
    // init
    xbee_reset = 0;
    wait_ms(1);
    xbee_reset = 1;
    // set the type of robot
    type = Leader;
    if(type == Leader)
        mode = Blaze;
    else if (type == Follower)
        mode = GetDirections;
        
    //calibrate(0.5, 40);  
    //move(100);  
    
    //wait(2);
    //move_state = Moving;
    
    //move(0.5, 80);
    
    distance = 10000 * srf;
    srand((int)distance);
    
    while(1) {
        if(DEBUG) {
            pc.printf("Left : %d  Goal: %d\n\r", l_tics, l_goal);
            pc.printf("Right: %d  Goal: %d\n\r", r_tics, r_goal);
        }
        
        //wait(0.2);
        distance = (254 - 6) * srf;
        
        if (DEBUG)
            pc.printf("Range: %f\n\r", distance);
        
        if(distance <= 8)
            rotate(1);
        
        //distance = srf / V_PER_IN;
        //if(DEBUG)
        //    pc.printf("Distance: %f in\n\r", distance);
        
        //if(xbee.readable())
        //    directions[1] = xbee.getc();
        
        /*
            This state machine swithces between the operating 
            modes of the robot and calls the appropriate 
            function in bees.cpp
        */
        
        //calibrate(0.5,50);
        
        /*switch(mode) {
            case Seek:
                if(DEBUG)
                    pc.printf("Seeking Pollen\n\r");
                if(find_pollen())
                    mode = Return;
                break;
            case Return:
                if(DEBUG)
                    pc.printf("Returning to Base\n\r");
                if(return_home()) {
                    if(type == Seeker)
                        mode = SendDirections;
                    else if(type == Gatherer)
                        mode = Gather;
                }
                break;
            case SendDirections:
                if(DEBUG)
                    pc.printf("Sending Directions\n\r");
                if(send_directions())
                    mode = Seek;
                break;
            case GetDirections:
                if(DEBUG)
                    pc.printf("Waiting for Directions\n\r");
                if(get_directions())
                    mode = Gather;
                break;
            case Gather:
                if(DEBUG)
                    pc.printf("Gathering Pollen\n\r");
                if(goto_pollen())
                    mode = Gather;
                break;
            default:
                if(DEBUG)
                    pc.printf("Broken :(\n\r");
        }*/
        
        switch(mode) {
            case Blaze:
                if(DEBUG)
                    pc.printf("Creating Path\n\r");
                blaze();
                //pc.printf("now exiting blaze\n\r");
                mode = Copy;
                break;
            case SendDirections:
                if(DEBUG)
                    pc.printf("Sending Directions\n\r");
                send_directions();
                break;
            case GetDirections:
                if(DEBUG)
                    pc.printf("Waiting for Directions\n\r");
                if(get_directions())
                    mode = Copy;
                break;
            case Copy:
                if(DEBUG)
                    pc.printf("Moving\n\r");
                if(copy()) {
                    if(type == Leader)
                        mode = SendDirections;
                }
                break;
            default:
                if(DEBUG)
                    pc.printf("Broken :(\n\r");
        }
        //pc.printf("now checking move_state\n\r");
        switch(move_state) {
            case Stopped:
                //do nothing
                r_motor.speed(NEUTRAL);
                l_motor.speed(NEUTRAL);
                break;
            /*case Stop:
                //stop the motors
                r_motor.stop();
                l_motor.stop();
                mov_state = Stopped;*/
            case Moving:
                //sample encoders
                //update_movement(0.5);
                //pc.printf("now checking encoders right = %d goal = %d, left = %d goal = %d\n\r",r_tics,r_goal,l_tics,l_goal);
                if ((r_tics >= r_goal) || (l_tics >= l_goal)) {
                    r_motor.speed(NEUTRAL);
                    l_motor.speed(NEUTRAL);
                    move_state = Stopped;
                }
                
                break;
        }
        
        myled = 1;
        wait(0.2);
        myled = 0;
        wait(0.2);
    }
}