File content as of revision 18:a00d6b065c6b:

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

Serial pc(USBTX, USBRX);
MRF24J40 mrf(p11, p12, p13, p14, p21);
Quadcopter myQuadcopter(&pc, &mrf); // 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(p23);
PwmOut motor_2(p24);
PwmOut motor_3(p25);
PwmOut motor_4(p26);


void controller_thread(void const *args)
{
    while(true) {
        myQuadcopter.controller();
        motors motorsPwm=myQuadcopter.getPwm();

        motor_2=motorsPwm.m2;
        motor_4=motorsPwm.m4;
        
        //pc.printf("m1: %f m2: %f \n\r",  motorsPwm.m2, motorsPwm.m4);

        Thread::wait(10);
    }
}

void rc_thread(void const *args)
{
    while(true) {
        myQuadcopter.readRc();
        Thread::wait(50);  // wait for some number of miliseconds
    }
}


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();
        //pc.printf("%lld: %f,%f,%f,%f\r\n", myQuadcopter.id, myQuadcopter.thrust, myQuadcopter.yaw, myQuadcopter.pitch, myQuadcopter.roll);

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