File content as of revision 4:b2a30c313297:

//*******************************************
//* Robot motor control and drive functions *
//*******************************************
 
#include "HMC6352.h"
#include "PositionSystem.h"
#include "SonarSystem.h"
#include <math.h>

#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
 
//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
 
//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);

float sampleCompass(int sample_size) {
    float c = 0;
    for(int i = 0; i < sample_size; i++)
        c += compass.sample() / 10.0;
    c = c / (double) sample_size;
    return c;
}
 
//Non-feedback corrected 'dumb' driving
void setMove(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;
}

// calibrate 90 degree turns
void turnCalibrate() {
    
    // tell positioning system to set the turn
    // calibration variable, PTURN_RIGHT
    PFlag = PFLAG_CALIB;
        
    // start turning hardcoded speed PTURN_SPEED
    setTurnRight(PTURN_SPEED);
    
    // wait until ROTATE_90_TIME has passed. This should
    // be the time to complete a 90 degree turn
    wait_ms(ROTATE_90_TIME);
    stop();
    
    // done calibrating, turn positioning system back on
    PFlag = PFLAG_ON;
}

// -------------------------------------------------------------------
 
//Turn right about the center
void turnRight(int delta_degrees)
{
    pc.printf("Turnleft: \n\r");
    
    // turn positioning system off during turns since
    // no x and y movements
    PFlag = PFLAG_OFF;
    
    // store new orientation after this turn executes
    t_position += 90;
    t_position %= 360;   
    
    // initialize turn variables
    turnInit();
    
    // start turning right
    setTurnRight(PTURN_SPEED);
    
    // read mouse sensor until 90 degress has completed
    while(pturn_x > PTURN_RIGHT) {
        pc.printf("%f, %f\n\r", pturn_x, pturn_y);
    }
    
    // stop turning
    stop();
    
    // turn positioning system back on
    PFlag = PFLAG_ON;
}

//Drive straight with compass correction
void move(float speed, bool reverse) {

    // begin moving
    setMove(speed, reverse);
    
    // blocking call
    while(1) {
  
        // read all sonar sensors
        float s1 = sonar1.read() * 100;
        float s2 = sonar2.read() * 100;
        float s3 = sonar3.read() * 100;
            
        // check if obstacle is near and adjust 
        if(s2 < SONAR_STOP) {
            pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3);
            if(s1 >= SONAR_STOP) {
               //turnLeft(90);
            } else if(s3 >= SONAR_STOP) {
               turnRight(90);
            } else {
               pc.printf("IM STUCK HALP\n\t");
            }
        }  
          
    }
 }