File content as of revision 2:56eb726bdb0d:

//*******************************************
//* Robot motor control and drive functions *
//*******************************************

#include "HMC6352.h"

//Radio Calibration Signals - Need to implement calibration routine...
#define CHAN3_MAX 2060  //Throttle Hi
#define CHAN3_MIN 1150  //Throttle Lo
#define CHAN2_MAX 1260  //Elevator Hi
#define CHAN2_MIN 2290  //Elevator Lo
#define CHAN1_MAX 2160  //Rudder Hi
#define CHAN1_MIN 1210  //Rudder Lo

//Debug
Serial pc(USBTX, USBRX);

//Gyro
HMC6352 compass(p28, p27);

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

//Compass sampling functions - TBA if compass keeps freaking out
int prev_sample, cur_sample;
void prepCompass() {
    int i;
    for (i = 0; i <= 4; i++) {
        //Chew through a few samples, the compass seems to return "stale" values if unsampled for awhile
        compass.sample();
    }
}

//Non-feedback corrected 'dumb' driving
void setDriveStraight(float speed, bool reverse) {
    //Set speed
    rightMotorPWM = speed;
    leftMotorPWM = speed;
    
    //Set motor function
    leftMotor1 = (!reverse) ? 0 : 1;
    leftMotor2 = (!reverse) ? 1 : 0;
    rightMotor1 = (!reverse) ? 0 : 1;
    rightMotor2 = (!reverse) ? 1 : 0;
}

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

//Turn left about the center
void centerTurnLeft(int delta_degrees) {
    prepCompass();
    
    float initial = (compass.sample() / 10);
    float target = initial - delta_degrees;
    double sample;
    bool rollover = false;
    
    if (target < 0) {
        target += 360;
        rollover = true;
    }

    pc.printf("Init: %f Target: %f\r\n", initial, target);

    //Take care of 360 degree rollover
    if(rollover) {
        pc.printf("begin  rollover \r\n");
        while(true) {
            setTurnLeft(0.6);
            sample = compass.sample() / 10;
            wait(0.02);
            if (sample > 345)
                break;
        }
        pc.printf("rollover complete\r\n");
    }

    //Continue to turn to target
    while (true) {
        setTurnLeft(0.6);
        sample = compass.sample()/10;
        wait(0.02);
        if(sample < target)
            break;
    }
    stop();
}

//Turn right about the center
void centerTurnRight(int delta_degrees)
{
    prepCompass();
    
    float initial = (compass.sample() / 10);
    float target = initial + delta_degrees;
    double sample;
    bool rollover = false;
    
    if (target > 360) {
        target -= 360;
        rollover = true;
    }

    pc.printf("Init: %f Target: %f\r\n", initial, target);

    //Take care of 360 degree rollover
    if(rollover) {
        pc.printf("begin  rollover \r\n");
        
        while(true) {
            setTurnRight(0.6);
            sample = compass.sample() / 10;
            wait(0.02);
            if (sample > 0 && sample < 10)
                break;
        }
        pc.printf("rollover complete\r\n");
    }

    //Continue to turn to target
    while (true) {
        setTurnRight(0.6);
        sample = compass.sample()/10;
        wait(0.02);
        if(sample > target)
            break;
    }
    stop();
}

//Drive straight with compass correction
void compassDriveStraight(float speed, bool reverse, int ms) {
    Timer timer;
    double sample;
    
    setDriveStraight(speed, reverse);
    wait(0.2);
    //really hacky way to compensate for motor EM interference
    prepCompass();
    float initial = compass.sample() / 10;
    
    //Really hacky way to deal with 360 rollover
    if (initial + 6 > 360)
        centerTurnLeft(7);
    if (initial - 6 < 0)
        centerTurnRight(7);
    
    timer.start();
    while (timer.read_ms() < ms) {
        sample = compass.sample() / 10;
        pc.printf("%f :::", sample);
        
        if (sample > initial + 3) {
            pc.printf("Steer Left\r\n");
            leftMotorPWM = speed - 0.1;
            rightMotorPWM = speed + 0.1;
            leftMotor1 = 0;
            leftMotor2 = 1;
            rightMotor1 = 0;
            rightMotor2 = 1;
        } else if (sample < initial - 3) {
            pc.printf("Steer Right \r\n");
            leftMotorPWM = speed + 0.1;
            rightMotorPWM = speed - 0.1;
            leftMotor1 = 0;
            leftMotor2 = 1;
            rightMotor1 = 0;
            rightMotor2 = 1;
        } else {
            pc.printf("Straight\r\n");
            setDriveStraight(speed, reverse);
        }
    }
    stop();
}