Diff: main.cpp

Revision:

0:0bbf2f16da9c

diff -r 000000000000 -r 0bbf2f16da9c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,224 @@
+/*
+TODO: Organize functions n files
+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
+
+
+*/
+
+#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 = 55.0;//60.0;
+    float G_roll_I = 46.0;
+    float G_roll_D = 53.0;
+    float G_pitch_P = 55.0;
+    float G_pitch_I = 46.0;
+    float G_pitch_D = 53.0;
+    float G_yaw_P = 70.0;
+    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("ptich is %f, pitchrate is %f\r\n", pitch, gyros[1]);
+        //pc.printf("roll is %f, rollrate is %f\r\n", roll, gyros[2]);
+        //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)));
+
+            //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;
+            
+            if (!integratestop){
+                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 = 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;
+
+            // Setting motor speeds
+            leftM(power + LRcorr - yawcorr);
+            rightM(power - LRcorr - yawcorr);
+            frontM(power + FRcorr + yawcorr);
+            rearM(power - FRcorr + yawcorr);
+
+            //integrate
+            if (!integratestop){
+                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.0020;
+                break;
+            case 1:
+                commands[1] = data[i] * -0.0020;
+                break;
+            case 2: {
+                float throttle = (unsigned char)data[i];
+                //commands[2] = (char)data[i] * 6.6 - (char)data[i] * (char)data[i] * 2.4; // / 5000.0;
+                commands[2] = throttle * 5.0f - 0.01f * throttle * throttle;
+                //pc.printf("throttle now at %f grams per motor\r\n", commands[2]);
+                if (commands[2] < 250)
+                    integratestop = 1;
+                else
+                    integratestop = 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;
+}
+
+
+
+
+