File content as of revision 1:41cee26b35cc:

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

#include "HMC6352.h"

//Gyro
HMC6352 compass(p28, p27);

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

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

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

//These functions are not completed yet...
void gyroDriveStraight(float speed, bool reverse);
void centerTurnLeft(int delta_degrees) {
    double initial = compass.sample() / 10;
    while ((compass.sample() / 10) > initial - delta_degrees){
        //Set speed
        rightMotorPWM = 0.8;
        leftMotorPWM = 0.8;
        leftMotor1 = 1;
        leftMotor2 = 0;
        rightMotor1 = 1;
        rightMotor1 = 0; 
    }
    stop();
}
void centerTurnRight(int delta_degrees);
void driveTurnLeft(int delta_degrees);
void driveTurnRight(int delta_degrees);