![](/media/cache/profiles/8ad6635f33710af6a535e8fb8486975e.50x50_q85.jpg)
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
- Committer:
- maetugr
- Date:
- 2012-11-17
- Revision:
- 23:955a7c7ddf8b
- Parent:
- 22:d301b455a1ad
- Child:
- 24:c5a3cba48498
File content as of revision 23:955a7c7ddf8b:
#include "mbed.h" // Standard Library #include "LED.h" // LEDs framework for blinking ;) #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) #include "L3G4200D.h" // Gyro (Gyroscope) #include "ADXL345.h" // Acc (Accelerometer) #include "HMC5883.h" // Comp (Compass) #include "BMP085_old.h" // Alt (Altitude sensor) #include "RC_Channel.h" // RemoteControl Chnnels with PPM #include "Servo_PWM.h" // Motor PPM using PwmOut #include "PID.h" // PID Library by Aaron Berk //#define RAD2DEG 57.295779513082320876798154814105 // ratio between radians and degree (360/2Pi) //TODO not needed?? #define RATE 0.02 // speed of the interrupt for Sensors and PID #define P_VALUE 0.035 // PID values #define I_VALUE 3.5 #define D_VALUE 0.04 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start #define PC_CONNECTED // decoment if you want to debug per USB and your PC Timer GlobalTimer; // global time to calculate processing speed Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC // initialisation of hardware (see includes for more info) LED LEDs; #ifdef PC_CONNECTED PC pc(USBTX, USBRX, 115200); #endif L3G4200D Gyro(p28, p27); ADXL345 Acc(p28, p27); HMC5883 Comp(p28, p27); BMP085_old Alt(p28, p27); RC_Channel RC[] = {p11, p12, p13, p14}; // noooo p19/p20 ! Servo_PWM Motor[] = {p21, p22, p23, p24}; // p21 - p26 only ! // 0:X:Roll 1:Y:Pitch 2:Z:Yaw 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 // global variables bool armed = false; // this variable is for security unsigned long dt_get_data = 0; // TODO: dt namen unsigned long time_get_data = 0; unsigned long dt_read_sensors = 0; unsigned long time_read_sensors = 0; float angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw] float tempangle = 0; // temporärer winkel für yaw ohne kompass float Gyro_angle[3] ={0,0,0}; float controller_value[] = {0,0,0}; float motor_value[] = {0,0,0,0}; float motor_calc(int rc_value, float contr_value) { return rc_value + contr_value > 50 ? rc_value + contr_value : 50; // nicht unter 0 sinken TODO: nicht Motor halten -> langsame Reaktion } void get_Data() // method which is called by the Ticker Datagetter every RATE seconds { time_read_sensors = GlobalTimer.read_us(); // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes Gyro.read(); Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig //Comp.read(); //Alt.Update(); TODO braucht zu lange zum auslesen! dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; // meassure dt dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop time_get_data = GlobalTimer.read_us(); // set new time for next measurement Gyro_angle[0] += Gyro.data[0] *dt_get_data/15000000.0; Gyro_angle[1] += Gyro.data[1] *dt_get_data/15000000.0; Gyro_angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // calculate angles for roll, pitch an yaw angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0; angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0; angle[2] = Gyro_angle[2]; // gyro only here // PID controlling if (!(RC[0].read() == -100)) { Controller[0].setSetPoint(-(int)((RC[0].read()-440)/440.0*90.0)); Controller[1].setSetPoint(-(int)((RC[1].read()-430)/430.0*90.0)); } for(int i=0;i<3;i++) { Controller[i].setProcessValue(angle[i]); controller_value[i] = Controller[i].compute() - 1000; } // Arming / disarming if(RC[2].read() < 20 && RC[3].read() > 850) armed = true; if(RC[2].read() < 30 && RC[3].read() < 30) armed = false; if(RC[3].read() < -10 || RC[2].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) armed = false; // calculate new motorspeeds if (armed) // for SECURITY! { // Pitch motor_value[0] = motor_calc(RC[2].read(), +controller_value[1]); motor_value[2] = motor_calc(RC[2].read(), -controller_value[1]); #if 0 // Roll motor_value[1] = motor_calc(RC[2].read(), +controller_value[0]); motor_value[3] = motor_calc(RC[2].read(), -controller_value[0]); // Yaw motor_value[0] -= controller_value[2]; motor_value[2] -= controller_value[2]; motor_value[1] += controller_value[2]; motor_value[3] += controller_value[2]; #endif } else { for(int i=0;i<4;i++) motor_value[i] = 0; } // Set new motorspeeds for(int i=0;i<4;i++) Motor[i] = (int)motor_value[i]; } int main() { // main programm only used for initialisation and debug output NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0) for(int i=0;i<3;i++) { Controller[i].setInputLimits(-90.0, 90.0); Controller[i].setOutputLimits(0.0, 2000.0); Controller[i].setBias(1000); Controller[i].setMode(MANUAL_MODE);//AUTO_MODE); Controller[i].setSetPoint(0); } #ifdef PC_CONNECTED #ifdef COMPASSCALIBRATE pc.locate(10,5); pc.printf("CALIBRATING"); Comp.calibrate(60); #endif // init screen pc.locate(10,5); pc.printf("Flybed v0.2"); #endif LEDs.roll(2); // Start! GlobalTimer.start(); Datagetter.attach(&get_Data, RATE); // start to get data all RATEms while(1) { #ifdef PC_CONNECTED pc.locate(30,0); // PC output pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); pc.locate(5,1); if(armed) pc.printf("ARMED!!!!!!!!!!!!!"); else pc.printf("DIS_ARMED "); pc.locate(5,3); pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); pc.printf("\n\r control Roll: %d control Pitch: %d ", (int)((RC[0].read()-440)/440.0*90.0), (int)((RC[1].read()-430)/430.0*90.0)); pc.locate(5,5); pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); pc.locate(5,6); pc.printf("Gyro_angle: X:%6.1f Y:%6.1f Z:%6.1f", Gyro_angle[0], Gyro_angle[1], Gyro_angle[2]); pc.locate(5,8); pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); pc.locate(5,9); pc.printf("Acc.angle: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); pc.locate(5,11); pc.printf("PID Result:"); for(int i=0;i<3;i++) pc.printf(" %d: %6.1f", i, controller_value[i]); pc.locate(5,12); pc.printf("Motor Result:"); for(int i=0;i<4;i++) pc.printf(" %d: %6.1f", i, motor_value[i]); pc.locate(5,14); pc.printf(" roll: %d pitch: %d ", -(int)((RC[0].read()-440)/440.0*90.0), -(int)((RC[1].read()-430)/430.0*90.0)); pc.locate(10,15); pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); pc.locate(10,16); pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle()); pc.locate(10,17); //pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private pc.locate(10,18); pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle()); pc.locate(10,19); pc.printf("RC0: %4d :[", RC[0].read()); for (int i = 0; i < RC[0].read()/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,20); pc.printf("RC1: %4d :[", RC[1].read()); for (int i = 0; i < RC[1].read()/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,21); pc.printf("RC2: %4d :[", RC[2].read()); for (int i = 0; i < RC[2].read()/17; i++) pc.printf("="); pc.printf(" "); pc.locate(10,22); pc.printf("RC3: %4d :[", RC[3].read()); for (int i = 0; i < RC[3].read()/17; i++) pc.printf("="); pc.printf(" "); #endif wait(0.01); if(armed){ LEDs.rollnext(); } else { LEDs.set(1); LEDs.set(2); LEDs.set(3); LEDs.set(4); } } }