File content as of revision 6:a4d6f3e4bf28:

#include "mbed.h"
#include "motor_controller.h"
#include "guardian.h"
#include "IMU.h"
#include "Servo.h"
#include "rtos.h"
#include "PwmReader.h"

#define THROTTLE_MAX 0.4
#define THROTTLE_MIN 0.2
#define FREQUENCY_MAX 0.4
#define FREQUENCY_MIN 0.2
#define RUDDER_MAX 0.4
#define RUDDER_MIN 0.2

bool quit=false;

PololuMController mcon(p22, p6, p5);
Servo servo(p21);
Guardian ap(p21, p23, p24, p25, p26, p26);
Serial xbee(p13, p14);
Serial pc(USBTX, USBRX);

float throttle, frequency, rudder;

//IMU imu(0.1, 0.3, 0.005, 0.005);
void motor_thread(void const *args) {
    
    Timer t;
    t.start();
    PwmReader thrDutyCycl(p7, THROTTLE_MIN, THROTTLE_MAX);
    PwmReader freqDutyCycl(p8, FREQUENCY_MIN, FREQUENCY_MAX);   
    PwmReader rudDutyCycl(p9, RUDDER_MIN, RUDDER_MAX);
    
    while (quit==false) {     
        throttle = thrDutyCycl.getDuty();
        frequency = freqDutyCycl.getDuty();
        rudder = rudDutyCycl.getDuty();
        
        
        mcon.drive_sinusoidal(t.read(), throttle, frequency); // (time,amplitude,frequency,freqoffset)
        Thread::wait(5);
    }
    t.stop();
}

int main()
{
    Thread thread(motor_thread);
    //ap.calibrate();
    //ap.set2D();
    ap.setoff();
        while(1) {
        //char buf[128];
//        if (xbee.readable())
//        {
//            xbee.gets(buf, 128);
//            //xbee.scanf("%d");
//            pc.puts(buf);
//        }
//        memset(buf, 0, 128);
         
         //print throttle and frequency to the terminal
          pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder);
               
          Thread::wait(500);
    }
}