Oskar Weigl
/
Quadcopterv2
This is for ICRS\' second generation Quadcopter
main.cpp@0:0bbf2f16da9c, 2011-11-18 (annotated)
- Committer:
- madcowswe
- Date:
- Fri Nov 18 18:23:33 2011 +0000
- Revision:
- 0:0bbf2f16da9c
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
madcowswe | 0:0bbf2f16da9c | 1 | /* |
madcowswe | 0:0bbf2f16da9c | 2 | TODO: Organize functions n files |
madcowswe | 0:0bbf2f16da9c | 3 | TODO: Add Altsensor |
madcowswe | 0:0bbf2f16da9c | 4 | TODO: Make yaw from P to PID |
madcowswe | 0:0bbf2f16da9c | 5 | TODO: Make radiolink bidirectional |
madcowswe | 0:0bbf2f16da9c | 6 | |
madcowswe | 0:0bbf2f16da9c | 7 | Positive X is forward, positive Y is left, positive Z is up! |
madcowswe | 0:0bbf2f16da9c | 8 | |
madcowswe | 0:0bbf2f16da9c | 9 | Wii motion plus wireing! |
madcowswe | 0:0bbf2f16da9c | 10 | blue: gnd |
madcowswe | 0:0bbf2f16da9c | 11 | black: also gnd |
madcowswe | 0:0bbf2f16da9c | 12 | green: data |
madcowswe | 0:0bbf2f16da9c | 13 | brown: 3v3 |
madcowswe | 0:0bbf2f16da9c | 14 | red: clk |
madcowswe | 0:0bbf2f16da9c | 15 | |
madcowswe | 0:0bbf2f16da9c | 16 | |
madcowswe | 0:0bbf2f16da9c | 17 | */ |
madcowswe | 0:0bbf2f16da9c | 18 | |
madcowswe | 0:0bbf2f16da9c | 19 | #include "mbed.h" |
madcowswe | 0:0bbf2f16da9c | 20 | #include "GlobalsNDefines.h" |
madcowswe | 0:0bbf2f16da9c | 21 | #include <algorithm> |
madcowswe | 0:0bbf2f16da9c | 22 | #include "Sensors.h" |
madcowswe | 0:0bbf2f16da9c | 23 | #include "motors.h" |
madcowswe | 0:0bbf2f16da9c | 24 | #include "System.h" |
madcowswe | 0:0bbf2f16da9c | 25 | #include "RF12B.h" |
madcowswe | 0:0bbf2f16da9c | 26 | #include "main_init.h" |
madcowswe | 0:0bbf2f16da9c | 27 | #include <string> |
madcowswe | 0:0bbf2f16da9c | 28 | |
madcowswe | 0:0bbf2f16da9c | 29 | |
madcowswe | 0:0bbf2f16da9c | 30 | //Prototypes. TODO: Stick the functions into seperate files. |
madcowswe | 0:0bbf2f16da9c | 31 | void readcommands(float commands[5], float oldcommands[5]); |
madcowswe | 0:0bbf2f16da9c | 32 | |
madcowswe | 0:0bbf2f16da9c | 33 | int main() { |
madcowswe | 0:0bbf2f16da9c | 34 | |
madcowswe | 0:0bbf2f16da9c | 35 | //Initialize motors, sensors, etc. |
madcowswe | 0:0bbf2f16da9c | 36 | main_init(); |
madcowswe | 0:0bbf2f16da9c | 37 | |
madcowswe | 0:0bbf2f16da9c | 38 | //GOGOGO |
madcowswe | 0:0bbf2f16da9c | 39 | wait(0.5); |
madcowswe | 0:0bbf2f16da9c | 40 | pc.printf("Starting main control loop\r\n"); |
madcowswe | 0:0bbf2f16da9c | 41 | |
madcowswe | 0:0bbf2f16da9c | 42 | //Variables belonging to the control loop |
madcowswe | 0:0bbf2f16da9c | 43 | float G_roll_P = 55.0;//60.0; |
madcowswe | 0:0bbf2f16da9c | 44 | float G_roll_I = 46.0; |
madcowswe | 0:0bbf2f16da9c | 45 | float G_roll_D = 53.0; |
madcowswe | 0:0bbf2f16da9c | 46 | float G_pitch_P = 55.0; |
madcowswe | 0:0bbf2f16da9c | 47 | float G_pitch_I = 46.0; |
madcowswe | 0:0bbf2f16da9c | 48 | float G_pitch_D = 53.0; |
madcowswe | 0:0bbf2f16da9c | 49 | float G_yaw_P = 70.0; |
madcowswe | 0:0bbf2f16da9c | 50 | float rintegral = 0; |
madcowswe | 0:0bbf2f16da9c | 51 | float pintegral = 0; |
madcowswe | 0:0bbf2f16da9c | 52 | float yaw = 0; |
madcowswe | 0:0bbf2f16da9c | 53 | float pitch = 0; |
madcowswe | 0:0bbf2f16da9c | 54 | float roll = 0; |
madcowswe | 0:0bbf2f16da9c | 55 | float eroll = 0; |
madcowswe | 0:0bbf2f16da9c | 56 | float epitch = 0; |
madcowswe | 0:0bbf2f16da9c | 57 | float apitch = 0; |
madcowswe | 0:0bbf2f16da9c | 58 | float aroll = 0; |
madcowswe | 0:0bbf2f16da9c | 59 | float gyros[3]; |
madcowswe | 0:0bbf2f16da9c | 60 | float depitch = 0; |
madcowswe | 0:0bbf2f16da9c | 61 | float deroll = 0; |
madcowswe | 0:0bbf2f16da9c | 62 | float altitude = 0.0; //Meters |
madcowswe | 0:0bbf2f16da9c | 63 | float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0}; |
madcowswe | 0:0bbf2f16da9c | 64 | float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0}; |
madcowswe | 0:0bbf2f16da9c | 65 | signed char axisbuff[3]; |
madcowswe | 0:0bbf2f16da9c | 66 | |
madcowswe | 0:0bbf2f16da9c | 67 | //Last thing before entering loop: start the watchdog timer |
madcowswe | 0:0bbf2f16da9c | 68 | watchdog.attach(&wdt, 0.2); |
madcowswe | 0:0bbf2f16da9c | 69 | |
madcowswe | 0:0bbf2f16da9c | 70 | while (true) { |
madcowswe | 0:0bbf2f16da9c | 71 | |
madcowswe | 0:0bbf2f16da9c | 72 | //pc.printf("power is %f\r\n", (float)); |
madcowswe | 0:0bbf2f16da9c | 73 | //pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]); |
madcowswe | 0:0bbf2f16da9c | 74 | //pc.printf("ptich is %f, pitchrate is %f\r\n", pitch, gyros[1]); |
madcowswe | 0:0bbf2f16da9c | 75 | //pc.printf("roll is %f, rollrate is %f\r\n", roll, gyros[2]); |
madcowswe | 0:0bbf2f16da9c | 76 | //pc.printf("\r\n"); |
madcowswe | 0:0bbf2f16da9c | 77 | |
madcowswe | 0:0bbf2f16da9c | 78 | //1 second inner loop to enable outer loop to print messages every 1s. |
madcowswe | 0:0bbf2f16da9c | 79 | //WARNING any delays, including printing, in the outer loop will deteriorate loop performace |
madcowswe | 0:0bbf2f16da9c | 80 | for (int looparound = 0; looparound < 100; looparound++) { |
madcowswe | 0:0bbf2f16da9c | 81 | |
madcowswe | 0:0bbf2f16da9c | 82 | /* |
madcowswe | 0:0bbf2f16da9c | 83 | //Stop button kill |
madcowswe | 0:0bbf2f16da9c | 84 | if (!Nkill) { |
madcowswe | 0:0bbf2f16da9c | 85 | kill("Killed by E-stop button"); |
madcowswe | 0:0bbf2f16da9c | 86 | } |
madcowswe | 0:0bbf2f16da9c | 87 | */ |
madcowswe | 0:0bbf2f16da9c | 88 | |
madcowswe | 0:0bbf2f16da9c | 89 | /* |
madcowswe | 0:0bbf2f16da9c | 90 | //Commands from PC Serial |
madcowswe | 0:0bbf2f16da9c | 91 | if (pc.readable()) |
madcowswe | 0:0bbf2f16da9c | 92 | readcommandstemp(commands, oldcommands); |
madcowswe | 0:0bbf2f16da9c | 93 | */ |
madcowswe | 0:0bbf2f16da9c | 94 | |
madcowswe | 0:0bbf2f16da9c | 95 | //Commands from RF link |
madcowswe | 0:0bbf2f16da9c | 96 | if (radiolink.available() >= 5) |
madcowswe | 0:0bbf2f16da9c | 97 | readcommands(commands, oldcommands); |
madcowswe | 0:0bbf2f16da9c | 98 | |
madcowswe | 0:0bbf2f16da9c | 99 | //Get sensor readings |
madcowswe | 0:0bbf2f16da9c | 100 | getaccel(axisbuff); |
madcowswe | 0:0bbf2f16da9c | 101 | getgyros(gyros); |
madcowswe | 0:0bbf2f16da9c | 102 | #ifdef ALTSENSOR |
madcowswe | 0:0bbf2f16da9c | 103 | altitude = getaltsensor(); |
madcowswe | 0:0bbf2f16da9c | 104 | #endif |
madcowswe | 0:0bbf2f16da9c | 105 | |
madcowswe | 0:0bbf2f16da9c | 106 | //One dimensional (small angle aproximation) linear to angular decoding |
madcowswe | 0:0bbf2f16da9c | 107 | 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:0bbf2f16da9c | 108 | aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2))); |
madcowswe | 0:0bbf2f16da9c | 109 | |
madcowswe | 0:0bbf2f16da9c | 110 | //low pass filter accelero and integrate gyros (note only 1 dimensional) |
madcowswe | 0:0bbf2f16da9c | 111 | pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME; |
madcowswe | 0:0bbf2f16da9c | 112 | roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME; |
madcowswe | 0:0bbf2f16da9c | 113 | |
madcowswe | 0:0bbf2f16da9c | 114 | if (!integratestop){ |
madcowswe | 0:0bbf2f16da9c | 115 | yaw += gyros[0] * LOOPTIME; |
madcowswe | 0:0bbf2f16da9c | 116 | |
madcowswe | 0:0bbf2f16da9c | 117 | //Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north) |
madcowswe | 0:0bbf2f16da9c | 118 | yaw -= commands[3]; |
madcowswe | 0:0bbf2f16da9c | 119 | } |
madcowswe | 0:0bbf2f16da9c | 120 | |
madcowswe | 0:0bbf2f16da9c | 121 | //Errors |
madcowswe | 0:0bbf2f16da9c | 122 | epitch = pitch - commands[0]; |
madcowswe | 0:0bbf2f16da9c | 123 | eroll = roll - commands[1]; |
madcowswe | 0:0bbf2f16da9c | 124 | |
madcowswe | 0:0bbf2f16da9c | 125 | //Error derivatives |
madcowswe | 0:0bbf2f16da9c | 126 | depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD; |
madcowswe | 0:0bbf2f16da9c | 127 | deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD; |
madcowswe | 0:0bbf2f16da9c | 128 | |
madcowswe | 0:0bbf2f16da9c | 129 | //Average power to motors |
madcowswe | 0:0bbf2f16da9c | 130 | #ifdef ALTSENSOR |
madcowswe | 0:0bbf2f16da9c | 131 | float power = commands[2] - (altitude * ALTGAIN); |
madcowswe | 0:0bbf2f16da9c | 132 | #else |
madcowswe | 0:0bbf2f16da9c | 133 | float power = commands[2]; |
madcowswe | 0:0bbf2f16da9c | 134 | #endif |
madcowswe | 0:0bbf2f16da9c | 135 | |
madcowswe | 0:0bbf2f16da9c | 136 | /* |
madcowswe | 0:0bbf2f16da9c | 137 | //For some reason, the min function is not defined for float and float literal. Defining a temp float. |
madcowswe | 0:0bbf2f16da9c | 138 | float maxyawwtf = YAWCAP; |
madcowswe | 0:0bbf2f16da9c | 139 | SOLVED: use x.xf, regular = double.. |
madcowswe | 0:0bbf2f16da9c | 140 | */ |
madcowswe | 0:0bbf2f16da9c | 141 | |
madcowswe | 0:0bbf2f16da9c | 142 | //R+L corr term |
madcowswe | 0:0bbf2f16da9c | 143 | float LRcorr = eroll * G_roll_P |
madcowswe | 0:0bbf2f16da9c | 144 | + rintegral * G_roll_I |
madcowswe | 0:0bbf2f16da9c | 145 | + deroll * G_roll_D; |
madcowswe | 0:0bbf2f16da9c | 146 | |
madcowswe | 0:0bbf2f16da9c | 147 | //F+R corr term |
madcowswe | 0:0bbf2f16da9c | 148 | float FRcorr = epitch * G_pitch_P |
madcowswe | 0:0bbf2f16da9c | 149 | + pintegral * G_pitch_I |
madcowswe | 0:0bbf2f16da9c | 150 | + depitch * G_pitch_D; |
madcowswe | 0:0bbf2f16da9c | 151 | |
madcowswe | 0:0bbf2f16da9c | 152 | //yaw corr term |
madcowswe | 0:0bbf2f16da9c | 153 | float yawcorr = min(yaw, YAWCAP) * G_yaw_P; |
madcowswe | 0:0bbf2f16da9c | 154 | |
madcowswe | 0:0bbf2f16da9c | 155 | // Setting motor speeds |
madcowswe | 0:0bbf2f16da9c | 156 | leftM(power + LRcorr - yawcorr); |
madcowswe | 0:0bbf2f16da9c | 157 | rightM(power - LRcorr - yawcorr); |
madcowswe | 0:0bbf2f16da9c | 158 | frontM(power + FRcorr + yawcorr); |
madcowswe | 0:0bbf2f16da9c | 159 | rearM(power - FRcorr + yawcorr); |
madcowswe | 0:0bbf2f16da9c | 160 | |
madcowswe | 0:0bbf2f16da9c | 161 | //integrate |
madcowswe | 0:0bbf2f16da9c | 162 | if (!integratestop){ |
madcowswe | 0:0bbf2f16da9c | 163 | rintegral += eroll * LOOPTIME; |
madcowswe | 0:0bbf2f16da9c | 164 | pintegral += epitch * LOOPTIME; |
madcowswe | 0:0bbf2f16da9c | 165 | } |
madcowswe | 0:0bbf2f16da9c | 166 | |
madcowswe | 0:0bbf2f16da9c | 167 | //pet the dog |
madcowswe | 0:0bbf2f16da9c | 168 | loopalive = 1; |
madcowswe | 0:0bbf2f16da9c | 169 | |
madcowswe | 0:0bbf2f16da9c | 170 | wait(LOOPTIME); |
madcowswe | 0:0bbf2f16da9c | 171 | } |
madcowswe | 0:0bbf2f16da9c | 172 | } |
madcowswe | 0:0bbf2f16da9c | 173 | } |
madcowswe | 0:0bbf2f16da9c | 174 | |
madcowswe | 0:0bbf2f16da9c | 175 | void readcommands(float commands[5], float oldcommands[5]) { |
madcowswe | 0:0bbf2f16da9c | 176 | //pc.printf("We try to read commands\r\n"); |
madcowswe | 0:0bbf2f16da9c | 177 | signed char data[5]; |
madcowswe | 0:0bbf2f16da9c | 178 | radiolink.read((unsigned char*)data, 5); |
madcowswe | 0:0bbf2f16da9c | 179 | |
madcowswe | 0:0bbf2f16da9c | 180 | for (int i = 0; i < 5; i++) { |
madcowswe | 0:0bbf2f16da9c | 181 | oldcommands[i] = commands[i]; |
madcowswe | 0:0bbf2f16da9c | 182 | |
madcowswe | 0:0bbf2f16da9c | 183 | switch (i) { |
madcowswe | 0:0bbf2f16da9c | 184 | case 0: |
madcowswe | 0:0bbf2f16da9c | 185 | commands[0] = data[i] * 0.0020; |
madcowswe | 0:0bbf2f16da9c | 186 | break; |
madcowswe | 0:0bbf2f16da9c | 187 | case 1: |
madcowswe | 0:0bbf2f16da9c | 188 | commands[1] = data[i] * -0.0020; |
madcowswe | 0:0bbf2f16da9c | 189 | break; |
madcowswe | 0:0bbf2f16da9c | 190 | case 2: { |
madcowswe | 0:0bbf2f16da9c | 191 | float throttle = (unsigned char)data[i]; |
madcowswe | 0:0bbf2f16da9c | 192 | //commands[2] = (char)data[i] * 6.6 - (char)data[i] * (char)data[i] * 2.4; // / 5000.0; |
madcowswe | 0:0bbf2f16da9c | 193 | commands[2] = throttle * 5.0f - 0.01f * throttle * throttle; |
madcowswe | 0:0bbf2f16da9c | 194 | //pc.printf("throttle now at %f grams per motor\r\n", commands[2]); |
madcowswe | 0:0bbf2f16da9c | 195 | if (commands[2] < 250) |
madcowswe | 0:0bbf2f16da9c | 196 | integratestop = 1; |
madcowswe | 0:0bbf2f16da9c | 197 | else |
madcowswe | 0:0bbf2f16da9c | 198 | integratestop = 0; |
madcowswe | 0:0bbf2f16da9c | 199 | /*if (data[i] < -100) { |
madcowswe | 0:0bbf2f16da9c | 200 | kill("Killed by throttle low position"); |
madcowswe | 0:0bbf2f16da9c | 201 | commands[4] = 1.0; |
madcowswe | 0:0bbf2f16da9c | 202 | }*/ |
madcowswe | 0:0bbf2f16da9c | 203 | break; |
madcowswe | 0:0bbf2f16da9c | 204 | } |
madcowswe | 0:0bbf2f16da9c | 205 | case 3: |
madcowswe | 0:0bbf2f16da9c | 206 | commands[3] = data[i] * 0.0005; |
madcowswe | 0:0bbf2f16da9c | 207 | break; |
madcowswe | 0:0bbf2f16da9c | 208 | case 4: |
madcowswe | 0:0bbf2f16da9c | 209 | commands[4] = max((float)data[i], commands[4]); |
madcowswe | 0:0bbf2f16da9c | 210 | if (commands[4] > 0.1) { |
madcowswe | 0:0bbf2f16da9c | 211 | //pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]); |
madcowswe | 0:0bbf2f16da9c | 212 | kill("Killed by controller E-stop"); |
madcowswe | 0:0bbf2f16da9c | 213 | } |
madcowswe | 0:0bbf2f16da9c | 214 | break; |
madcowswe | 0:0bbf2f16da9c | 215 | } |
madcowswe | 0:0bbf2f16da9c | 216 | } |
madcowswe | 0:0bbf2f16da9c | 217 | |
madcowswe | 0:0bbf2f16da9c | 218 | commandsalive = 1; |
madcowswe | 0:0bbf2f16da9c | 219 | } |
madcowswe | 0:0bbf2f16da9c | 220 | |
madcowswe | 0:0bbf2f16da9c | 221 | |
madcowswe | 0:0bbf2f16da9c | 222 | |
madcowswe | 0:0bbf2f16da9c | 223 | |
madcowswe | 0:0bbf2f16da9c | 224 |