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@27:9e546fa47c33, 2012-11-28 (annotated)

Committer:
maetugr
Date:
Wed Nov 28 12:29:02 2012 +0000
Revision:
27:9e546fa47c33
Parent:
26:96a072233d7a
Child:
28:ba6ca9f4def4
after inserting RC-calibration (not tested yet)

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 26:96a072233d7a 11 #include "IMU_Filter.h" // Class to calculate position angles
maetugr 26:96a072233d7a 12 #include "Mixer.h" // Class to calculate motorspeeds from Angles, Regulation and RC-Signals
maetugr 0:0c4fafa398b4 13
maetugr 15:753c5d6a63b3 14 #define RATE 0.02 // speed of the interrupt for Sensors and PID
maetugr 15:753c5d6a63b3 15
maetugr 25:0498d3041afa 16 #define P_VALUE 0.02 // PID values
maetugr 25:0498d3041afa 17 #define I_VALUE 20 // Werte die bis jetzt am besten funktioniert haben
maetugr 25:0498d3041afa 18 #define D_VALUE 0.004
maetugr 25:0498d3041afa 19
maetugr 14:cf260677ecde 20 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
maetugr 21:c2a2e7cbabdd 21 #define PC_CONNECTED // decoment if you want to debug per USB and your PC
maetugr 2:93f703d2c4d7 22
maetugr 14:cf260677ecde 23 Timer GlobalTimer; // global time to calculate processing speed
maetugr 14:cf260677ecde 24 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
maetugr 14:cf260677ecde 25
maetugr 14:cf260677ecde 26 // initialisation of hardware (see includes for more info)
maetugr 5:818c0668fd2d 27 LED LEDs;
maetugr 21:c2a2e7cbabdd 28 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 29 PC pc(USBTX, USBRX, 115200);
maetugr 21:c2a2e7cbabdd 30 #endif
maetugr 25:0498d3041afa 31 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
maetugr 25:0498d3041afa 32 FILE *Logger;
maetugr 5:818c0668fd2d 33 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 34 ADXL345 Acc(p28, p27);
maetugr 11:9bf69bc6df45 35 HMC5883 Comp(p28, p27);
maetugr 14:cf260677ecde 36 BMP085_old Alt(p28, p27);
maetugr 27:9e546fa47c33 37 RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,3), RC_Channel(p14,4)}; // no p19/p20 !
maetugr 26:96a072233d7a 38 Servo_PWM ESC[] = {p21, p22, p23, p24}; // p21 - p26 only because PWM!
maetugr 26:96a072233d7a 39 IMU_Filter IMU; // don't write () after constructor for no arguments!
maetugr 26:96a072233d7a 40 Mixer MIX;
maetugr 8:d25ecdcdbeb5 41
maetugr 21:c2a2e7cbabdd 42 // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 22:d301b455a1ad 43 PID Controller[] = {PID(P_VALUE, I_VALUE, D_VALUE, RATE), PID(P_VALUE, I_VALUE, D_VALUE, RATE), PID(0.02, 100, 0.005, RATE)}; // TODO: RATE != dt immer anpassen
maetugr 14:cf260677ecde 44
maetugr 22:d301b455a1ad 45 // global variables
maetugr 20:e116e596e540 46 bool armed = false; // this variable is for security
maetugr 26:96a072233d7a 47 unsigned long dt = 0;
maetugr 26:96a072233d7a 48 unsigned long time_for_dt = 0;
maetugr 12:67a06c9b69d5 49 unsigned long dt_read_sensors = 0;
maetugr 12:67a06c9b69d5 50 unsigned long time_read_sensors = 0;
maetugr 26:96a072233d7a 51 float tempangle = 0; // temporärer winkel für yaw mit kompass
maetugr 21:c2a2e7cbabdd 52 float controller_value[] = {0,0,0};
maetugr 21:c2a2e7cbabdd 53
maetugr 14:cf260677ecde 54 void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
maetugr 8:d25ecdcdbeb5 55 {
maetugr 12:67a06c9b69d5 56 time_read_sensors = GlobalTimer.read_us();
maetugr 12:67a06c9b69d5 57
maetugr 15:753c5d6a63b3 58 // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
maetugr 14:cf260677ecde 59 Gyro.read();
maetugr 15:753c5d6a63b3 60 Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig
maetugr 21:c2a2e7cbabdd 61 //Comp.read();
maetugr 13:4737ee9ebfee 62 //Alt.Update(); TODO braucht zu lange zum auslesen!
maetugr 12:67a06c9b69d5 63
maetugr 12:67a06c9b69d5 64 dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
maetugr 8:d25ecdcdbeb5 65
maetugr 8:d25ecdcdbeb5 66 // meassure dt
maetugr 26:96a072233d7a 67 dt = GlobalTimer.read_us() - time_for_dt; // time in us since last loop
maetugr 26:96a072233d7a 68 time_for_dt = GlobalTimer.read_us(); // set new time for next measurement
maetugr 12:67a06c9b69d5 69
maetugr 26:96a072233d7a 70 // TODO: RC_signal füllen!!!
maetugr 24:c5a3cba48498 71
maetugr 26:96a072233d7a 72 IMU.compute(dt, Gyro.data, Acc.data);
maetugr 26:96a072233d7a 73 MIX.compute(dt, IMU.angle, RC[2].read(), controller_value);
maetugr 8:d25ecdcdbeb5 74
maetugr 21:c2a2e7cbabdd 75 // Arming / disarming
maetugr 25:0498d3041afa 76 if(RC[2].read() < 20 && RC[3].read() > 850) {
maetugr 21:c2a2e7cbabdd 77 armed = true;
maetugr 25:0498d3041afa 78 #ifdef LOGGER
maetugr 25:0498d3041afa 79 if(Logger == NULL)
maetugr 25:0498d3041afa 80 Logger = fopen("/local/log.csv", "a");
maetugr 25:0498d3041afa 81 #endif
maetugr 25:0498d3041afa 82 }
maetugr 25:0498d3041afa 83 if((RC[2].read() < 30 && RC[3].read() < 30) || RC[3].read() < -10 || RC[2].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) {
maetugr 20:e116e596e540 84 armed = false;
maetugr 25:0498d3041afa 85 #ifdef LOGGER
maetugr 25:0498d3041afa 86 if(Logger != NULL) {
maetugr 25:0498d3041afa 87 fclose(Logger);
maetugr 25:0498d3041afa 88 Logger = NULL;
maetugr 25:0498d3041afa 89 }
maetugr 25:0498d3041afa 90 #endif
maetugr 25:0498d3041afa 91 }
maetugr 20:e116e596e540 92
maetugr 21:c2a2e7cbabdd 93 if (armed) // for SECURITY!
maetugr 22:d301b455a1ad 94 {
maetugr 24:c5a3cba48498 95 // PID controlling
maetugr 24:c5a3cba48498 96 if (!(RC[0].read() == -100)) {
maetugr 25:0498d3041afa 97 //Controller[0].setSetPoint(-(int)((RC[0].read()-440)/440.0*90.0));
maetugr 25:0498d3041afa 98 //Controller[1].setSetPoint(-(int)((RC[1].read()-430)/430.0*90.0));
maetugr 24:c5a3cba48498 99 //Controller[2].setSetPoint(-(int)((RC[3].read()-424)/424.0*180.0)); // TODO: muss später += werden
maetugr 24:c5a3cba48498 100 }
maetugr 24:c5a3cba48498 101 for(int i=0;i<3;i++) {
maetugr 26:96a072233d7a 102 Controller[i].setProcessValue(IMU.angle[i]);
maetugr 24:c5a3cba48498 103 controller_value[i] = Controller[i].compute() - 1000;
maetugr 24:c5a3cba48498 104 }
maetugr 26:96a072233d7a 105 // Set new motorspeeds
maetugr 26:96a072233d7a 106 for(int i=0;i<4;i++)
maetugr 26:96a072233d7a 107 ESC[i] = (int)MIX.Motor_speed[i];
maetugr 25:0498d3041afa 108
maetugr 25:0498d3041afa 109 #ifdef LOGGER
maetugr 25:0498d3041afa 110 // Writing Log
maetugr 25:0498d3041afa 111 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 112 fprintf(Logger, "%f;", angle[i]);
maetugr 25:0498d3041afa 113 fprintf(Logger, "%f;", controller_value[i]);
maetugr 25:0498d3041afa 114 }
maetugr 25:0498d3041afa 115 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 116 #endif
maetugr 15:753c5d6a63b3 117 } else {
maetugr 26:96a072233d7a 118 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 26:96a072233d7a 119 ESC[i] = 0;
maetugr 24:c5a3cba48498 120 for(int i=0;i<3;i++)
maetugr 26:96a072233d7a 121 Controller[i].reset(); // TODO: schon ok so? anfangspeek?!
maetugr 21:c2a2e7cbabdd 122 }
maetugr 8:d25ecdcdbeb5 123 }
maetugr 5:818c0668fd2d 124
maetugr 26:96a072233d7a 125 int main() { // main programm for initialisation and debug output
maetugr 26:96a072233d7a 126 NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
maetugr 15:753c5d6a63b3 127
maetugr 25:0498d3041afa 128 #ifdef LOGGER
maetugr 25:0498d3041afa 129 Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
maetugr 25:0498d3041afa 130 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 131 fprintf(Logger, "angle[%d];", i);
maetugr 25:0498d3041afa 132 fprintf(Logger, "controller_value[%d];", i);
maetugr 25:0498d3041afa 133 }
maetugr 25:0498d3041afa 134 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 135 fclose(Logger);
maetugr 25:0498d3041afa 136 Logger = NULL;
maetugr 25:0498d3041afa 137 #endif
maetugr 25:0498d3041afa 138
maetugr 25:0498d3041afa 139 // Prepare PID Controllers
maetugr 21:c2a2e7cbabdd 140 for(int i=0;i<3;i++) {
maetugr 21:c2a2e7cbabdd 141 Controller[i].setInputLimits(-90.0, 90.0);
maetugr 21:c2a2e7cbabdd 142 Controller[i].setOutputLimits(0.0, 2000.0);
maetugr 21:c2a2e7cbabdd 143 Controller[i].setBias(1000);
maetugr 21:c2a2e7cbabdd 144 Controller[i].setMode(MANUAL_MODE);//AUTO_MODE);
maetugr 21:c2a2e7cbabdd 145 Controller[i].setSetPoint(0);
maetugr 21:c2a2e7cbabdd 146 }
maetugr 26:96a072233d7a 147 //Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad TODO: Yawsteuerung mit -180 bis 180 grad
maetugr 12:67a06c9b69d5 148
maetugr 21:c2a2e7cbabdd 149 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 150 #ifdef COMPASSCALIBRATE
maetugr 21:c2a2e7cbabdd 151 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 152 pc.printf("CALIBRATING");
maetugr 21:c2a2e7cbabdd 153 Comp.calibrate(60);
maetugr 21:c2a2e7cbabdd 154 #endif
maetugr 21:c2a2e7cbabdd 155
maetugr 21:c2a2e7cbabdd 156 // init screen
maetugr 12:67a06c9b69d5 157 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 158 pc.printf("Flybed v0.2");
maetugr 12:67a06c9b69d5 159 #endif
maetugr 1:5a64632b1eb9 160 LEDs.roll(2);
maetugr 5:818c0668fd2d 161
maetugr 21:c2a2e7cbabdd 162 // Start!
maetugr 2:93f703d2c4d7 163 GlobalTimer.start();
maetugr 12:67a06c9b69d5 164 Datagetter.attach(&get_Data, RATE); // start to get data all RATEms
maetugr 12:67a06c9b69d5 165
maetugr 12:67a06c9b69d5 166 while(1) {
maetugr 21:c2a2e7cbabdd 167 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 168 pc.locate(30,0); // PC output
maetugr 26:96a072233d7a 169 pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 21:c2a2e7cbabdd 170 pc.locate(5,1);
maetugr 21:c2a2e7cbabdd 171 if(armed)
maetugr 21:c2a2e7cbabdd 172 pc.printf("ARMED!!!!!!!!!!!!!");
maetugr 21:c2a2e7cbabdd 173 else
maetugr 21:c2a2e7cbabdd 174 pc.printf("DIS_ARMED ");
maetugr 21:c2a2e7cbabdd 175 pc.locate(5,3);
maetugr 26:96a072233d7a 176 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
maetugr 21:c2a2e7cbabdd 177
maetugr 21:c2a2e7cbabdd 178 pc.locate(5,5);
maetugr 21:c2a2e7cbabdd 179 pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
maetugr 21:c2a2e7cbabdd 180
maetugr 21:c2a2e7cbabdd 181 pc.locate(5,8);
maetugr 21:c2a2e7cbabdd 182 pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 21:c2a2e7cbabdd 183
maetugr 21:c2a2e7cbabdd 184 pc.locate(5,11);
maetugr 21:c2a2e7cbabdd 185 pc.printf("PID Result:");
maetugr 21:c2a2e7cbabdd 186 for(int i=0;i<3;i++)
maetugr 21:c2a2e7cbabdd 187 pc.printf(" %d: %6.1f", i, controller_value[i]);
maetugr 21:c2a2e7cbabdd 188
maetugr 23:955a7c7ddf8b 189 pc.locate(5,14);
maetugr 26:96a072233d7a 190 pc.printf("RC: roll: %d pitch: %d ", -(int)((RC[0].read()-440)/440.0*90.0), -(int)((RC[1].read()-430)/430.0*90.0));
maetugr 23:955a7c7ddf8b 191
maetugr 21:c2a2e7cbabdd 192 pc.locate(10,15);
maetugr 21:c2a2e7cbabdd 193 pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle);
maetugr 21:c2a2e7cbabdd 194 pc.locate(10,16);
maetugr 21:c2a2e7cbabdd 195 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 21:c2a2e7cbabdd 196 pc.locate(10,17);
maetugr 21:c2a2e7cbabdd 197 //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 21:c2a2e7cbabdd 198 pc.locate(10,18);
maetugr 21:c2a2e7cbabdd 199 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 21:c2a2e7cbabdd 200
maetugr 21:c2a2e7cbabdd 201 pc.locate(10,19);
maetugr 21:c2a2e7cbabdd 202 pc.printf("RC0: %4d :[", RC[0].read());
maetugr 21:c2a2e7cbabdd 203 for (int i = 0; i < RC[0].read()/17; i++)
maetugr 21:c2a2e7cbabdd 204 pc.printf("=");
maetugr 21:c2a2e7cbabdd 205 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 206 pc.locate(10,20);
maetugr 21:c2a2e7cbabdd 207 pc.printf("RC1: %4d :[", RC[1].read());
maetugr 21:c2a2e7cbabdd 208 for (int i = 0; i < RC[1].read()/17; i++)
maetugr 21:c2a2e7cbabdd 209 pc.printf("=");
maetugr 21:c2a2e7cbabdd 210 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 211 pc.locate(10,21);
maetugr 21:c2a2e7cbabdd 212 pc.printf("RC2: %4d :[", RC[2].read());
maetugr 21:c2a2e7cbabdd 213 for (int i = 0; i < RC[2].read()/17; i++)
maetugr 21:c2a2e7cbabdd 214 pc.printf("=");
maetugr 21:c2a2e7cbabdd 215 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 216 pc.locate(10,22);
maetugr 21:c2a2e7cbabdd 217 pc.printf("RC3: %4d :[", RC[3].read());
maetugr 21:c2a2e7cbabdd 218 for (int i = 0; i < RC[3].read()/17; i++)
maetugr 21:c2a2e7cbabdd 219 pc.printf("=");
maetugr 21:c2a2e7cbabdd 220 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 221 #endif
maetugr 21:c2a2e7cbabdd 222 wait(0.01);
maetugr 21:c2a2e7cbabdd 223 if(armed){
maetugr 21:c2a2e7cbabdd 224 LEDs.rollnext();
maetugr 21:c2a2e7cbabdd 225 } else {
maetugr 26:96a072233d7a 226 for(int i=1;i<=4;i++)
maetugr 26:96a072233d7a 227 LEDs.set(i);
maetugr 21:c2a2e7cbabdd 228 }
maetugr 0:0c4fafa398b4 229 }
maetugr 0:0c4fafa398b4 230 }