Oskar Weigl
Very early flyable code.
Diff: main.cpp
- Revision:
- 0:9fcb3bf5c231
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,282 @@
+/*
+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;
+}
+
+
+
+
+