Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
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?
User | Revision | Line number | New 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 | } |