Erik van de Coevering / quadV3

This is my quadcopter prototype software, still in development!

quadv3/main.cpp@1:ac68f0368a77, 2013-07-23 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 23 14:01:42 2013 +0000
Revision:
1:ac68f0368a77
Parent:
0:978110f7f027 
Other accelerometer added

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
Anaesthetix 0:978110f7f027 35 DigitalOut myled(LED1);
Anaesthetix 0:978110f7f027 36
Anaesthetix 0:978110f7f027 37 Timer trudder;
Anaesthetix 0:978110f7f027 38 Timer tthrottle;
Anaesthetix 0:978110f7f027 39 Timer televator;
Anaesthetix 0:978110f7f027 40 Timer taileron;
Anaesthetix 0:978110f7f027 41
Anaesthetix 0:978110f7f027 42 //Rx variables
Anaesthetix 0:978110f7f027 43 int ruddertime = 1616;
Anaesthetix 0:978110f7f027 44 int throttletime = 1100;
Anaesthetix 0:978110f7f027 45 int elevatortime = 850;
Anaesthetix 0:978110f7f027 46 int ailerontime = 850;
Anaesthetix 0:978110f7f027 47 int ruddercenter = 1616;
Anaesthetix 0:978110f7f027 48 int elevcenter = 1616;
Anaesthetix 0:978110f7f027 49 int aileroncenter = 1616;
Anaesthetix 0:978110f7f027 50
Anaesthetix 0:978110f7f027 51 //Variables for calccomp
Anaesthetix 0:978110f7f027 52 float xcomp = 0;
Anaesthetix 0:978110f7f027 53 float ycomp = 0;
Anaesthetix 0:978110f7f027 54 int xcomp2 = 0;
Anaesthetix 0:978110f7f027 55 int ycomp2 = 0;
Anaesthetix 0:978110f7f027 56 float ruddercomp = 0;
Anaesthetix 0:978110f7f027 57 float ratio = 0.38;
Anaesthetix 0:978110f7f027 58 int ailer = 0;
Anaesthetix 0:978110f7f027 59 int elev = 0;
Anaesthetix 0:978110f7f027 60 int rud = 0;
Anaesthetix 0:978110f7f027 61 int zcomp = 0;
Anaesthetix 0:978110f7f027 62 int throttle = 0;
Anaesthetix 0:978110f7f027 63 float ailer2 = 0;
Anaesthetix 0:978110f7f027 64 float elev2 = 0;
Anaesthetix 0:978110f7f027 65 signed int m1 = 0;
Anaesthetix 0:978110f7f027 66 signed int m2 = 0;
Anaesthetix 0:978110f7f027 67 signed int m3 = 0;
Anaesthetix 0:978110f7f027 68 signed int m4 = 0;
Anaesthetix 0:978110f7f027 69 bool armed = false;
Anaesthetix 0:978110f7f027 70 float xsteering = 0;
Anaesthetix 0:978110f7f027 71 float ysteering = 0;
Anaesthetix 0:978110f7f027 72
Anaesthetix 0:978110f7f027 73 DigitalIn rxthrottle(p25);
Anaesthetix 0:978110f7f027 74 DigitalIn rxaileron(p26);
Anaesthetix 0:978110f7f027 75 DigitalIn rxelevator(p29);
Anaesthetix 0:978110f7f027 76 DigitalIn rxrudder(p30);
Anaesthetix 0:978110f7f027 77 DigitalIn rxextra(p19);
Anaesthetix 0:978110f7f027 78
Anaesthetix 0:978110f7f027 79 void calccomp(void)
Anaesthetix 0:978110f7f027 80 {
Anaesthetix 0:978110f7f027 81 gyrosample();
Anaesthetix 0:978110f7f027 82
Anaesthetix 0:978110f7f027 83 //Scale rx channels
Anaesthetix 0:978110f7f027 84 ailer = ailerontime - aileroncenter;
Anaesthetix 0:978110f7f027 85 elev = elevatortime - elevcenter;
Anaesthetix 0:978110f7f027 86 rud = ruddertime - ruddercenter;
Anaesthetix 0:978110f7f027 87 throttle = throttletime;
Anaesthetix 0:978110f7f027 88
Anaesthetix 0:978110f7f027 89 xsteering = ailer;
Anaesthetix 0:978110f7f027 90 ysteering = elev;
Anaesthetix 0:978110f7f027 91 xsteering = xsteering/8;
Anaesthetix 0:978110f7f027 92 ysteering = ysteering/8;
Anaesthetix 0:978110f7f027 93 rud = rud/3;
Anaesthetix 0:978110f7f027 94 throttle = ((throttle/2.2)+720);
Anaesthetix 0:978110f7f027 95
Anaesthetix 0:978110f7f027 96 //Start by mixing throttle
Anaesthetix 0:978110f7f027 97 m1 = throttle;
Anaesthetix 0:978110f7f027 98 m2 = throttle;
Anaesthetix 0:978110f7f027 99 m3 = throttle;
Anaesthetix 0:978110f7f027 100 m4 = throttle;
Anaesthetix 0:978110f7f027 101
Anaesthetix 0:978110f7f027 102 //Calc aileron compensation and mix with rx input
Anaesthetix 1:ac68f0368a77 103 xcomp = (((((newax)-xsteering)*(1-ratio)) + ((yag+((newax-xsteering)*0.7))*ratio))*1.10);
Anaesthetix 1:ac68f0368a77 104 xcomp2 = (xcomp*-1);
Anaesthetix 1:ac68f0368a77 105 // xcomp2 = 0;
Anaesthetix 0:978110f7f027 106
Anaesthetix 0:978110f7f027 107 //Mix aileron
Anaesthetix 0:978110f7f027 108 m1 = m1 - xcomp2;
Anaesthetix 0:978110f7f027 109 m2 = m2 + xcomp2;
Anaesthetix 0:978110f7f027 110 m3 = m3 + xcomp2;
Anaesthetix 0:978110f7f027 111 m4 = m4 - xcomp2;
Anaesthetix 0:978110f7f027 112
Anaesthetix 0:978110f7f027 113 //Calc elevator compensation and mix with rx input
Anaesthetix 1:ac68f0368a77 114 ycomp = (((((neway)+ysteering)*(1-ratio)) + ((xag+((neway+ysteering)*0.7))*ratio))*1.10);
Anaesthetix 0:978110f7f027 115 ycomp2 = ycomp;
Anaesthetix 1:ac68f0368a77 116 // ycomp2 = 0;
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 1:ac68f0368a77 124 //Calc rudder compensation and mix with rx input -> PI-controller?
Anaesthetix 1:ac68f0368a77 125 ruddercomp = (rud + (zarg*1)); //has drift
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