File content as of revision 0:aa02ddcd8bc3:

#include "mbed.h"
#include "C12832_lcd.h"
Serial pc(USBTX, USBRX); // tx, rx

C12832_LCD lcd;

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 = .072;

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

float kp = 0.00015; // Decreasing kp made the adjustments smaller and smoother
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;
    north = baseline + pot1.read() * 180.0 ;
    err = pos - north;
    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 ) {
    
    //lcd.locate (0, 0);
    
    pc.printf ( "Position: %f, Control Duty Cycle: %f, North: %f \n", pos, ctrlval, north);
}

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) {
        
    }
}