Matthias Grob / Mbed 2 deprecated FlyBed2

My fully self designed first stable working Quadrocopter Software.

Dependencies:   mbed

Dependents:   fluy343

/media/uploads/maetugr/dsc09031.jpg

main.cpp@1:5e2b81f2d0b4, 2013-09-08 (annotated)

Committer:
maetugr
Date:
Sun Sep 08 20:53:33 2013 +0000
Revision:
1:5e2b81f2d0b4
Parent:
0:12950aa67f2a
Child:
2:03e5f7ab473f
D only with Gyro as source is very stable on one axis;; next changing the PID class to get only gyro input for derivative

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:12950aa67f2a 1 #include "mbed.h"
maetugr 0:12950aa67f2a 2 #include "LED.h" // LEDs framework for blinking ;)
maetugr 0:12950aa67f2a 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 0:12950aa67f2a 4
maetugr 0:12950aa67f2a 5 #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085)
maetugr 0:12950aa67f2a 6 #include "RC_Channel.h" // RemoteControl Channels with PPM
maetugr 0:12950aa67f2a 7 #include "PID.h" // PID Library (slim, self written)
maetugr 0:12950aa67f2a 8 #include "Servo_PWM.h" // Motor PPM using PwmOut
maetugr 0:12950aa67f2a 9
maetugr 0:12950aa67f2a 10 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 0:12950aa67f2a 11 #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors
maetugr 0:12950aa67f2a 12 #define AILERON 0 // RC
maetugr 0:12950aa67f2a 13 #define ELEVATOR 1
maetugr 0:12950aa67f2a 14 #define RUDDER 2
maetugr 0:12950aa67f2a 15 #define THROTTLE 3
maetugr 1:5e2b81f2d0b4 16 #define CHANNEL8 4
maetugr 1:5e2b81f2d0b4 17 #define CHANNEL7 5
maetugr 1:5e2b81f2d0b4 18 #define CHANNEL6 6
maetugr 0:12950aa67f2a 19 #define ROLL 0 // Axes
maetugr 0:12950aa67f2a 20 #define PITCH 1
maetugr 0:12950aa67f2a 21 #define YAW 2
maetugr 0:12950aa67f2a 22
maetugr 0:12950aa67f2a 23 bool armed = false; // this variable is for security (when false no motor rotates any more)
maetugr 1:5e2b81f2d0b4 24 float P = 15, I = 0, D = 10; // PID values
maetugr 0:12950aa67f2a 25 float controller_value = 0; // The calculated answer form the Controller
maetugr 0:12950aa67f2a 26 float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send
maetugr 0:12950aa67f2a 27
maetugr 0:12950aa67f2a 28 LED LEDs;
maetugr 0:12950aa67f2a 29 PC pc(USBTX, USBRX, 921600); // USB
maetugr 1:5e2b81f2d0b4 30 //PC pc(p9, p10, 115200); // Bluetooth
maetugr 0:12950aa67f2a 31 IMU_10DOF IMU(p28, p27);
maetugr 1:5e2b81f2d0b4 32 RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p7,2), RC_Channel(p5,4), RC_Channel(p6,3), RC_Channel(p15,2), RC_Channel(p16,4), RC_Channel(p17,3)}; // no p19/p20 !
maetugr 0:12950aa67f2a 33 PID Controller(P, I, D, INTEGRAL_MAX); // X:Roll alone
maetugr 0:12950aa67f2a 34 Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
maetugr 0:12950aa67f2a 35
maetugr 1:5e2b81f2d0b4 36 extern "C" void mbed_reset();
maetugr 1:5e2b81f2d0b4 37
maetugr 0:12950aa67f2a 38 void executer() {
maetugr 1:5e2b81f2d0b4 39 char command = pc.getc();
maetugr 1:5e2b81f2d0b4 40 if (command == 'X')
maetugr 1:5e2b81f2d0b4 41 mbed_reset();
maetugr 0:12950aa67f2a 42 pc.putc(pc.getc());
maetugr 0:12950aa67f2a 43 LEDs.tilt(2);
maetugr 0:12950aa67f2a 44 }
maetugr 0:12950aa67f2a 45
maetugr 0:12950aa67f2a 46 int main() {
maetugr 0:12950aa67f2a 47 pc.attach(&executer);
maetugr 0:12950aa67f2a 48 while(1) {
maetugr 0:12950aa67f2a 49 // IMU
maetugr 0:12950aa67f2a 50 IMU.readAngles();
maetugr 0:12950aa67f2a 51 //IMU.readAltitude(); // reading altitude takes much more time than the angles -> don't do this in your fast loop
maetugr 0:12950aa67f2a 52 //pc.printf("%.1f,%.1f,%.1f,%.1f'C,%.1fhPa,%.1fmaS,%.5fs,%.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.temperature, IMU.pressure, IMU.altitude, IMU.dt, IMU.dt_sensors); // Output for Python
maetugr 0:12950aa67f2a 53
maetugr 0:12950aa67f2a 54 // Arming / disarming
maetugr 1:5e2b81f2d0b4 55 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
maetugr 1:5e2b81f2d0b4 56 armed = true;
maetugr 1:5e2b81f2d0b4 57 //RC_angle[YAW] = IMU.angle[YAW];
maetugr 1:5e2b81f2d0b4 58 }
maetugr 1:5e2b81f2d0b4 59 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || RC[THROTTLE].read() == -100) {
maetugr 0:12950aa67f2a 60 armed = false;
maetugr 0:12950aa67f2a 61 }
maetugr 0:12950aa67f2a 62
maetugr 1:5e2b81f2d0b4 63 // Setting PID Values
maetugr 1:5e2b81f2d0b4 64 if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
maetugr 1:5e2b81f2d0b4 65 P = ((float)RC[CHANNEL8].read()) * 30 / 1000;
maetugr 1:5e2b81f2d0b4 66 if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
maetugr 1:5e2b81f2d0b4 67 D = ((float)RC[CHANNEL7].read()) * 18 / 1000;
maetugr 1:5e2b81f2d0b4 68 Controller.setPID(P,I,D); // give the controller the new PID values
maetugr 1:5e2b81f2d0b4 69
maetugr 1:5e2b81f2d0b4 70 float rc;
maetugr 1:5e2b81f2d0b4 71 if (RC[ELEVATOR].read() != -100)
maetugr 1:5e2b81f2d0b4 72 rc = (RC[ELEVATOR].read() - 500)*0.05;
maetugr 1:5e2b81f2d0b4 73 else
maetugr 1:5e2b81f2d0b4 74 rc = 0;
maetugr 1:5e2b81f2d0b4 75
maetugr 0:12950aa67f2a 76 // Controlling
maetugr 0:12950aa67f2a 77 Controller.setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 1:5e2b81f2d0b4 78 controller_value = Controller.compute(0, IMU.angle[PITCH]); // give the controller the actual angle and get his advice to correct
maetugr 0:12950aa67f2a 79
maetugr 0:12950aa67f2a 80 // Mixing
maetugr 0:12950aa67f2a 81 if (armed) // for SECURITY!
maetugr 0:12950aa67f2a 82 {
maetugr 1:5e2b81f2d0b4 83 Motor_speed[0] = RC[THROTTLE].read() + controller_value;
maetugr 1:5e2b81f2d0b4 84 Motor_speed[2] = RC[THROTTLE].read() - controller_value;
maetugr 0:12950aa67f2a 85 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 0:12950aa67f2a 86 ESC[i] = (int)Motor_speed[i];
maetugr 0:12950aa67f2a 87
maetugr 0:12950aa67f2a 88 } else {
maetugr 0:12950aa67f2a 89 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 0:12950aa67f2a 90 ESC[i] = 0;
maetugr 0:12950aa67f2a 91 }
maetugr 0:12950aa67f2a 92
maetugr 1:5e2b81f2d0b4 93 //pc.printf("%d,%.3f,%.3f,%.3f,%.5fs,%.5fs,%4d,%4d,%4d,%4d\r\n", armed, IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.dt, IMU.dt_sensors, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
maetugr 1:5e2b81f2d0b4 94 pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%d,%.3f\r\n", armed, P, D, IMU.angle[PITCH], controller_value, IMU.Gyro.raw[1], IMU.Gyro.data[1], RC[ELEVATOR].read(), IMU.dt);
maetugr 0:12950aa67f2a 95 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.5f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.Gyro.data[0], IMU.Gyro.data[1], IMU.Gyro.data[2], IMU.dt);
maetugr 0:12950aa67f2a 96
maetugr 0:12950aa67f2a 97 //wait(0.01);
maetugr 0:12950aa67f2a 98
maetugr 0:12950aa67f2a 99 LEDs.rollnext();
maetugr 0:12950aa67f2a 100 }
maetugr 0:12950aa67f2a 101 }