File content as of revision 0:84d5aa80fd77:

#include "mbed.h"

//Radio Calibration Signals - Need to implement calibration routine...
#define CHAN2_MID 1470  //Elevator Midpoint
#define CHAN1_MID 1470  //Rudder Midpoint
#define DEADZONE 105
#define MAXMIN_OFFSET 400

//Drive Modes
typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
DRIVEMODE mode;
float xpwm, ypwm;

//H-bridge
PwmOut rightMotorPWM(p22);  //Channel A
PwmOut leftMotorPWM(p21);   //Channel B
DigitalOut leftMotor1(p23);
DigitalOut leftMotor2(p24);
DigitalOut rightMotor1(p25);
DigitalOut rightMotor2(p26);

//Radio 
Serial pc(USBTX, USBRX);
DigitalIn rudder(p11);
DigitalIn elevator(p12);

//Xbee
Serial xbee(p13, p14);
DigitalOut xbeeReset(p15);

void setTurnLeft(float speed) {
    //Set speed
    rightMotorPWM = speed;
    leftMotorPWM = speed;
    
    //Set motor function
    leftMotor1 = 0;
    leftMotor2 = 1;
    rightMotor1 = 1;
    rightMotor2 = 0;
}

void setTurnRight(float speed) {
    //Set speed
    rightMotorPWM = speed;
    leftMotorPWM = speed;
    
    //Set motor function
    leftMotor1 = 1;
    leftMotor2 = 0;
    rightMotor1 = 0;
    rightMotor2 = 1;
}

//Stop with braking
void stop() {
    rightMotorPWM = 0;
    leftMotorPWM = 0;
    leftMotor1 = 0;
    leftMotor2 = 1;
    rightMotor1 = 1;
    rightMotor2 = 0;
}


int getRadioX() {
    Timer timer;
    timer.reset();
    int dur;
    while(!rudder);
    timer.start();
    while(rudder);
    dur = timer.read_us();
    return dur;
}

int getRadioY() {
    Timer timer;
    timer.reset();
    int dur;
    while(!elevator);
    timer.start();
    while(elevator);
    dur = timer.read_us();
    return dur;
}

//Primary function for setting motors for radio driving
void radioDrive() {
    int radioY = getRadioY();
    int radioX = getRadioX();
    
    //Identify drive mode from radio values
    if (radioY < CHAN2_MID - DEADZONE)
        mode = FORWARD;
    if (radioY > CHAN2_MID + DEADZONE)
        mode = REVERSE;
    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
        radioX < CHAN1_MID - DEADZONE)
        mode = CENTERLEFT;
    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
        radioX > CHAN1_MID + DEADZONE)
        mode = CENTERRIGHT;        
    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
        radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
        mode = STOP;
        
    //Normalize values for PWM magnitude
    xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
    ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
        
    //Set Motors Accordingly
    switch(mode) {
        case FORWARD:
            //Handle variable turns
            if (radioX > CHAN1_MID + DEADZONE) {
                rightMotorPWM = ypwm - 0.8*xpwm;
                leftMotorPWM = ypwm;
            } else if (radioX < CHAN1_MID - DEADZONE) {
                rightMotorPWM = ypwm;
                leftMotorPWM = ypwm - 0.8*xpwm;
            } else {
                rightMotorPWM = ypwm;
                leftMotorPWM = ypwm; 
            }
            leftMotor1 =  0;
            leftMotor2 = 1;
            rightMotor1 = 0;
            rightMotor2 = 1;
            break;
        
        case REVERSE:
            //Handle variable turns
            if (radioX > CHAN1_MID + DEADZONE) {
                rightMotorPWM = ypwm - 0.8*xpwm;
                leftMotorPWM = ypwm;
            } else if (radioX < CHAN1_MID - DEADZONE) {
                rightMotorPWM = ypwm;
                leftMotorPWM = ypwm - 0.8*xpwm;
            } else {
                rightMotorPWM = ypwm;
                leftMotorPWM = ypwm; 
            }
            leftMotor1 =  1;
            leftMotor2 = 0;
            rightMotor1 = 1;
            rightMotor2 = 0;
            break;
            
        case CENTERRIGHT:
            rightMotorPWM = xpwm;
            leftMotorPWM = xpwm;
            leftMotor1 = 1;
            leftMotor2 = 0;
            rightMotor1 = 0;
            rightMotor2 = 1;
            break;

        case CENTERLEFT:
            rightMotorPWM = xpwm;
            leftMotorPWM = xpwm;
            leftMotor1 = 0;
            leftMotor2 = 1;
            rightMotor1 = 1;
            rightMotor2 = 0;
            break;

        case STOP:
            rightMotorPWM = 0;
            leftMotorPWM = 0;
            leftMotor1 = 0;
            leftMotor2 = 1;
            rightMotor1 = 1;
            rightMotor2 = 0;
            break;
    }
}

//Transmit position and heading
void xbeeTelemetry() {
    xbee.printf("Human Update \r\n");   
    wait_ms(10);
}

bool isGameOver() {
    return xbee.readable();
}
        


int main() {
    
    //Init Xbee
    xbeeReset = 0;
    wait_ms(1);
    xbeeReset = 1;
    wait_ms(1);
    
    while(1) {
        xbee.printf("HELLO!\r\n");
        wait(0.1);
    }
}