File content as of revision 2:56eb726bdb0d:

#include "mbed.h"
#include "RobotControl.h"
#include "rtos.h"

#define ENTERCALIB 0x43
#define EXITCALIB 0x45

InterruptIn sw(p30);

void StopISR()
{
    rightMotorPWM = 0;
    leftMotorPWM = 0;
    exit(1);
}

void InitCompass() {
    //Init compass
    compass.setReset();
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    //Calibration level - Garrus
    //Rotate robot one full rotation during this section
    pc.printf("Begin Calibration...\r\n");
    compass.setCalibrationMode(ENTERCALIB);
    wait(10);
    compass.setCalibrationMode(EXITCALIB);
    
    //Sample a few to clean out buffer
    compass.sample();
    compass.sample();
    compass.sample();
}

int main() {
    //Emergency stop mechanism
    sw.rise(&StopISR);

    //Setup mode and perform calibration
    InitCompass();
    
    compassDriveStraight(0.7, false, 20000);
    
    while(1) {
        float sample = compass.sample() / 10;
        pc.printf("%f\r\n", sample);
    }
}