Matthias Grob / Mbed 2 deprecated FlyBed1

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.

Dependencies:   mbed MODI2C

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?

UserRevisionLine numberNew 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 }