Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_AutonomousDriver

Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.

Dependencies:   IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos

HumanControl.h

Committer:
Strikewolf
Date:
2014-04-30
Revision:
3:0a6e4d139b86
Parent:
1:dacf7db790f6

File content as of revision 3:0a6e4d139b86:

//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
DigitalIn rudder(p11);
DigitalIn elevator(p12);

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 && !gameOver);
    timer.start();
    while(rudder && !gameOver);
    dur = timer.read_us();
    return dur;
}

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

//Primary function for setting motors for radio driving
void radioDrive()
{   
    __disable_irq(); // disable interrupts
    int radioY = getRadioY();
    int radioX = getRadioX();
    __enable_irq(); // enable interrupts

    //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 = 0.2;
                leftMotorPWM = 0.85;
            } else if (radioX < CHAN1_MID - DEADZONE) {
                rightMotorPWM = 0.85;
                leftMotorPWM = 0.2;
            } else {
                rightMotorPWM = .85;
                leftMotorPWM = .85;
            }
            leftMotor1 =  0;
            leftMotor2 = 1;
            rightMotor1 = 0;
            rightMotor2 = 1;
            break;

        case REVERSE:
            //Handle variable turns
            if (radioX > CHAN1_MID + DEADZONE) {
                rightMotorPWM = 0.2;
                leftMotorPWM = 0.85;
            } else if (radioX < CHAN1_MID - DEADZONE) {
                rightMotorPWM = 0.85;
                leftMotorPWM = 0.2;
            } else {
                rightMotorPWM = .85;
                leftMotorPWM = .85;
            }
            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;
    }
}