File content as of revision 0:9fcb3bf5c231:

/*
TODO: Organize functions n files
TODO: move hardcoded values to defines
TODO: Add Altsensor
TODO: Make yaw from P to PID
TODO: Make radiolink bidirectional

Positive X is forward, positive Y is left, positive Z is up!

Wii motion plus wireing!
blue: gnd
black: also gnd
green: data
brown: 3v3
red: clk

critical gain = NOPOWER / 50
period = 30/11s

*/

#include "mbed.h"
#include "GlobalsNDefines.h"
#include <algorithm>
#include "Sensors.h"
#include "motors.h"
#include "System.h"
#include "RF12B.h"
#include "main_init.h"
#include <string>


//Prototypes. TODO: Stick the functions into seperate files.
void readcommands(float commands[5], float oldcommands[5]);

int main() {

    //Initialize motors, sensors, etc.
    main_init();

    //GOGOGO
    wait(0.5);
    pc.printf("Starting main control loop\r\n");

    //Variables belonging to the control loop
    float G_roll_P = 35.65;
    float G_roll_I = 35.65;
    float G_roll_D = 22.28;
    float G_pitch_P = 35.65;
    float G_pitch_I = 35.65;
    float G_pitch_D = 22.28;
    float G_yaw_P = 31.20;
    float rintegral = 0;
    float pintegral = 0;
    float yaw = 0;
    float pitch = 0;
    float roll = 0;
    float eroll = 0;
    float epitch = 0;
    float apitch = 0;
    float aroll = 0;
    float gyros[3];
    float depitch = 0;
    float deroll = 0;
    float altitude = 0.0; //Meters
    float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    signed char axisbuff[3];

    //Last thing before entering loop: start the watchdog timer
    //watchdog.attach(&wdt, 0.2);

    while (true) {

        //pc.printf("power is %f\r\n", (float));
        pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]);
        pc.printf("\r\n");

        //1 second inner loop to enable outer loop to print messages every 1s.
        //WARNING any delays, including printing, in the outer loop will deteriorate loop performace
        for (int looparound = 0; looparound < 100; looparound++) {

            /*
            //Stop button kill
            if (!Nkill) {
                kill("Killed by E-stop button");
            }
            */

            /*
            //Commands from PC Serial
            if (pc.readable())
                readcommandstemp(commands, oldcommands);
            */

            //Commands from RF link
            if (radiolink.available() >= 5)
                readcommands(commands, oldcommands);

            //Get sensor readings
            getaccel(axisbuff);
            getgyros(gyros);
#ifdef ALTSENSOR
            altitude = getaltsensor();
#endif

            //One dimensional (small angle aproximation) linear to angular decoding
            apitch = 0.05 + atan2(-(float)axisbuff[0], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[1], 2) + pow((float)axisbuff[2], 2)));
            aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2)));

            //try a different trim (hack) WARNING THIS IS SLOW!
            //apitch -= commands[0];
            //aroll -= commands[1];

            //low pass filter accelero and integrate gyros (note only 1 dimensional)
            pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME;
            roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME;
            yaw += gyros[0] * LOOPTIME;

            //Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north)
            yaw -= commands[3];

            //Errors
            epitch = pitch - commands[0];
            eroll = roll - commands[1];

            //Error derivatives
            depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD;
            deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD;

            /*
            //Average power to motors
            #ifdef ALTSENSOR
            float power = NOPOWER - 0.002 + commands[2] - (altitude * ALTGAIN);
            #else
            float power = *//*pot / 10*//* NOPOWER - 0.002 + commands[2];
#endif
*/

            //Average power to motors
#ifdef ALTSENSOR
            float power = commands[2] - (altitude * ALTGAIN);
#else
            float power = commands[2];
#endif

            /*
            //For some reason, the min function is not defined for float and float literal. Defining a temp float.
            float maxyawwtf = YAWCAP;
            SOLVED: use x.xf, regular = double..
            */

            //R+L corr term
            float LRcorr = eroll * G_roll_P
                           + rintegral * G_roll_I
                           + deroll * G_roll_D;

            //F+R corr term
            float FRcorr = epitch * G_pitch_P
                           + pintegral * G_pitch_I
                           + depitch * G_pitch_D;

            //yaw corr term
            float yawcorr = min(yaw, YAWCAP) * G_yaw_P;

            //left pid
            motormaxled = motormaxled || leftM(power - LRcorr - yawcorr);

            //right pid
            motormaxled = motormaxled || rightM(power + LRcorr - yawcorr);

            //front pid
            motormaxled = motormaxled || frontM(power - FRcorr + yawcorr);

            //rear pid
            motormaxled = motormaxled || rearM(power + FRcorr + yawcorr);


            /*
            //left pid
            left = min(max(power - (

                               eroll * (0.8 * NOPOWER / 25)
                               + rintegral * (0.8 * NOPOWER / 25)
                               + deroll * (0.5 * NOPOWER / 25)
                           ) - (
                               min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)

                           ), 0.060), NOPOWER * 1.5);


            //right pid
            right = min(max(power + (

                                eroll * (0.8 * NOPOWER / 25)
                                + rintegral * (0.8 * NOPOWER / 25)
                                + deroll * (0.5 * NOPOWER / 25)
                            ) - (
                                min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)

                            ), 0.060), NOPOWER * 1.5);


            //front pid
            front = min(max(power - (

                                epitch * (0.8 * NOPOWER / 25)
                                + pintegral * (0.8 * NOPOWER / 25)
                                + depitch * (0.5 * NOPOWER / 25)
                            ) + (
                                min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)

                            ), 0.060), NOPOWER * 1.5);


            //rear pid
            rear = min(max(power + (

                               epitch * (0.8 * NOPOWER / 25)
                               + pintegral * (0.8 * NOPOWER / 25)
                               + depitch * (0.5 * NOPOWER / 25)
                           ) + (
                               min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)

                           ), 0.060), NOPOWER * 1.5);

            */

            //integrate
            rintegral += eroll * LOOPTIME;
            pintegral += epitch * LOOPTIME;

            //pet the dog
            loopalive = 1;

            wait(LOOPTIME);
        }
    }
}

void readcommands(float commands[5], float oldcommands[5]) {
    //pc.printf("We try to read commands\r\n");
    signed char data[5];
    radiolink.read((unsigned char*)data, 5);

    for (int i = 0; i < 5; i++) {
        oldcommands[i] = commands[i];

        switch (i) {
            case 0:
                commands[0] = data[i] * -0.0014;
                break;
            case 1:
                commands[1] = data[i] * 0.0014;
                break;
            case 2:
                commands[2] = data[i] * 2.8; // / 5000.0;
                /*if (data[i] < -100) {
                    kill("Killed by throttle low position");
                    commands[4] = 1.0;
                }*/
                break;
            case 3:
                commands[3] = data[i] * 0.0005;
                break;
            case 4:
                commands[4] = max((float)data[i], commands[4]);
                if (commands[4] > 0.1) {
                    //pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]);
                    kill("Killed by controller E-stop");
                }
                break;
        }
    }

    commandsalive = 1;
}