File content as of revision 0:5d512f2b35f8:

/* Program Example 13.3: Closed loop compass program
                                                                    */
#include "mbed.h"

// mbed objects
I2C compass(p28, p27);        // sda, scl
PwmOut PWM(p25);
AnalogIn Ain(p20);
Serial pc(USBTX, USBRX);         // tx, rx
Ticker s100hz_tick;              // 100 Hz (10ms) ticker
Ticker s5hz_tick;                // 5 Hz (200ms) ticker

// variables
const int addr = 0x42; // define the I2C write Address
char cmd[3];
float pos;             // measured position
float setpos=0;        // setpoint position = zero (North)
float error;           // calculated error 
float ctrlval;         // PWM control value
float kp=0.0002;           // proportional gain
float PWM_zero=0.075;      // zero value

// function prototypes
void s100hz_task(void);    // 100 Hz task
void s5hz_task(void);      // 5 Hz task

// main code
int main() {
  // initialise and setup data
  PWM.period(0.02);
  cmd[0] = 0x47;                 // 'G' write to RAM address
  cmd[1] = 0x74;                 // Operation mode register address
  cmd[2] = 0x72;                 // Op mode = 20H, S/R, continuous 
  compass.write(addr,cmd, 3);    // Send operation 
  // assign timers
  s100hz_tick.attach(&s100hz_task,0.01);  //attach 100 Hz task to 10ms tick
  s5hz_tick.attach(&s5hz_task,0.2);       //attach 5Hz task to 200ms tick
  while(1){
  // loop forever
  }
}

// function 100hz_task
void s100hz_task(void) {                  
  compass.read(addr, cmd, 2); // read the two-byte echo result 
  //convert data to degrees
  pos = 0.1 * ((cmd[0] << 8) + cmd[1]);
  if (pos>180)
    pos=pos-360;       
  error = setpos - pos;           // get error
  ctrlval = (kp * error);         // calculate ctrlval (proportional)
  ctrlval = ctrlval + PWM_zero;   // add control value to zero position
  PWM = ctrlval;                  // output to PWM
}

// function 5hz_task
void s5hz_task(void) {
  pc.printf("deg = %.1f error=%.1f ctrlval=%.4f\n",pos,error,ctrlval);
}