File content as of revision 0:7c80cce5d0d3:

#include "HMC5983.h"
#include "mbed.h"
#include "steppercontroller.h"


Serial pc(USBTX, USBRX);

I2C i2c(D14, D15);
HMC5983 compass(i2c);

Timer displayTimer;
Timer stepperRelaxTimer;
Timer compassPollTimer;
int crtState, prvState;
StepperController stepper(D7, D6, D5, D4);
double desiredAngle, actualAngle;

double angleDiff(double a, double b);

int main()
{
    prvState = 0;
    stepper.setSequenceType(StepperController::Interleaved);
    displayTimer.start();
    stepperRelaxTimer.start();
    compassPollTimer.start();
    stepper.setPulseWidth(0.2);
    desiredAngle = 0.0;
    while (1) {
        if (compassPollTimer.read() > 1) {
            compassPollTimer.reset();
            actualAngle = compass.read();
            
            pc.printf("Compass: %f\n", actualAngle);
            pc.printf("Difference: %f\n", angleDiff(actualAngle, desiredAngle));
            if (abs(angleDiff(actualAngle, desiredAngle)) > 5) {
                if (angleDiff(actualAngle, desiredAngle) > 0) {
                    stepper.setDirection(StepperController::DirectionCCW);
                    stepper.advance();        
                }
                else {
                    stepper.setDirection(StepperController::DirectionCW);
                    stepper.advance(); 
                }    
            }                 
        }
    }
}

double angleDiff(double a, double b)
{
    double diff = a - b;
    
    if (diff > 180)
        diff -= 360;
    if (diff < -180)
        diff += 360;
    return diff;
}