Erik van de Coevering / quadV3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

quadv3/main.cpp@0:978110f7f027, 2013-01-30 (annotated)

Committer:
Anaesthetix
Date:
Wed Jan 30 13:14:44 2013 +0000
Revision:
0:978110f7f027
Child:
1:ac68f0368a77
My quadcopter prototype software, still in development.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:978110f7f027 1 /**
Anaesthetix 0:978110f7f027 2 * @author Erik van de Coevering
Anaesthetix 0:978110f7f027 3 *
Anaesthetix 0:978110f7f027 4 * @section LICENSE
Anaesthetix 0:978110f7f027 5 *
Anaesthetix 0:978110f7f027 6 *
Anaesthetix 0:978110f7f027 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
Anaesthetix 0:978110f7f027 8 * of this software and associated documentation files (the "Software"), to deal
Anaesthetix 0:978110f7f027 9 * in the Software without restriction, including without limitation the rights
Anaesthetix 0:978110f7f027 10 * to use, copy, modify, merge and/or publish copies of the Software, and to permit
Anaesthetix 0:978110f7f027 11 * persons to whom the Software is furnished to do so, subject to the following conditions:
Anaesthetix 0:978110f7f027 12 *
Anaesthetix 0:978110f7f027 13 * The above copyright notice and this permission notice shall be included in
Anaesthetix 0:978110f7f027 14 * all copies or substantial portions of the Software.
Anaesthetix 0:978110f7f027 15 *
Anaesthetix 0:978110f7f027 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Anaesthetix 0:978110f7f027 17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Anaesthetix 0:978110f7f027 18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Anaesthetix 0:978110f7f027 19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Anaesthetix 0:978110f7f027 20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Anaesthetix 0:978110f7f027 21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Anaesthetix 0:978110f7f027 22 * THE SOFTWARE.
Anaesthetix 0:978110f7f027 23 */
Anaesthetix 0:978110f7f027 24
Anaesthetix 0:978110f7f027 25 #include "mbed.h"
Anaesthetix 0:978110f7f027 26 #include "ADXL345.h"
Anaesthetix 0:978110f7f027 27 #include "ITG3200.h"
Anaesthetix 0:978110f7f027 28 #include "math.h"
Anaesthetix 0:978110f7f027 29 //#include "accelero.h"
Anaesthetix 0:978110f7f027 30 #include "bma180.h"
Anaesthetix 0:978110f7f027 31 #include "gyro.h"
Anaesthetix 0:978110f7f027 32 #include "IOMacros.h"
Anaesthetix 0:978110f7f027 33 #include "pwmout.h"
Anaesthetix 0:978110f7f027 34 #include "TextLCD.h"
Anaesthetix 0:978110f7f027 35
Anaesthetix 0:978110f7f027 36 TextLCD lcd(p11, p12, p13, p14, p15, p16, TextLCD::LCD16x2);
Anaesthetix 0:978110f7f027 37 DigitalOut myled(LED1);
Anaesthetix 0:978110f7f027 38
Anaesthetix 0:978110f7f027 39 Timer trudder;
Anaesthetix 0:978110f7f027 40 Timer tthrottle;
Anaesthetix 0:978110f7f027 41 Timer televator;
Anaesthetix 0:978110f7f027 42 Timer taileron;
Anaesthetix 0:978110f7f027 43
Anaesthetix 0:978110f7f027 44 //Rx variables
Anaesthetix 0:978110f7f027 45 int ruddertime = 1616;
Anaesthetix 0:978110f7f027 46 int throttletime = 1100;
Anaesthetix 0:978110f7f027 47 int elevatortime = 850;
Anaesthetix 0:978110f7f027 48 int ailerontime = 850;
Anaesthetix 0:978110f7f027 49 int ruddercenter = 1616;
Anaesthetix 0:978110f7f027 50 int elevcenter = 1616;
Anaesthetix 0:978110f7f027 51 int aileroncenter = 1616;
Anaesthetix 0:978110f7f027 52
Anaesthetix 0:978110f7f027 53 //Variables for calccomp
Anaesthetix 0:978110f7f027 54 float xcomp = 0;
Anaesthetix 0:978110f7f027 55 float ycomp = 0;
Anaesthetix 0:978110f7f027 56 int xcomp2 = 0;
Anaesthetix 0:978110f7f027 57 int ycomp2 = 0;
Anaesthetix 0:978110f7f027 58 float ruddercomp = 0;
Anaesthetix 0:978110f7f027 59 float ratio = 0.38;
Anaesthetix 0:978110f7f027 60 int ailer = 0;
Anaesthetix 0:978110f7f027 61 int elev = 0;
Anaesthetix 0:978110f7f027 62 int rud = 0;
Anaesthetix 0:978110f7f027 63 int zcomp = 0;
Anaesthetix 0:978110f7f027 64 int throttle = 0;
Anaesthetix 0:978110f7f027 65 float ailer2 = 0;
Anaesthetix 0:978110f7f027 66 float elev2 = 0;
Anaesthetix 0:978110f7f027 67 signed int m1 = 0;
Anaesthetix 0:978110f7f027 68 signed int m2 = 0;
Anaesthetix 0:978110f7f027 69 signed int m3 = 0;
Anaesthetix 0:978110f7f027 70 signed int m4 = 0;
Anaesthetix 0:978110f7f027 71 bool armed = false;
Anaesthetix 0:978110f7f027 72 float xsteering = 0;
Anaesthetix 0:978110f7f027 73 float ysteering = 0;
Anaesthetix 0:978110f7f027 74
Anaesthetix 0:978110f7f027 75 DigitalIn rxthrottle(p25);
Anaesthetix 0:978110f7f027 76 DigitalIn rxaileron(p26);
Anaesthetix 0:978110f7f027 77 DigitalIn rxelevator(p29);
Anaesthetix 0:978110f7f027 78 DigitalIn rxrudder(p30);
Anaesthetix 0:978110f7f027 79 DigitalIn rxextra(p19);
Anaesthetix 0:978110f7f027 80
Anaesthetix 0:978110f7f027 81 void calccomp(void)
Anaesthetix 0:978110f7f027 82 {
Anaesthetix 0:978110f7f027 83 gyrosample();
Anaesthetix 0:978110f7f027 84
Anaesthetix 0:978110f7f027 85 //Scale rx channels
Anaesthetix 0:978110f7f027 86 ailer = ailerontime - aileroncenter;
Anaesthetix 0:978110f7f027 87 elev = elevatortime - elevcenter;
Anaesthetix 0:978110f7f027 88 rud = ruddertime - ruddercenter;
Anaesthetix 0:978110f7f027 89 throttle = throttletime;
Anaesthetix 0:978110f7f027 90
Anaesthetix 0:978110f7f027 91 xsteering = ailer;
Anaesthetix 0:978110f7f027 92 ysteering = elev;
Anaesthetix 0:978110f7f027 93 xsteering = xsteering/8;
Anaesthetix 0:978110f7f027 94 ysteering = ysteering/8;
Anaesthetix 0:978110f7f027 95 rud = rud/3;
Anaesthetix 0:978110f7f027 96 throttle = ((throttle/2.2)+720);
Anaesthetix 0:978110f7f027 97
Anaesthetix 0:978110f7f027 98 //Start by mixing throttle
Anaesthetix 0:978110f7f027 99 m1 = throttle;
Anaesthetix 0:978110f7f027 100 m2 = throttle;
Anaesthetix 0:978110f7f027 101 m3 = throttle;
Anaesthetix 0:978110f7f027 102 m4 = throttle;
Anaesthetix 0:978110f7f027 103
Anaesthetix 0:978110f7f027 104 //Calc aileron compensation and mix with rx input
Anaesthetix 0:978110f7f027 105 xcomp = (((((newax)-xsteering)*(1-ratio)) + ((yag+((newax-xsteering)*0.7))*ratio))*0.8);
Anaesthetix 0:978110f7f027 106 xcomp2 = xcomp;
Anaesthetix 0:978110f7f027 107
Anaesthetix 0:978110f7f027 108 //Mix aileron
Anaesthetix 0:978110f7f027 109 m1 = m1 - xcomp2;
Anaesthetix 0:978110f7f027 110 m2 = m2 + xcomp2;
Anaesthetix 0:978110f7f027 111 m3 = m3 + xcomp2;
Anaesthetix 0:978110f7f027 112 m4 = m4 - xcomp2;
Anaesthetix 0:978110f7f027 113
Anaesthetix 0:978110f7f027 114 //Calc elevator compensation and mix with rx input
Anaesthetix 0:978110f7f027 115 ycomp = (((((neway)+ysteering)*(1-ratio)) + ((xag+((neway+ysteering)*0.7))*ratio))*0.8);
Anaesthetix 0:978110f7f027 116 ycomp2 = ycomp;
Anaesthetix 0:978110f7f027 117
Anaesthetix 0:978110f7f027 118 //Mix elevator
Anaesthetix 0:978110f7f027 119 m1 = m1 - ycomp2;
Anaesthetix 0:978110f7f027 120 m2 = m2 - ycomp2;
Anaesthetix 0:978110f7f027 121 m3 = m3 + ycomp2;
Anaesthetix 0:978110f7f027 122 m4 = m4 + ycomp2;
Anaesthetix 0:978110f7f027 123
Anaesthetix 0:978110f7f027 124 //Calc rudder compensation and mix with rx input
Anaesthetix 0:978110f7f027 125 ruddercomp = (rud + (zarg*1));
Anaesthetix 0:978110f7f027 126 zcomp = ruddercomp;
Anaesthetix 0:978110f7f027 127
Anaesthetix 0:978110f7f027 128 //Mix rudder
Anaesthetix 0:978110f7f027 129 m1 = m1 - zcomp;
Anaesthetix 0:978110f7f027 130 m2 = m2 + zcomp;
Anaesthetix 0:978110f7f027 131 m3 = m3 - zcomp;
Anaesthetix 0:978110f7f027 132 m4 = m4 + zcomp;
Anaesthetix 0:978110f7f027 133
Anaesthetix 0:978110f7f027 134 //Prevent motors from stalling
Anaesthetix 0:978110f7f027 135 // if(m1 < 1075 && throttle > 1000) m1 = 1280;
Anaesthetix 0:978110f7f027 136 // if(m2 < 1075 && throttle > 1000) m2 = 1280;
Anaesthetix 0:978110f7f027 137 // if(m3 < 1075 && throttle > 1000) m3 = 1280;
Anaesthetix 0:978110f7f027 138 // if(m4 < 1075 && throttle > 1000) m4 = 1280;
Anaesthetix 0:978110f7f027 139
Anaesthetix 0:978110f7f027 140 //When throttle down or if not armed, stop motors
Anaesthetix 0:978110f7f027 141 if(throttle < 1350 || armed == false) {
Anaesthetix 0:978110f7f027 142 m1=1200;
Anaesthetix 0:978110f7f027 143 m2=1200;
Anaesthetix 0:978110f7f027 144 m3=1200;
Anaesthetix 0:978110f7f027 145 m4=1200;
Anaesthetix 0:978110f7f027 146 }
Anaesthetix 0:978110f7f027 147
Anaesthetix 0:978110f7f027 148 //Stick arming
Anaesthetix 0:978110f7f027 149 if(throttle < 1325 && rud > 120) {
Anaesthetix 0:978110f7f027 150 armed = true;
Anaesthetix 0:978110f7f027 151 myled = 1;
Anaesthetix 0:978110f7f027 152 }
Anaesthetix 0:978110f7f027 153 if(throttle < 1325 && rud < -105) {
Anaesthetix 0:978110f7f027 154 armed = false;
Anaesthetix 0:978110f7f027 155 myled = 0;
Anaesthetix 0:978110f7f027 156 }
Anaesthetix 0:978110f7f027 157
Anaesthetix 0:978110f7f027 158 m3 += 20;
Anaesthetix 0:978110f7f027 159
Anaesthetix 0:978110f7f027 160 if(m1 > 1640) m1 = 1640;
Anaesthetix 0:978110f7f027 161 if(m2 > 1640) m2 = 1640;
Anaesthetix 0:978110f7f027 162 if(m3 > 1640) m3 = 1640;
Anaesthetix 0:978110f7f027 163 if(m4 > 1640) m4 = 1640;
Anaesthetix 0:978110f7f027 164
Anaesthetix 0:978110f7f027 165 //Output to motors
Anaesthetix 0:978110f7f027 166 setpwm(m1, m2, m3, m4);
Anaesthetix 0:978110f7f027 167 }
Anaesthetix 0:978110f7f027 168
Anaesthetix 0:978110f7f027 169
Anaesthetix 0:978110f7f027 170
Anaesthetix 0:978110f7f027 171 int main()
Anaesthetix 0:978110f7f027 172 {
Anaesthetix 0:978110f7f027 173 bool rudderflag = false;
Anaesthetix 0:978110f7f027 174 bool throttleflag = false;
Anaesthetix 0:978110f7f027 175 bool elevatorflag = false;
Anaesthetix 0:978110f7f027 176 bool aileronflag = false;
Anaesthetix 0:978110f7f027 177 bool extraflag = false;
Anaesthetix 0:978110f7f027 178 bool exitflag = true;
Anaesthetix 0:978110f7f027 179
Anaesthetix 0:978110f7f027 180 int th = 0;
Anaesthetix 0:978110f7f027 181 int ai = 0;
Anaesthetix 0:978110f7f027 182 int el = 0;
Anaesthetix 0:978110f7f027 183 int ru = 0;
Anaesthetix 0:978110f7f027 184
Anaesthetix 0:978110f7f027 185 myled = 0;
Anaesthetix 0:978110f7f027 186
Anaesthetix 0:978110f7f027 187 startpwm();
Anaesthetix 0:978110f7f027 188 // accstart();
Anaesthetix 0:978110f7f027 189 acc1.init();
Anaesthetix 0:978110f7f027 190 acc1.range(0);
Anaesthetix 0:978110f7f027 191 acc1.bw(7);
Anaesthetix 0:978110f7f027 192 gyrostart();
Anaesthetix 0:978110f7f027 193 wait(1);
Anaesthetix 0:978110f7f027 194 gyrocal();
Anaesthetix 0:978110f7f027 195
Anaesthetix 0:978110f7f027 196 while(1) {
Anaesthetix 0:978110f7f027 197 if(!throttleflag && rxthrottle) {
Anaesthetix 0:978110f7f027 198 tthrottle.start();
Anaesthetix 0:978110f7f027 199 throttleflag = true;
Anaesthetix 0:978110f7f027 200 extraflag = false;
Anaesthetix 0:978110f7f027 201 calccomp();
Anaesthetix 0:978110f7f027 202 }
Anaesthetix 0:978110f7f027 203
Anaesthetix 0:978110f7f027 204 if(!aileronflag && rxaileron) {
Anaesthetix 0:978110f7f027 205 th = tthrottle.read_us();
Anaesthetix 0:978110f7f027 206 if(th > 1000 && th < 2200) throttletime = th;
Anaesthetix 0:978110f7f027 207 taileron.start();
Anaesthetix 0:978110f7f027 208 tthrottle.reset();
Anaesthetix 0:978110f7f027 209 aileronflag = true;
Anaesthetix 0:978110f7f027 210 throttleflag = false;
Anaesthetix 0:978110f7f027 211 calccomp();
Anaesthetix 0:978110f7f027 212 }
Anaesthetix 0:978110f7f027 213
Anaesthetix 0:978110f7f027 214 if(!elevatorflag && rxelevator) {
Anaesthetix 0:978110f7f027 215 ai = taileron.read_us();
Anaesthetix 0:978110f7f027 216 if(ai > 1000 && ai < 2200) ailerontime = ai;
Anaesthetix 0:978110f7f027 217 televator.start();
Anaesthetix 0:978110f7f027 218 taileron.reset();
Anaesthetix 0:978110f7f027 219 elevatorflag = true;
Anaesthetix 0:978110f7f027 220 aileronflag = false;
Anaesthetix 0:978110f7f027 221 calccomp();
Anaesthetix 0:978110f7f027 222 }
Anaesthetix 0:978110f7f027 223
Anaesthetix 0:978110f7f027 224 if(!rudderflag && rxrudder) {
Anaesthetix 0:978110f7f027 225 el = televator.read_us();
Anaesthetix 0:978110f7f027 226 if(el > 1000 && el < 2200) elevatortime = el;
Anaesthetix 0:978110f7f027 227 trudder.start();
Anaesthetix 0:978110f7f027 228 televator.reset();
Anaesthetix 0:978110f7f027 229 rudderflag = true;
Anaesthetix 0:978110f7f027 230 elevatorflag = false;
Anaesthetix 0:978110f7f027 231 calccomp();
Anaesthetix 0:978110f7f027 232 }
Anaesthetix 0:978110f7f027 233
Anaesthetix 0:978110f7f027 234 if(!extraflag && rxextra) {
Anaesthetix 0:978110f7f027 235 ru = trudder.read_us();
Anaesthetix 0:978110f7f027 236 if(ru > 1000 && ru < 2200) ruddertime = ru;
Anaesthetix 0:978110f7f027 237 trudder.reset();
Anaesthetix 0:978110f7f027 238 extraflag = true;
Anaesthetix 0:978110f7f027 239 rudderflag = false;
Anaesthetix 0:978110f7f027 240 calccomp();
Anaesthetix 0:978110f7f027 241 wait_us(500);
Anaesthetix 0:978110f7f027 242 calccomp();
Anaesthetix 0:978110f7f027 243 wait_us(500);
Anaesthetix 0:978110f7f027 244 calccomp();
Anaesthetix 0:978110f7f027 245 wait_us(500);
Anaesthetix 0:978110f7f027 246 calccomp();
Anaesthetix 0:978110f7f027 247 wait_us(500);
Anaesthetix 0:978110f7f027 248 calccomp();
Anaesthetix 0:978110f7f027 249 wait_us(500);
Anaesthetix 0:978110f7f027 250 calccomp();
Anaesthetix 0:978110f7f027 251 wait_us(500);
Anaesthetix 0:978110f7f027 252 calccomp();
Anaesthetix 0:978110f7f027 253 wait_us(500);
Anaesthetix 0:978110f7f027 254 calccomp();
Anaesthetix 0:978110f7f027 255 wait_us(500);
Anaesthetix 0:978110f7f027 256 calccomp();
Anaesthetix 0:978110f7f027 257 }
Anaesthetix 0:978110f7f027 258 }
Anaesthetix 0:978110f7f027 259 }
Anaesthetix 0:978110f7f027 260