File content as of revision 14:64b06476d943:

#include "mbed.h"
#include "rtos.h"
#define _MBED_
//#include "controller.h"
#include "sensor.h"
#include "quadcopter.h"

Serial pc(USBTX, USBRX);
Quadcopter myQuadcopter(&pc); // instantiate Quadcopter object

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

void controller_thread(void const *args) {
  while(true){
  myQuadcopter.controller();
  Thread::wait(10);
  }
}

void rc_thread(void const *args) {
  while(true){
      myQuadcopter.readRc();
      Thread::wait(100);
  }
}


int main() {
    //Thread thread(controller_thread);

    // 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;
    };
    
    //
    Thread threadR(rc_thread);

    //check if remote controll is working with if statement
    
    Thread threadC(controller_thread);

    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("m1: %f m2: %f \n\r",  motorsPwm.m2, motorsPwm.m4);
        //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);
    }
}