File content as of revision 22:92401a4fec13:

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

// to read the battery voltage
AnalogIn battery(p20);

void controller_thread(void const *args)
{
    while(true) {
        //myQuadcopter.readSensorValues();

        myQuadcopter.controller();
        motors motorsPwm=myQuadcopter.getPwm();

        motor_1=motorsPwm.m1;
        motor_2=motorsPwm.m2;
        motor_3=motorsPwm.m3;
        motor_4=motorsPwm.m4;

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

        Thread::wait(10);
    }
}

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

void battery_thread(void const *args)
{
    float threshold_voltage = 13.0; // desired lowest battery voltage
    float max_voltage = 14.8; // max voltage level of battery
    float saturating_voltage = 18.38; // voltage at which ADC == 1
    float max_adc = 0.80522; // when battery is at 14.8V
    float threshold_adc = max_adc * threshold_voltage / max_voltage;
    while(true) {
        if (battery.read() < threshold_adc) {
            printf("low battery! %f\r\n", battery.read() * saturating_voltage);
        }
        Thread::wait(1000);  // 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.01);          // motor requires a 2ms period
    motor_2.period(0.01);          // motor requires a 2ms period
    motor_3.period(0.01);          // motor requires a 2ms period
    motor_4.period(0.01);          // motor requires a 2ms period

    //pc.printf("Pestingt.\n\r");




    // 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.1;
    motor_2 = 0.1;
    motor_3 = 0.1;
    motor_4 = 0.1;

    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("Outputting duty cycle specified below now press all but b to abort.\n\r");
             motor_1 = 0.07;


     char c = pc.getc();
     if (c!='c') {
         pc.printf("Aborting");
         return 0;
     };
     */


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