Oskar Weigl / Mbed 2 deprecated Quadcopter_copy

Very early flyable code.

Dependencies:   mbed RF12B

main.cpp@0:9fcb3bf5c231, 2011-10-01 (annotated)

Committer:
madcowswe
Date:
Sat Oct 01 12:57:23 2011 +0000
Revision:
0:9fcb3bf5c231
This edit is for testing: not flyable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:9fcb3bf5c231 1 /*
madcowswe 0:9fcb3bf5c231 2 TODO: Organize functions n files
madcowswe 0:9fcb3bf5c231 3 TODO: move hardcoded values to defines
madcowswe 0:9fcb3bf5c231 4 TODO: Add Altsensor
madcowswe 0:9fcb3bf5c231 5 TODO: Make yaw from P to PID
madcowswe 0:9fcb3bf5c231 6 TODO: Make radiolink bidirectional
madcowswe 0:9fcb3bf5c231 7
madcowswe 0:9fcb3bf5c231 8 Positive X is forward, positive Y is left, positive Z is up!
madcowswe 0:9fcb3bf5c231 9
madcowswe 0:9fcb3bf5c231 10 Wii motion plus wireing!
madcowswe 0:9fcb3bf5c231 11 blue: gnd
madcowswe 0:9fcb3bf5c231 12 black: also gnd
madcowswe 0:9fcb3bf5c231 13 green: data
madcowswe 0:9fcb3bf5c231 14 brown: 3v3
madcowswe 0:9fcb3bf5c231 15 red: clk
madcowswe 0:9fcb3bf5c231 16
madcowswe 0:9fcb3bf5c231 17 critical gain = NOPOWER / 50
madcowswe 0:9fcb3bf5c231 18 period = 30/11s
madcowswe 0:9fcb3bf5c231 19
madcowswe 0:9fcb3bf5c231 20 */
madcowswe 0:9fcb3bf5c231 21
madcowswe 0:9fcb3bf5c231 22 #include "mbed.h"
madcowswe 0:9fcb3bf5c231 23 #include "GlobalsNDefines.h"
madcowswe 0:9fcb3bf5c231 24 #include <algorithm>
madcowswe 0:9fcb3bf5c231 25 #include "Sensors.h"
madcowswe 0:9fcb3bf5c231 26 #include "motors.h"
madcowswe 0:9fcb3bf5c231 27 #include "System.h"
madcowswe 0:9fcb3bf5c231 28 #include "RF12B.h"
madcowswe 0:9fcb3bf5c231 29 #include "main_init.h"
madcowswe 0:9fcb3bf5c231 30 #include <string>
madcowswe 0:9fcb3bf5c231 31
madcowswe 0:9fcb3bf5c231 32
madcowswe 0:9fcb3bf5c231 33 //Prototypes. TODO: Stick the functions into seperate files.
madcowswe 0:9fcb3bf5c231 34 void readcommands(float commands[5], float oldcommands[5]);
madcowswe 0:9fcb3bf5c231 35
madcowswe 0:9fcb3bf5c231 36 int main() {
madcowswe 0:9fcb3bf5c231 37
madcowswe 0:9fcb3bf5c231 38 //Initialize motors, sensors, etc.
madcowswe 0:9fcb3bf5c231 39 main_init();
madcowswe 0:9fcb3bf5c231 40
madcowswe 0:9fcb3bf5c231 41 //GOGOGO
madcowswe 0:9fcb3bf5c231 42 wait(0.5);
madcowswe 0:9fcb3bf5c231 43 pc.printf("Starting main control loop\r\n");
madcowswe 0:9fcb3bf5c231 44
madcowswe 0:9fcb3bf5c231 45 //Variables belonging to the control loop
madcowswe 0:9fcb3bf5c231 46 float G_roll_P = 35.65;
madcowswe 0:9fcb3bf5c231 47 float G_roll_I = 35.65;
madcowswe 0:9fcb3bf5c231 48 float G_roll_D = 22.28;
madcowswe 0:9fcb3bf5c231 49 float G_pitch_P = 35.65;
madcowswe 0:9fcb3bf5c231 50 float G_pitch_I = 35.65;
madcowswe 0:9fcb3bf5c231 51 float G_pitch_D = 22.28;
madcowswe 0:9fcb3bf5c231 52 float G_yaw_P = 31.20;
madcowswe 0:9fcb3bf5c231 53 float rintegral = 0;
madcowswe 0:9fcb3bf5c231 54 float pintegral = 0;
madcowswe 0:9fcb3bf5c231 55 float yaw = 0;
madcowswe 0:9fcb3bf5c231 56 float pitch = 0;
madcowswe 0:9fcb3bf5c231 57 float roll = 0;
madcowswe 0:9fcb3bf5c231 58 float eroll = 0;
madcowswe 0:9fcb3bf5c231 59 float epitch = 0;
madcowswe 0:9fcb3bf5c231 60 float apitch = 0;
madcowswe 0:9fcb3bf5c231 61 float aroll = 0;
madcowswe 0:9fcb3bf5c231 62 float gyros[3];
madcowswe 0:9fcb3bf5c231 63 float depitch = 0;
madcowswe 0:9fcb3bf5c231 64 float deroll = 0;
madcowswe 0:9fcb3bf5c231 65 float altitude = 0.0; //Meters
madcowswe 0:9fcb3bf5c231 66 float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
madcowswe 0:9fcb3bf5c231 67 float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
madcowswe 0:9fcb3bf5c231 68 signed char axisbuff[3];
madcowswe 0:9fcb3bf5c231 69
madcowswe 0:9fcb3bf5c231 70 //Last thing before entering loop: start the watchdog timer
madcowswe 0:9fcb3bf5c231 71 //watchdog.attach(&wdt, 0.2);
madcowswe 0:9fcb3bf5c231 72
madcowswe 0:9fcb3bf5c231 73 while (true) {
madcowswe 0:9fcb3bf5c231 74
madcowswe 0:9fcb3bf5c231 75 //pc.printf("power is %f\r\n", (float));
madcowswe 0:9fcb3bf5c231 76 pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]);
madcowswe 0:9fcb3bf5c231 77 pc.printf("\r\n");
madcowswe 0:9fcb3bf5c231 78
madcowswe 0:9fcb3bf5c231 79 //1 second inner loop to enable outer loop to print messages every 1s.
madcowswe 0:9fcb3bf5c231 80 //WARNING any delays, including printing, in the outer loop will deteriorate loop performace
madcowswe 0:9fcb3bf5c231 81 for (int looparound = 0; looparound < 100; looparound++) {
madcowswe 0:9fcb3bf5c231 82
madcowswe 0:9fcb3bf5c231 83 /*
madcowswe 0:9fcb3bf5c231 84 //Stop button kill
madcowswe 0:9fcb3bf5c231 85 if (!Nkill) {
madcowswe 0:9fcb3bf5c231 86 kill("Killed by E-stop button");
madcowswe 0:9fcb3bf5c231 87 }
madcowswe 0:9fcb3bf5c231 88 */
madcowswe 0:9fcb3bf5c231 89
madcowswe 0:9fcb3bf5c231 90 /*
madcowswe 0:9fcb3bf5c231 91 //Commands from PC Serial
madcowswe 0:9fcb3bf5c231 92 if (pc.readable())
madcowswe 0:9fcb3bf5c231 93 readcommandstemp(commands, oldcommands);
madcowswe 0:9fcb3bf5c231 94 */
madcowswe 0:9fcb3bf5c231 95
madcowswe 0:9fcb3bf5c231 96 //Commands from RF link
madcowswe 0:9fcb3bf5c231 97 if (radiolink.available() >= 5)
madcowswe 0:9fcb3bf5c231 98 readcommands(commands, oldcommands);
madcowswe 0:9fcb3bf5c231 99
madcowswe 0:9fcb3bf5c231 100 //Get sensor readings
madcowswe 0:9fcb3bf5c231 101 getaccel(axisbuff);
madcowswe 0:9fcb3bf5c231 102 getgyros(gyros);
madcowswe 0:9fcb3bf5c231 103 #ifdef ALTSENSOR
madcowswe 0:9fcb3bf5c231 104 altitude = getaltsensor();
madcowswe 0:9fcb3bf5c231 105 #endif
madcowswe 0:9fcb3bf5c231 106
madcowswe 0:9fcb3bf5c231 107 //One dimensional (small angle aproximation) linear to angular decoding
madcowswe 0:9fcb3bf5c231 108 apitch = 0.05 + atan2(-(float)axisbuff[0], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[1], 2) + pow((float)axisbuff[2], 2)));
madcowswe 0:9fcb3bf5c231 109 aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2)));
madcowswe 0:9fcb3bf5c231 110
madcowswe 0:9fcb3bf5c231 111 //try a different trim (hack) WARNING THIS IS SLOW!
madcowswe 0:9fcb3bf5c231 112 //apitch -= commands[0];
madcowswe 0:9fcb3bf5c231 113 //aroll -= commands[1];
madcowswe 0:9fcb3bf5c231 114
madcowswe 0:9fcb3bf5c231 115 //low pass filter accelero and integrate gyros (note only 1 dimensional)
madcowswe 0:9fcb3bf5c231 116 pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME;
madcowswe 0:9fcb3bf5c231 117 roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME;
madcowswe 0:9fcb3bf5c231 118 yaw += gyros[0] * LOOPTIME;
madcowswe 0:9fcb3bf5c231 119
madcowswe 0:9fcb3bf5c231 120 //Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north)
madcowswe 0:9fcb3bf5c231 121 yaw -= commands[3];
madcowswe 0:9fcb3bf5c231 122
madcowswe 0:9fcb3bf5c231 123 //Errors
madcowswe 0:9fcb3bf5c231 124 epitch = pitch - commands[0];
madcowswe 0:9fcb3bf5c231 125 eroll = roll - commands[1];
madcowswe 0:9fcb3bf5c231 126
madcowswe 0:9fcb3bf5c231 127 //Error derivatives
madcowswe 0:9fcb3bf5c231 128 depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD;
madcowswe 0:9fcb3bf5c231 129 deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD;
madcowswe 0:9fcb3bf5c231 130
madcowswe 0:9fcb3bf5c231 131 /*
madcowswe 0:9fcb3bf5c231 132 //Average power to motors
madcowswe 0:9fcb3bf5c231 133 #ifdef ALTSENSOR
madcowswe 0:9fcb3bf5c231 134 float power = NOPOWER - 0.002 + commands[2] - (altitude * ALTGAIN);
madcowswe 0:9fcb3bf5c231 135 #else
madcowswe 0:9fcb3bf5c231 136 float power = *//*pot / 10*//* NOPOWER - 0.002 + commands[2];
madcowswe 0:9fcb3bf5c231 137 #endif
madcowswe 0:9fcb3bf5c231 138 */
madcowswe 0:9fcb3bf5c231 139
madcowswe 0:9fcb3bf5c231 140 //Average power to motors
madcowswe 0:9fcb3bf5c231 141 #ifdef ALTSENSOR
madcowswe 0:9fcb3bf5c231 142 float power = commands[2] - (altitude * ALTGAIN);
madcowswe 0:9fcb3bf5c231 143 #else
madcowswe 0:9fcb3bf5c231 144 float power = commands[2];
madcowswe 0:9fcb3bf5c231 145 #endif
madcowswe 0:9fcb3bf5c231 146
madcowswe 0:9fcb3bf5c231 147 /*
madcowswe 0:9fcb3bf5c231 148 //For some reason, the min function is not defined for float and float literal. Defining a temp float.
madcowswe 0:9fcb3bf5c231 149 float maxyawwtf = YAWCAP;
madcowswe 0:9fcb3bf5c231 150 SOLVED: use x.xf, regular = double..
madcowswe 0:9fcb3bf5c231 151 */
madcowswe 0:9fcb3bf5c231 152
madcowswe 0:9fcb3bf5c231 153 //R+L corr term
madcowswe 0:9fcb3bf5c231 154 float LRcorr = eroll * G_roll_P
madcowswe 0:9fcb3bf5c231 155 + rintegral * G_roll_I
madcowswe 0:9fcb3bf5c231 156 + deroll * G_roll_D;
madcowswe 0:9fcb3bf5c231 157
madcowswe 0:9fcb3bf5c231 158 //F+R corr term
madcowswe 0:9fcb3bf5c231 159 float FRcorr = epitch * G_pitch_P
madcowswe 0:9fcb3bf5c231 160 + pintegral * G_pitch_I
madcowswe 0:9fcb3bf5c231 161 + depitch * G_pitch_D;
madcowswe 0:9fcb3bf5c231 162
madcowswe 0:9fcb3bf5c231 163 //yaw corr term
madcowswe 0:9fcb3bf5c231 164 float yawcorr = min(yaw, YAWCAP) * G_yaw_P;
madcowswe 0:9fcb3bf5c231 165
madcowswe 0:9fcb3bf5c231 166 //left pid
madcowswe 0:9fcb3bf5c231 167 motormaxled = motormaxled || leftM(power - LRcorr - yawcorr);
madcowswe 0:9fcb3bf5c231 168
madcowswe 0:9fcb3bf5c231 169 //right pid
madcowswe 0:9fcb3bf5c231 170 motormaxled = motormaxled || rightM(power + LRcorr - yawcorr);
madcowswe 0:9fcb3bf5c231 171
madcowswe 0:9fcb3bf5c231 172 //front pid
madcowswe 0:9fcb3bf5c231 173 motormaxled = motormaxled || frontM(power - FRcorr + yawcorr);
madcowswe 0:9fcb3bf5c231 174
madcowswe 0:9fcb3bf5c231 175 //rear pid
madcowswe 0:9fcb3bf5c231 176 motormaxled = motormaxled || rearM(power + FRcorr + yawcorr);
madcowswe 0:9fcb3bf5c231 177
madcowswe 0:9fcb3bf5c231 178
madcowswe 0:9fcb3bf5c231 179 /*
madcowswe 0:9fcb3bf5c231 180 //left pid
madcowswe 0:9fcb3bf5c231 181 left = min(max(power - (
madcowswe 0:9fcb3bf5c231 182
madcowswe 0:9fcb3bf5c231 183 eroll * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 184 + rintegral * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 185 + deroll * (0.5 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 186 ) - (
madcowswe 0:9fcb3bf5c231 187 min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 188
madcowswe 0:9fcb3bf5c231 189 ), 0.060), NOPOWER * 1.5);
madcowswe 0:9fcb3bf5c231 190
madcowswe 0:9fcb3bf5c231 191
madcowswe 0:9fcb3bf5c231 192 //right pid
madcowswe 0:9fcb3bf5c231 193 right = min(max(power + (
madcowswe 0:9fcb3bf5c231 194
madcowswe 0:9fcb3bf5c231 195 eroll * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 196 + rintegral * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 197 + deroll * (0.5 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 198 ) - (
madcowswe 0:9fcb3bf5c231 199 min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 200
madcowswe 0:9fcb3bf5c231 201 ), 0.060), NOPOWER * 1.5);
madcowswe 0:9fcb3bf5c231 202
madcowswe 0:9fcb3bf5c231 203
madcowswe 0:9fcb3bf5c231 204 //front pid
madcowswe 0:9fcb3bf5c231 205 front = min(max(power - (
madcowswe 0:9fcb3bf5c231 206
madcowswe 0:9fcb3bf5c231 207 epitch * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 208 + pintegral * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 209 + depitch * (0.5 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 210 ) + (
madcowswe 0:9fcb3bf5c231 211 min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 212
madcowswe 0:9fcb3bf5c231 213 ), 0.060), NOPOWER * 1.5);
madcowswe 0:9fcb3bf5c231 214
madcowswe 0:9fcb3bf5c231 215
madcowswe 0:9fcb3bf5c231 216 //rear pid
madcowswe 0:9fcb3bf5c231 217 rear = min(max(power + (
madcowswe 0:9fcb3bf5c231 218
madcowswe 0:9fcb3bf5c231 219 epitch * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 220 + pintegral * (0.8 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 221 + depitch * (0.5 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 222 ) + (
madcowswe 0:9fcb3bf5c231 223 min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
madcowswe 0:9fcb3bf5c231 224
madcowswe 0:9fcb3bf5c231 225 ), 0.060), NOPOWER * 1.5);
madcowswe 0:9fcb3bf5c231 226
madcowswe 0:9fcb3bf5c231 227 */
madcowswe 0:9fcb3bf5c231 228
madcowswe 0:9fcb3bf5c231 229 //integrate
madcowswe 0:9fcb3bf5c231 230 rintegral += eroll * LOOPTIME;
madcowswe 0:9fcb3bf5c231 231 pintegral += epitch * LOOPTIME;
madcowswe 0:9fcb3bf5c231 232
madcowswe 0:9fcb3bf5c231 233 //pet the dog
madcowswe 0:9fcb3bf5c231 234 loopalive = 1;
madcowswe 0:9fcb3bf5c231 235
madcowswe 0:9fcb3bf5c231 236 wait(LOOPTIME);
madcowswe 0:9fcb3bf5c231 237 }
madcowswe 0:9fcb3bf5c231 238 }
madcowswe 0:9fcb3bf5c231 239 }
madcowswe 0:9fcb3bf5c231 240
madcowswe 0:9fcb3bf5c231 241 void readcommands(float commands[5], float oldcommands[5]) {
madcowswe 0:9fcb3bf5c231 242 //pc.printf("We try to read commands\r\n");
madcowswe 0:9fcb3bf5c231 243 signed char data[5];
madcowswe 0:9fcb3bf5c231 244 radiolink.read((unsigned char*)data, 5);
madcowswe 0:9fcb3bf5c231 245
madcowswe 0:9fcb3bf5c231 246 for (int i = 0; i < 5; i++) {
madcowswe 0:9fcb3bf5c231 247 oldcommands[i] = commands[i];
madcowswe 0:9fcb3bf5c231 248
madcowswe 0:9fcb3bf5c231 249 switch (i) {
madcowswe 0:9fcb3bf5c231 250 case 0:
madcowswe 0:9fcb3bf5c231 251 commands[0] = data[i] * -0.0014;
madcowswe 0:9fcb3bf5c231 252 break;
madcowswe 0:9fcb3bf5c231 253 case 1:
madcowswe 0:9fcb3bf5c231 254 commands[1] = data[i] * 0.0014;
madcowswe 0:9fcb3bf5c231 255 break;
madcowswe 0:9fcb3bf5c231 256 case 2:
madcowswe 0:9fcb3bf5c231 257 commands[2] = data[i] * 2.8; // / 5000.0;
madcowswe 0:9fcb3bf5c231 258 /*if (data[i] < -100) {
madcowswe 0:9fcb3bf5c231 259 kill("Killed by throttle low position");
madcowswe 0:9fcb3bf5c231 260 commands[4] = 1.0;
madcowswe 0:9fcb3bf5c231 261 }*/
madcowswe 0:9fcb3bf5c231 262 break;
madcowswe 0:9fcb3bf5c231 263 case 3:
madcowswe 0:9fcb3bf5c231 264 commands[3] = data[i] * 0.0005;
madcowswe 0:9fcb3bf5c231 265 break;
madcowswe 0:9fcb3bf5c231 266 case 4:
madcowswe 0:9fcb3bf5c231 267 commands[4] = max((float)data[i], commands[4]);
madcowswe 0:9fcb3bf5c231 268 if (commands[4] > 0.1) {
madcowswe 0:9fcb3bf5c231 269 //pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]);
madcowswe 0:9fcb3bf5c231 270 kill("Killed by controller E-stop");
madcowswe 0:9fcb3bf5c231 271 }
madcowswe 0:9fcb3bf5c231 272 break;
madcowswe 0:9fcb3bf5c231 273 }
madcowswe 0:9fcb3bf5c231 274 }
madcowswe 0:9fcb3bf5c231 275
madcowswe 0:9fcb3bf5c231 276 commandsalive = 1;
madcowswe 0:9fcb3bf5c231 277 }
madcowswe 0:9fcb3bf5c231 278
madcowswe 0:9fcb3bf5c231 279
madcowswe 0:9fcb3bf5c231 280
madcowswe 0:9fcb3bf5c231 281
madcowswe 0:9fcb3bf5c231 282