File content as of revision 13:291ba30c7806:

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

#include "quadcopter.h"

// initialze serial connection
Serial pc(USBTX, USBRX);
Serial *pcPntr=&pc; // pointer used to enable serial connection with print method in Quadcopter class

// pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
PwmOut motor_1(p21);
PwmOut motor_2(p22);
PwmOut motor_3(p23);
PwmOut motor_4(p24);

int main()
{
    Quadcopter myQuadcopter; // initialze Quadcopter object
    myQuadcopter.setSerial(pcPntr); // give Quadcopter object pc Pointer in order to print
    myQuadcopter.initAllSensors(); // initialize sensors

// get desired values from joystick (to be implemented)
// myQuadcopter.setDes(...)

    motor_1.period(0.002);          // motor requires a 2ms period
    motor_2.period(0.002);          // motor requires a 2ms period
    motor_3.period(0.002);          // motor requires a 2ms period
    motor_4.period(0.002);          // motor requires a 2ms period

    // startup procedure
    pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
    char a= pc.getc();
    if (a!='s') {
        pc.printf("Aborting");
        return 0;
    };

    // Duty cycle at beginning must be  50%
    pc.printf("Starting up ESCs\n\r");
    motor_1 = 0.5;
    motor_2 = 0.5;
    motor_3 = 0.5;
    motor_4 = 0.5;

    pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
    char b= pc.getc();
    if (b!='c') {
        pc.printf("Aborting");
        return 0;
    };

    pc.printf("Entering control loop\n\r");

    while (1) {
        myQuadcopter.readSensorValues();

        myQuadcopter.controller();


        wait(0.01);

        // Set duty cycle
        motors motorsPwm=myQuadcopter.getPwm();

        motor_2=motorsPwm.m2;
        motor_4=motorsPwm.m4;

        //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);
    }
}