File content as of revision 5:8db82a61e052:

// example to test the mbed Lab Board lcd lib with the mbed rtos

#include "mbed.h"
#include "rtos.h"
#include "stdio.h"
#include "Motor.h"
#include "LSM9DS1.h"
#include "Servo.h"

#define PI 3.14159

Serial pc(USBTX, USBRX);
Motor m(p21, p16, p15); // pwm, fwd, rev
Servo myservo(p24); // pwm

using namespace std;

// mutex to make pitch R/W safe
Mutex pitch_mutex;

// global pitch float value
float pitch;

// returns pitch, can also return roll
float getAttitude(float ax, float ay, float az)
{
    float roll = atan2(ay, az);
    float pitch_res = atan2(-ax, sqrt(ay * ay + az * az));

    // Convert everything from radians to degrees:
    pitch_res *= 180.0 / PI;
    roll  *= 180.0 / PI;
    if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch_res,roll);
    return pitch_res;
}

// Thread 1
// propeller PWM control
void thread1(void const *args)
{
    
    while(true) {       // thread loop
        pitch_mutex.lock(); // cruising speed between +/-15.0 degrees
        if (pitch < -15.0) m.speed(0.5 - ((pitch/-75.0 > 1.0 ? 1.0 : pitch/-75.0) * 0.5));
        else if (pitch > 15.0) m.speed(0.5 + ((pitch/45.0 > 1.0 ? 1.0 : pitch/45.0) * 0.5));
        else m.speed(0.5);
        pitch_mutex.unlock();
        Thread::wait(100); // wait 0.1s
    }
}

// Thread 2
// Servo control
void thread2(void const *args)
{
    while(true) {       // thread loop
        for (float pos = 0.0; pos <= 1.0; pos+=0.001){
            myservo = pos;
            Thread::wait(10); // wait 0.01s 
        }
        for (float pos = 1.0; pos >= 0.0; pos-=0.001){
            myservo = pos;
            Thread::wait(10); // wait 0.01s 
        }
        Thread::wait(500); // wait 0.5s   
    }
}

int main() {   
    myservo.calibrate(0.0011, 45.0);   
    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    while(!IMU.accelAvailable());
    IMU.readAccel();
    pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
    float pitch_reading;
    Thread t1(thread1); //start thread1
    Thread t2(thread2); //start thread2
    // IMU reading control loop
    while(true){
        while(!IMU.accelAvailable());
        IMU.readAccel();
        pitch_reading = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
        pitch_mutex.lock();
        pitch = pitch_reading;
        pitch_mutex.unlock();
        Thread::wait(50); // wait 0.05s    
    }
}