NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
main.cpp@20:e116e596e540, 2012-11-05 (annotated)
- Committer:
- maetugr
- Date:
- Mon Nov 05 09:19:01 2012 +0000
- Revision:
- 20:e116e596e540
- Parent:
- 19:40c252b4a792
- Child:
- 21:c2a2e7cbabdd
Eine Achse stabil! bereits abgehoben an Schnur! (erst Gyro im I2C_Sensor)(acos nan abgefangen)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 7:9d4313510646 | 1 | #include "mbed.h" // Standard Library |
maetugr | 7:9d4313510646 | 2 | #include "LED.h" // LEDs framework for blinking ;) |
maetugr | 13:4737ee9ebfee | 3 | #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) |
maetugr | 7:9d4313510646 | 4 | #include "L3G4200D.h" // Gyro (Gyroscope) |
maetugr | 7:9d4313510646 | 5 | #include "ADXL345.h" // Acc (Accelerometer) |
maetugr | 7:9d4313510646 | 6 | #include "HMC5883.h" // Comp (Compass) |
maetugr | 14:cf260677ecde | 7 | #include "BMP085_old.h" // Alt (Altitude sensor) |
maetugr | 7:9d4313510646 | 8 | #include "RC_Channel.h" // RemoteControl Chnnels with PPM |
maetugr | 15:753c5d6a63b3 | 9 | #include "Servo_PWM.h" // Motor PPM using PwmOut |
maetugr | 13:4737ee9ebfee | 10 | #include "PID.h" // PID Library by Aaron Berk |
maetugr | 0:0c4fafa398b4 | 11 | |
maetugr | 14:cf260677ecde | 12 | #define PI 3.1415926535897932384626433832795 // ratio of a circle's circumference to its diameter |
maetugr | 17:e096e85f6c36 | 13 | #define RAD2DEG 57.295779513082320876798154814105 // ratio between radians and degree (360/2Pi) |
maetugr | 15:753c5d6a63b3 | 14 | #define RATE 0.02 // speed of the interrupt for Sensors and PID |
maetugr | 15:753c5d6a63b3 | 15 | |
maetugr | 20:e116e596e540 | 16 | #define P_VALUE 0.05 // PID values |
maetugr | 20:e116e596e540 | 17 | #define I_VALUE 5 |
maetugr | 20:e116e596e540 | 18 | #define D_VALUE 0.015 |
maetugr | 12:67a06c9b69d5 | 19 | |
maetugr | 14:cf260677ecde | 20 | //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start |
maetugr | 2:93f703d2c4d7 | 21 | |
maetugr | 14:cf260677ecde | 22 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 14:cf260677ecde | 23 | Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC |
maetugr | 14:cf260677ecde | 24 | |
maetugr | 14:cf260677ecde | 25 | // initialisation of hardware (see includes for more info) |
maetugr | 5:818c0668fd2d | 26 | LED LEDs; |
maetugr | 19:40c252b4a792 | 27 | PC pc(USBTX, USBRX, 115200); |
maetugr | 5:818c0668fd2d | 28 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 29 | ADXL345 Acc(p28, p27); |
maetugr | 11:9bf69bc6df45 | 30 | HMC5883 Comp(p28, p27); |
maetugr | 14:cf260677ecde | 31 | BMP085_old Alt(p28, p27); |
maetugr | 15:753c5d6a63b3 | 32 | RC_Channel RC[] = {(p11), (p12), (p13), (p14)}; // noooo p19/p20 ! |
maetugr | 15:753c5d6a63b3 | 33 | Servo_PWM Motor[] = {(p21), (p22), (p23), (p24)}; // p21 - p26 only ! |
maetugr | 8:d25ecdcdbeb5 | 34 | |
maetugr | 15:753c5d6a63b3 | 35 | //PID Controller[] = {(P_VALUE, I_VALUE, D_VALUE, RATE), (P_VALUE, I_VALUE, D_VALUE, RATE), (P_VALUE, I_VALUE, D_VALUE, RATE)}; // TODO: RATE != dt immer anpassen |
maetugr | 15:753c5d6a63b3 | 36 | //PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25 |
maetugr | 15:753c5d6a63b3 | 37 | PID Controller(P_VALUE, I_VALUE, D_VALUE, RATE); |
maetugr | 14:cf260677ecde | 38 | |
maetugr | 14:cf260677ecde | 39 | |
maetugr | 14:cf260677ecde | 40 | // global varibles |
maetugr | 20:e116e596e540 | 41 | bool armed = false; // this variable is for security |
maetugr | 14:cf260677ecde | 42 | unsigned long dt_get_data = 0; // TODO: dt namen |
maetugr | 12:67a06c9b69d5 | 43 | unsigned long time_get_data = 0; |
maetugr | 12:67a06c9b69d5 | 44 | unsigned long dt_read_sensors = 0; |
maetugr | 12:67a06c9b69d5 | 45 | unsigned long time_read_sensors = 0; |
maetugr | 14:cf260677ecde | 46 | float angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw] |
maetugr | 14:cf260677ecde | 47 | float tempangle = 0; // temporärer winkel für yaw ohne kompass |
maetugr | 20:e116e596e540 | 48 | float Gyro_angle[3] ={0,0,0}; |
maetugr | 15:753c5d6a63b3 | 49 | float co; // PID test |
maetugr | 8:d25ecdcdbeb5 | 50 | |
maetugr | 14:cf260677ecde | 51 | void get_Data() // method which is called by the Ticker Datagetter every RATE seconds |
maetugr | 8:d25ecdcdbeb5 | 52 | { |
maetugr | 12:67a06c9b69d5 | 53 | time_read_sensors = GlobalTimer.read_us(); |
maetugr | 12:67a06c9b69d5 | 54 | |
maetugr | 15:753c5d6a63b3 | 55 | // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes |
maetugr | 14:cf260677ecde | 56 | Gyro.read(); |
maetugr | 15:753c5d6a63b3 | 57 | Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig |
maetugr | 15:753c5d6a63b3 | 58 | Comp.read(); |
maetugr | 13:4737ee9ebfee | 59 | //Alt.Update(); TODO braucht zu lange zum auslesen! |
maetugr | 12:67a06c9b69d5 | 60 | |
maetugr | 12:67a06c9b69d5 | 61 | dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; |
maetugr | 8:d25ecdcdbeb5 | 62 | |
maetugr | 8:d25ecdcdbeb5 | 63 | // meassure dt |
maetugr | 12:67a06c9b69d5 | 64 | dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop |
maetugr | 12:67a06c9b69d5 | 65 | time_get_data = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 8:d25ecdcdbeb5 | 66 | |
maetugr | 20:e116e596e540 | 67 | Gyro_angle[0] += Gyro.data[0] *dt_get_data/15000000.0; |
maetugr | 20:e116e596e540 | 68 | Gyro_angle[1] += Gyro.data[1] *dt_get_data/15000000.0; |
maetugr | 20:e116e596e540 | 69 | Gyro_angle[2] += Gyro.data[2] *dt_get_data/15000000.0; |
maetugr | 20:e116e596e540 | 70 | |
maetugr | 8:d25ecdcdbeb5 | 71 | // calculate angles for roll, pitch an yaw |
maetugr | 12:67a06c9b69d5 | 72 | angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0; |
maetugr | 12:67a06c9b69d5 | 73 | angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen |
maetugr | 12:67a06c9b69d5 | 74 | tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0; |
maetugr | 20:e116e596e540 | 75 | angle[2] = Gyro_angle[2]; // gyro only here |
maetugr | 12:67a06c9b69d5 | 76 | |
maetugr | 15:753c5d6a63b3 | 77 | // PID controlling |
maetugr | 15:753c5d6a63b3 | 78 | Controller.setProcessValue(angle[1]); |
maetugr | 8:d25ecdcdbeb5 | 79 | |
maetugr | 20:e116e596e540 | 80 | // Aming/ disarming |
maetugr | 20:e116e596e540 | 81 | if(RC[2].read() < 1020 && RC[3].read() < 1020) |
maetugr | 20:e116e596e540 | 82 | armed = false; |
maetugr | 20:e116e596e540 | 83 | if(RC[2].read() < 500 || RC[1].read() < 500 || RC[0].read() < 500) |
maetugr | 20:e116e596e540 | 84 | armed = false; |
maetugr | 20:e116e596e540 | 85 | if(RC[2].read() < 1020 && RC[3].read() > 1850) |
maetugr | 20:e116e596e540 | 86 | armed = true; |
maetugr | 20:e116e596e540 | 87 | |
maetugr | 12:67a06c9b69d5 | 88 | // calculate new motorspeeds |
maetugr | 15:753c5d6a63b3 | 89 | co = Controller.compute() - 1000; |
maetugr | 20:e116e596e540 | 90 | if (armed) // zur SICHERHEIT! |
maetugr | 15:753c5d6a63b3 | 91 | { |
maetugr | 20:e116e596e540 | 92 | #if 0 |
maetugr | 20:e116e596e540 | 93 | Motor[0] = RC[2].read(); |
maetugr | 20:e116e596e540 | 94 | Motor[1] = RC[2].read(); |
maetugr | 20:e116e596e540 | 95 | Motor[2] = RC[2].read(); |
maetugr | 20:e116e596e540 | 96 | Motor[3] = RC[2].read(); |
maetugr | 20:e116e596e540 | 97 | #else |
maetugr | 20:e116e596e540 | 98 | Motor[0] = RC[2].read()+co; |
maetugr | 20:e116e596e540 | 99 | Motor[2] = RC[2].read()-co; |
maetugr | 20:e116e596e540 | 100 | #endif |
maetugr | 20:e116e596e540 | 101 | |
maetugr | 15:753c5d6a63b3 | 102 | /*Motor[0] = RC[2].read()+((RC[0].read() - 1500)/10.0)+40; |
maetugr | 15:753c5d6a63b3 | 103 | Motor[2] = RC[2].read()-((RC[0].read() - 1500)/10.0)-40;*/ |
maetugr | 20:e116e596e540 | 104 | /**/ |
maetugr | 15:753c5d6a63b3 | 105 | } else { |
maetugr | 15:753c5d6a63b3 | 106 | Motor[0] = 1000; |
maetugr | 15:753c5d6a63b3 | 107 | Motor[1] = 1000; |
maetugr | 15:753c5d6a63b3 | 108 | Motor[2] = 1000; |
maetugr | 15:753c5d6a63b3 | 109 | Motor[3] = 1000; |
maetugr | 20:e116e596e540 | 110 | } |
maetugr | 20:e116e596e540 | 111 | |
maetugr | 16:74a6531350b5 | 112 | /*Motor[0] = 1000 + (100 - (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000; // test für erste reaktion der motoren entgegen der Auslenkung |
maetugr | 15:753c5d6a63b3 | 113 | Motor[1] = 1000 + (100 - (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000; |
maetugr | 15:753c5d6a63b3 | 114 | Motor[2] = 1000 + (100 + (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000; |
maetugr | 15:753c5d6a63b3 | 115 | Motor[3] = 1000 + (100 + (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;*/ |
maetugr | 8:d25ecdcdbeb5 | 116 | } |
maetugr | 5:818c0668fd2d | 117 | |
maetugr | 14:cf260677ecde | 118 | int main() { // main programm only used for initialisation and debug output |
maetugr | 16:74a6531350b5 | 119 | NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0) |
maetugr | 15:753c5d6a63b3 | 120 | |
maetugr | 15:753c5d6a63b3 | 121 | //for(int i=0;i<3;i++) |
maetugr | 15:753c5d6a63b3 | 122 | Controller.setInputLimits(-90.0, 90.0); |
maetugr | 15:753c5d6a63b3 | 123 | Controller.setOutputLimits(0.0, 2000.0); |
maetugr | 15:753c5d6a63b3 | 124 | Controller.setBias(1000); |
maetugr | 15:753c5d6a63b3 | 125 | Controller.setMode(MANUAL_MODE);//AUTO_MODE); |
maetugr | 15:753c5d6a63b3 | 126 | Controller.setSetPoint(0); |
maetugr | 12:67a06c9b69d5 | 127 | |
maetugr | 12:67a06c9b69d5 | 128 | #ifdef COMPASSCALIBRATE |
maetugr | 12:67a06c9b69d5 | 129 | pc.locate(10,5); |
maetugr | 12:67a06c9b69d5 | 130 | pc.printf("CALIBRATING"); |
maetugr | 12:67a06c9b69d5 | 131 | Comp.calibrate(60); |
maetugr | 12:67a06c9b69d5 | 132 | #endif |
maetugr | 12:67a06c9b69d5 | 133 | |
maetugr | 6:179752756e9f | 134 | // init screen |
maetugr | 6:179752756e9f | 135 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 136 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 137 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 138 | |
maetugr | 14:cf260677ecde | 139 | // Start! TODO: Motor und RC start (armed....?) |
maetugr | 2:93f703d2c4d7 | 140 | GlobalTimer.start(); |
maetugr | 12:67a06c9b69d5 | 141 | Datagetter.attach(&get_Data, RATE); // start to get data all RATEms |
maetugr | 12:67a06c9b69d5 | 142 | |
maetugr | 12:67a06c9b69d5 | 143 | while(1) { |
maetugr | 20:e116e596e540 | 144 | pc.locate(30,0); // PC output |
maetugr | 12:67a06c9b69d5 | 145 | pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 20:e116e596e540 | 146 | pc.locate(5,1); |
maetugr | 20:e116e596e540 | 147 | if(armed) |
maetugr | 20:e116e596e540 | 148 | pc.printf("ARMED!!!!!!!!!!!!!"); |
maetugr | 20:e116e596e540 | 149 | else |
maetugr | 20:e116e596e540 | 150 | pc.printf("DIS_ARMED "); |
maetugr | 20:e116e596e540 | 151 | pc.locate(5,3); |
maetugr | 20:e116e596e540 | 152 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 20:e116e596e540 | 153 | |
maetugr | 20:e116e596e540 | 154 | pc.locate(5,5); |
maetugr | 20:e116e596e540 | 155 | pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); |
maetugr | 20:e116e596e540 | 156 | pc.locate(5,6); |
maetugr | 20:e116e596e540 | 157 | pc.printf("Gyro_angle: X:%6.1f Y:%6.1f Z:%6.1f", Gyro_angle[0], Gyro_angle[1], Gyro_angle[2]); |
maetugr | 20:e116e596e540 | 158 | |
maetugr | 20:e116e596e540 | 159 | pc.locate(5,8); |
maetugr | 20:e116e596e540 | 160 | pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); |
maetugr | 20:e116e596e540 | 161 | pc.locate(5,9); |
maetugr | 20:e116e596e540 | 162 | pc.printf("Acc.angle: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); |
maetugr | 20:e116e596e540 | 163 | |
maetugr | 20:e116e596e540 | 164 | pc.locate(5,11); |
maetugr | 20:e116e596e540 | 165 | pc.printf("PID Test: %6.1f", co); |
maetugr | 20:e116e596e540 | 166 | |
maetugr | 20:e116e596e540 | 167 | pc.locate(10,15); |
maetugr | 12:67a06c9b69d5 | 168 | pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); |
maetugr | 20:e116e596e540 | 169 | pc.locate(10,16); |
maetugr | 12:67a06c9b69d5 | 170 | pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle()); |
maetugr | 20:e116e596e540 | 171 | pc.locate(10,17); |
maetugr | 14:cf260677ecde | 172 | //pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private |
maetugr | 19:40c252b4a792 | 173 | pc.locate(10,18); |
maetugr | 19:40c252b4a792 | 174 | pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle()); |
maetugr | 12:67a06c9b69d5 | 175 | |
maetugr | 11:9bf69bc6df45 | 176 | pc.locate(10,19); |
maetugr | 14:cf260677ecde | 177 | pc.printf("RC0: %4d :[", RC[0].read()); |
maetugr | 12:67a06c9b69d5 | 178 | for (int i = 0; i < (RC[0].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 179 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 180 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 181 | pc.locate(10,20); |
maetugr | 14:cf260677ecde | 182 | pc.printf("RC1: %4d :[", RC[1].read()); |
maetugr | 12:67a06c9b69d5 | 183 | for (int i = 0; i < (RC[1].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 184 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 185 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 186 | pc.locate(10,21); |
maetugr | 14:cf260677ecde | 187 | pc.printf("RC2: %4d :[", RC[2].read()); |
maetugr | 12:67a06c9b69d5 | 188 | for (int i = 0; i < (RC[2].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 189 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 190 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 191 | pc.locate(10,22); |
maetugr | 14:cf260677ecde | 192 | pc.printf("RC3: %4d :[", RC[3].read()); |
maetugr | 12:67a06c9b69d5 | 193 | for (int i = 0; i < (RC[3].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 194 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 195 | pc.printf(" "); |
maetugr | 10:953afcbcebfc | 196 | LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 197 | } |
maetugr | 0:0c4fafa398b4 | 198 | } |