File content as of revision 3:828e82089564:

#include "mbed.h"
#define _MBED_
#include "Adafruit_9DOF.h"
#include "Serial_base.h"
#include "controller.h"
#include "sensor.h"

DigitalOut myled(LED1);
Adafruit_9DOF dof = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);

Serial pc(USBTX, USBRX);



int main()
{
    control result;
    offset offset_gyro; 

    initSensors(accel, mag, gyro,offset_gyro);

    sensors_event_t accel_event;
    sensors_event_t mag_event;
    sensors_event_t gyro_event;
    sensors_vec_t   orientation;
    


    while (1) {

        /* Calculate pitch and roll from the raw accelerometer data */
        accel.getEvent(&accel_event);
        if (dof.accelGetOrientation(&accel_event, &orientation)) {
            /* 'orientation' should have valid .roll and .pitch fields */
            //s_com->print(("Roll: "));
            //s_com->print(orientation.roll);
            //s_com->print(("; "));
            //s_com->print(("Pitch: "));
            //s_com->print(orientation.pitch);
            //s_com->print((";\t"));
        }
        /* Calculate the heading using the magnetometer */
        mag.getEvent(&mag_event);
        if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
            /* 'orientation' should have valid .heading data now */
            //s_com->print(("Heading: "));
            //s_com->print(orientation.heading);
          //  s_com->print((";\r\n"));
        }
        /* Get angular rate data from gyroscope */
        gyro.getEvent(&gyro_event);
        gyro_event.gyro.x -= offset_gyro.x_offset;
        gyro_event.gyro.y -= offset_gyro.y_offset;
        gyro_event.gyro.z -= offset_gyro.z_offset;

        wait(0.1);

        // get sensor values

        // call controller
        controller(gyro_event, orientation, &result);

        // plot

        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
        pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
    }
}