File content as of revision 0:312f6a44fcac:

#include "mbed.h"

Serial pc(USBTX, USBRX); // tx, rx

AnalogIn pot1( p19 );
PwmOut servo( p21 );
I2C compass( p28, p27);
Ticker tick100hz;
Ticker tick5hz;

const int compass_addr = 0x42;
char cmd[3];
char compass_raw[2];
float pos = 0;
float ctrlval = 0;
float PWM_zero = .000061;

float north = 0.0;
float err, error_int;
float error_old = 0.0;
float error_int_old = 0.0;
float T = 0.01;

float kp = 0.0002;
float ki = 0.0;


void s100hz_task(void) {
    
    compass.read (compass_addr, compass_raw, 2 );
    pos = 0.1*((compass_raw[0]<<8) + compass_raw[1]) -90;
    if (pos>180) {
        pos = pos -360;
         
    }
    ctrlval = pot1.read() * .05;
    err = north-pos;
    error_int = error_int_old + .5 * (err + error_old) * T ;
    ctrlval = kp * err + ki * error_int;
    ctrlval = ctrlval + PWM_zero;
    error_old = err;
    error_int_old = error_int;    
    

    
    servo = ctrlval;    
}

void s5hz_task ( void ) {
    
    pc.printf ( "Position: %f, Control Duty Cycle: %f\n", pos, ctrlval );
}

int main() {
    
    cmd[0] = 0x47;
    cmd[1] = 0x74;
    cmd[2] = 0x72;
    
    compass.write ( compass_addr, cmd, 3 );
    
    tick100hz.attach ( &s100hz_task, .01 );
    tick5hz.attach ( &s5hz_task, .2 );
    
    while (1) {
        
    }
}