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@19:40c252b4a792, 2012-11-03 (annotated)
- Committer:
- maetugr
- Date:
- Sat Nov 03 10:48:18 2012 +0000
- Revision:
- 19:40c252b4a792
- Parent:
- 18:c8c09a3913ba
- Child:
- 20:e116e596e540
trotz i2c adress?nderung funktionieren sensoren nicht!
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 | 15:753c5d6a63b3 | 16 | #define P_VALUE 0.1 // viel zu tief!!!!! |
maetugr | 15:753c5d6a63b3 | 17 | #define I_VALUE 0.0 // |
maetugr | 15:753c5d6a63b3 | 18 | #define D_VALUE 0.0 // |
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 | 14:cf260677ecde | 41 | unsigned long dt_get_data = 0; // TODO: dt namen |
maetugr | 12:67a06c9b69d5 | 42 | unsigned long time_get_data = 0; |
maetugr | 12:67a06c9b69d5 | 43 | unsigned long dt_read_sensors = 0; |
maetugr | 12:67a06c9b69d5 | 44 | unsigned long time_read_sensors = 0; |
maetugr | 14:cf260677ecde | 45 | float angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw] |
maetugr | 14:cf260677ecde | 46 | float tempangle = 0; // temporärer winkel für yaw ohne kompass |
maetugr | 15:753c5d6a63b3 | 47 | float co; // PID test |
maetugr | 8:d25ecdcdbeb5 | 48 | |
maetugr | 14:cf260677ecde | 49 | void get_Data() // method which is called by the Ticker Datagetter every RATE seconds |
maetugr | 8:d25ecdcdbeb5 | 50 | { |
maetugr | 12:67a06c9b69d5 | 51 | time_read_sensors = GlobalTimer.read_us(); |
maetugr | 12:67a06c9b69d5 | 52 | |
maetugr | 15:753c5d6a63b3 | 53 | // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes |
maetugr | 14:cf260677ecde | 54 | Gyro.read(); |
maetugr | 15:753c5d6a63b3 | 55 | Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig |
maetugr | 15:753c5d6a63b3 | 56 | Comp.read(); |
maetugr | 13:4737ee9ebfee | 57 | //Alt.Update(); TODO braucht zu lange zum auslesen! |
maetugr | 12:67a06c9b69d5 | 58 | |
maetugr | 12:67a06c9b69d5 | 59 | dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; |
maetugr | 8:d25ecdcdbeb5 | 60 | |
maetugr | 8:d25ecdcdbeb5 | 61 | // meassure dt |
maetugr | 12:67a06c9b69d5 | 62 | dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop |
maetugr | 12:67a06c9b69d5 | 63 | time_get_data = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 8:d25ecdcdbeb5 | 64 | |
maetugr | 8:d25ecdcdbeb5 | 65 | // calculate angles for roll, pitch an yaw |
maetugr | 12:67a06c9b69d5 | 66 | angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0; |
maetugr | 12:67a06c9b69d5 | 67 | angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen |
maetugr | 12:67a06c9b69d5 | 68 | tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0; |
maetugr | 12:67a06c9b69d5 | 69 | angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here |
maetugr | 12:67a06c9b69d5 | 70 | |
maetugr | 15:753c5d6a63b3 | 71 | // PID controlling |
maetugr | 15:753c5d6a63b3 | 72 | Controller.setProcessValue(angle[1]); |
maetugr | 8:d25ecdcdbeb5 | 73 | |
maetugr | 12:67a06c9b69d5 | 74 | // calculate new motorspeeds |
maetugr | 15:753c5d6a63b3 | 75 | co = Controller.compute() - 1000; |
maetugr | 16:74a6531350b5 | 76 | if (RC[2].read() > 1100) // zu SICHERHEIT! zum testen (später ersetzen armed unarmed) |
maetugr | 15:753c5d6a63b3 | 77 | { |
maetugr | 15:753c5d6a63b3 | 78 | /*Motor[0] = RC[2].read()+((RC[0].read() - 1500)/10.0)+40; |
maetugr | 15:753c5d6a63b3 | 79 | Motor[2] = RC[2].read()-((RC[0].read() - 1500)/10.0)-40;*/ |
maetugr | 15:753c5d6a63b3 | 80 | Motor[0] = RC[2].read()+co+40; |
maetugr | 15:753c5d6a63b3 | 81 | Motor[2] = RC[2].read()-co-40; |
maetugr | 15:753c5d6a63b3 | 82 | } else { |
maetugr | 15:753c5d6a63b3 | 83 | Motor[0] = 1000; |
maetugr | 15:753c5d6a63b3 | 84 | Motor[1] = 1000; |
maetugr | 15:753c5d6a63b3 | 85 | Motor[2] = 1000; |
maetugr | 15:753c5d6a63b3 | 86 | Motor[3] = 1000; |
maetugr | 15:753c5d6a63b3 | 87 | } |
maetugr | 16:74a6531350b5 | 88 | /*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 | 89 | Motor[1] = 1000 + (100 - (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000; |
maetugr | 15:753c5d6a63b3 | 90 | Motor[2] = 1000 + (100 + (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000; |
maetugr | 15:753c5d6a63b3 | 91 | Motor[3] = 1000 + (100 + (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;*/ |
maetugr | 8:d25ecdcdbeb5 | 92 | } |
maetugr | 5:818c0668fd2d | 93 | |
maetugr | 14:cf260677ecde | 94 | int main() { // main programm only used for initialisation and debug output |
maetugr | 16:74a6531350b5 | 95 | NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0) |
maetugr | 15:753c5d6a63b3 | 96 | |
maetugr | 15:753c5d6a63b3 | 97 | //for(int i=0;i<3;i++) |
maetugr | 15:753c5d6a63b3 | 98 | Controller.setInputLimits(-90.0, 90.0); |
maetugr | 15:753c5d6a63b3 | 99 | Controller.setOutputLimits(0.0, 2000.0); |
maetugr | 15:753c5d6a63b3 | 100 | Controller.setBias(1000); |
maetugr | 15:753c5d6a63b3 | 101 | Controller.setMode(MANUAL_MODE);//AUTO_MODE); |
maetugr | 15:753c5d6a63b3 | 102 | Controller.setSetPoint(0); |
maetugr | 12:67a06c9b69d5 | 103 | |
maetugr | 12:67a06c9b69d5 | 104 | #ifdef COMPASSCALIBRATE |
maetugr | 12:67a06c9b69d5 | 105 | pc.locate(10,5); |
maetugr | 12:67a06c9b69d5 | 106 | pc.printf("CALIBRATING"); |
maetugr | 12:67a06c9b69d5 | 107 | Comp.calibrate(60); |
maetugr | 12:67a06c9b69d5 | 108 | #endif |
maetugr | 12:67a06c9b69d5 | 109 | |
maetugr | 6:179752756e9f | 110 | // init screen |
maetugr | 6:179752756e9f | 111 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 112 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 113 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 114 | |
maetugr | 14:cf260677ecde | 115 | // Start! TODO: Motor und RC start (armed....?) |
maetugr | 2:93f703d2c4d7 | 116 | GlobalTimer.start(); |
maetugr | 12:67a06c9b69d5 | 117 | Datagetter.attach(&get_Data, RATE); // start to get data all RATEms |
maetugr | 12:67a06c9b69d5 | 118 | |
maetugr | 12:67a06c9b69d5 | 119 | while(1) { |
maetugr | 9:4e0c3936c756 | 120 | pc.locate(10,5); // PC output |
maetugr | 12:67a06c9b69d5 | 121 | pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 7:9d4313510646 | 122 | pc.locate(10,8); |
maetugr | 6:179752756e9f | 123 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 10:953afcbcebfc | 124 | pc.locate(10,9); |
maetugr | 10:953afcbcebfc | 125 | pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); |
maetugr | 7:9d4313510646 | 126 | pc.locate(10,10); |
maetugr | 12:67a06c9b69d5 | 127 | pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); |
maetugr | 7:9d4313510646 | 128 | pc.locate(10,12); |
maetugr | 12:67a06c9b69d5 | 129 | 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 | 7:9d4313510646 | 130 | pc.locate(10,13); |
maetugr | 14:cf260677ecde | 131 | //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 | 10:953afcbcebfc | 132 | pc.locate(10,15); |
maetugr | 15:753c5d6a63b3 | 133 | pc.printf("PID Test: %6.1f", co); |
maetugr | 19:40c252b4a792 | 134 | pc.locate(10,16); |
maetugr | 19:40c252b4a792 | 135 | pc.printf("Gyro_data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); |
maetugr | 19:40c252b4a792 | 136 | pc.locate(10,17); |
maetugr | 19:40c252b4a792 | 137 | pc.printf("Acc_data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]); |
maetugr | 19:40c252b4a792 | 138 | pc.locate(10,18); |
maetugr | 19:40c252b4a792 | 139 | 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 | 140 | |
maetugr | 11:9bf69bc6df45 | 141 | pc.locate(10,19); |
maetugr | 14:cf260677ecde | 142 | pc.printf("RC0: %4d :[", RC[0].read()); |
maetugr | 12:67a06c9b69d5 | 143 | for (int i = 0; i < (RC[0].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 144 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 145 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 146 | pc.locate(10,20); |
maetugr | 14:cf260677ecde | 147 | pc.printf("RC1: %4d :[", RC[1].read()); |
maetugr | 12:67a06c9b69d5 | 148 | for (int i = 0; i < (RC[1].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 149 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 150 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 151 | pc.locate(10,21); |
maetugr | 14:cf260677ecde | 152 | pc.printf("RC2: %4d :[", RC[2].read()); |
maetugr | 12:67a06c9b69d5 | 153 | for (int i = 0; i < (RC[2].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 154 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 155 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 156 | pc.locate(10,22); |
maetugr | 14:cf260677ecde | 157 | pc.printf("RC3: %4d :[", RC[3].read()); |
maetugr | 12:67a06c9b69d5 | 158 | for (int i = 0; i < (RC[3].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 159 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 160 | pc.printf(" "); |
maetugr | 10:953afcbcebfc | 161 | LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 162 | } |
maetugr | 0:0c4fafa398b4 | 163 | } |