File content as of revision 29:ae765492fa8b:

#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);
DigitalOut batteryLed(LED1);

int emergencyOff = 0;

void controller_thread(void const *args)
{
    while(emergencyOff != 1) {
        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);
    }
}

void rc_thread(void const *args)
{
    while(true) {
        myQuadcopter.readRc();
    }
}

void battery_thread(void const *args)
{
    float threshold_voltage = 13.0; // desired lowest battery voltage
    float emergencyVoltage = 12.5; // switch off motors below it
    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;
    float emergency_adc = max_adc * emergencyVoltage / max_voltage;

    while(true) {
        if (battery.read() < threshold_adc) {
            printf("low battery! %f\r\n", battery.read() * saturating_voltage);
            batteryLed = 1;
            if (battery.read() < emergency_adc) {
                emergencyOff = 1;
                motor_1 = 0.1;
                motor_2 = 0.1;
                motor_3 = 0.1;
                motor_4 = 0.1;
            }
        }
        Thread::wait(1000);  // wait for some number of miliseconds
    }
}



int main()
{
    // ESCs requires a 100Hz frequency
    motor_1.period(0.01);
    motor_2.period(0.01);
    motor_3.period(0.01);
    motor_4.period(0.01);

    Thread threadR(rc_thread);
    double forceThreshold = -0.3;
    double forceRc = myQuadcopter.getForce();

    // TODO figure out why forceRc occasionally goes below the threshold without input
    while (forceRc > forceThreshold) { // wait until Joystick is in starting position
        forceRc = myQuadcopter.getForce();
        wait(1);
    }

    motor_1 = 0.1;
    motor_2 = 0.1;
    motor_3 = 0.1;
    motor_4 = 0.1;

    wait(2); // hold startup duty cycle for 2 seconds

    // TODO assign priorities to threads, test if it really works as we expect
    Thread battery(battery_thread);
    Thread thread(controller_thread);

    // TODO is this needed?
    while (1);
}