Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
main.cpp@8:609a2ad4c30e, 2015-11-19 (annotated)
- Committer:
- maetugr
- Date:
- Thu Nov 19 18:47:27 2015 +0000
- Revision:
- 8:609a2ad4c30e
- Parent:
- 7:90f876d47862
made I2C-Sensors working in parallel, added rolling buffer for PID derivative, played with the PID and frequency parameters in main
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:37f0c1e8fa66 | 1 | /* X- Configuration |
maetugr | 2:f0a9ecb4d049 | 2 | m2 m3 -- > |
maetugr | 2:f0a9ecb4d049 | 3 | \ / / \ / |
maetugr | 2:f0a9ecb4d049 | 4 | / \ V | |
maetugr | 2:f0a9ecb4d049 | 5 | m1 m0 \ |
maetugr | 2:f0a9ecb4d049 | 6 | ROLL PITCH */ |
maetugr | 0:37f0c1e8fa66 | 7 | #include "mbed.h" |
maetugr | 0:37f0c1e8fa66 | 8 | #include "LED.h" // LEDs framework for blinking ;) |
maetugr | 0:37f0c1e8fa66 | 9 | #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:37f0c1e8fa66 | 10 | |
maetugr | 0:37f0c1e8fa66 | 11 | #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085) |
maetugr | 0:37f0c1e8fa66 | 12 | #include "RC_Channel.h" // RemoteControl Channels with PPM |
maetugr | 0:37f0c1e8fa66 | 13 | #include "PID.h" // PID Library (slim, self written) |
maetugr | 0:37f0c1e8fa66 | 14 | #include "Servo.h" // Motor PPM using any DigitalOut Pin |
maetugr | 0:37f0c1e8fa66 | 15 | |
maetugr | 0:37f0c1e8fa66 | 16 | #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) |
maetugr | 0:37f0c1e8fa66 | 17 | #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors |
maetugr | 8:609a2ad4c30e | 18 | #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for |
maetugr | 0:37f0c1e8fa66 | 19 | #define YAWSPEED 1.0 // maximal speed of yaw rotation in degree per Rate |
maetugr | 0:37f0c1e8fa66 | 20 | #define AILERON 0 // RC |
maetugr | 0:37f0c1e8fa66 | 21 | #define ELEVATOR 1 |
maetugr | 0:37f0c1e8fa66 | 22 | #define RUDDER 2 |
maetugr | 0:37f0c1e8fa66 | 23 | #define THROTTLE 3 |
maetugr | 0:37f0c1e8fa66 | 24 | #define CHANNEL8 4 |
maetugr | 0:37f0c1e8fa66 | 25 | #define CHANNEL7 5 |
maetugr | 0:37f0c1e8fa66 | 26 | #define CHANNEL6 6 |
maetugr | 0:37f0c1e8fa66 | 27 | #define ROLL 0 // Axes |
maetugr | 0:37f0c1e8fa66 | 28 | #define PITCH 1 |
maetugr | 0:37f0c1e8fa66 | 29 | #define YAW 2 |
maetugr | 0:37f0c1e8fa66 | 30 | |
maetugr | 0:37f0c1e8fa66 | 31 | #define SQRT2 0.7071067811865 |
maetugr | 0:37f0c1e8fa66 | 32 | |
maetugr | 0:37f0c1e8fa66 | 33 | bool armed = false; // is for security (when false no motor rotates any more) |
maetugr | 2:f0a9ecb4d049 | 34 | bool debug = true; // shows if we want output for the computer |
maetugr | 3:3709be130495 | 35 | bool level = false; // switches between self leveling and acro mode |
maetugr | 0:37f0c1e8fa66 | 36 | bool RC_present = false; // shows if an RC is present |
maetugr | 8:609a2ad4c30e | 37 | float P_R = 2.6, I_R = 0.3, D_R = 0; // PID values for the rate controller |
maetugr | 8:609a2ad4c30e | 38 | float P_A = 1.9, I_A = 0.2, D_A = 0; // PID values for the angle controller P_A = 1.865, I_A = 1.765, D_A = 0 |
maetugr | 4:b2efa7f03701 | 39 | float PY = 2.3, IY = 0, DY = 0; // PID values for Yaw |
maetugr | 0:37f0c1e8fa66 | 40 | float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC |
maetugr | 0:37f0c1e8fa66 | 41 | float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send |
maetugr | 0:37f0c1e8fa66 | 42 | |
maetugr | 7:90f876d47862 | 43 | Timer LoopTimer; |
maetugr | 7:90f876d47862 | 44 | float Times[10] = {0,0,0,0,0,0,0,0,0,0}; |
maetugr | 8:609a2ad4c30e | 45 | float control_frequency = 800;//PPM_FREQU; // frequency for the main loop in Hz |
maetugr | 8:609a2ad4c30e | 46 | int counter = 0; |
maetugr | 8:609a2ad4c30e | 47 | int divider = 20; |
maetugr | 7:90f876d47862 | 48 | |
maetugr | 0:37f0c1e8fa66 | 49 | LED LEDs; |
maetugr | 8:609a2ad4c30e | 50 | //PC pc(USBTX, USBRX, 115200); // USB |
maetugr | 8:609a2ad4c30e | 51 | PC pc(p9, p10, 115200); // Bluetooth PIN: 1234 |
maetugr | 8:609a2ad4c30e | 52 | IMU_10DOF IMU(p5, p6, p7, p19, p28, p27); |
maetugr | 0:37f0c1e8fa66 | 53 | RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p15,2), RC_Channel(p17,4), RC_Channel(p16,3), RC_Channel(p25,2), RC_Channel(p26,4), RC_Channel(p29,3)}; // no p19/p20 ! |
maetugr | 0:37f0c1e8fa66 | 54 | PID Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw |
maetugr | 0:37f0c1e8fa66 | 55 | PID Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)}; |
maetugr | 0:37f0c1e8fa66 | 56 | Servo ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)}; // use any DigitalOit Pin |
maetugr | 0:37f0c1e8fa66 | 57 | |
maetugr | 0:37f0c1e8fa66 | 58 | extern "C" void mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 59 | |
maetugr | 7:90f876d47862 | 60 | void loop() { |
maetugr | 7:90f876d47862 | 61 | LoopTimer.start(); |
maetugr | 7:90f876d47862 | 62 | |
maetugr | 7:90f876d47862 | 63 | // IMU |
maetugr | 7:90f876d47862 | 64 | IMU.readAngles(); |
maetugr | 7:90f876d47862 | 65 | Times[1] = LoopTimer.read(); // 197us |
maetugr | 7:90f876d47862 | 66 | |
maetugr | 7:90f876d47862 | 67 | // Arming / disarming |
maetugr | 7:90f876d47862 | 68 | RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe |
maetugr | 7:90f876d47862 | 69 | if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { |
maetugr | 7:90f876d47862 | 70 | armed = true; |
maetugr | 7:90f876d47862 | 71 | RC_angle[YAW] = IMU.angle[YAW]; |
maetugr | 7:90f876d47862 | 72 | } |
maetugr | 7:90f876d47862 | 73 | if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { |
maetugr | 7:90f876d47862 | 74 | armed = false; |
maetugr | 7:90f876d47862 | 75 | } |
maetugr | 7:90f876d47862 | 76 | |
maetugr | 7:90f876d47862 | 77 | // Setting PID Values from auxiliary RC channels |
maetugr | 7:90f876d47862 | 78 | for(int i=0;i<3;i++) |
maetugr | 7:90f876d47862 | 79 | Controller_Angle[i].setPID(P_A,I_A,D_A); |
maetugr | 7:90f876d47862 | 80 | for(int i=0;i<2;i++) |
maetugr | 7:90f876d47862 | 81 | Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller |
maetugr | 7:90f876d47862 | 82 | Controller_Rate[YAW].setPID(PY,IY,DY); |
maetugr | 7:90f876d47862 | 83 | Times[2] = LoopTimer.read(); // 7us |
maetugr | 7:90f876d47862 | 84 | |
maetugr | 7:90f876d47862 | 85 | // RC Angle ROLL-PITCH-Part |
maetugr | 7:90f876d47862 | 86 | for(int i=0;i<2;i++) { // calculate new angle we want the QC to have |
maetugr | 7:90f876d47862 | 87 | if (RC_present) |
maetugr | 7:90f876d47862 | 88 | RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; |
maetugr | 7:90f876d47862 | 89 | else |
maetugr | 7:90f876d47862 | 90 | RC_angle[i] = 0; |
maetugr | 7:90f876d47862 | 91 | } |
maetugr | 7:90f876d47862 | 92 | |
maetugr | 7:90f876d47862 | 93 | // RC Angle YAW-Part |
maetugr | 7:90f876d47862 | 94 | float RC_yaw_adding; // temporary variable to take the desired yaw adjustment |
maetugr | 7:90f876d47862 | 95 | if (RC_present && RC[THROTTLE].read() > 20) |
maetugr | 7:90f876d47862 | 96 | RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; // the yaw angle is integrated from stick input |
maetugr | 7:90f876d47862 | 97 | else |
maetugr | 7:90f876d47862 | 98 | RC_yaw_adding = 0; |
maetugr | 7:90f876d47862 | 99 | |
maetugr | 7:90f876d47862 | 100 | RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding < -180 ? RC_angle[YAW] + 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; // make shure it's in the cycle -180 to 180 |
maetugr | 7:90f876d47862 | 101 | RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; |
maetugr | 7:90f876d47862 | 102 | Times[3] = LoopTimer.read(); // 6us |
maetugr | 7:90f876d47862 | 103 | |
maetugr | 7:90f876d47862 | 104 | // Controlling |
maetugr | 7:90f876d47862 | 105 | if (level) { |
maetugr | 7:90f876d47862 | 106 | for(int i=0;i<2;i++) { // LEVEL |
maetugr | 7:90f876d47862 | 107 | Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 8:609a2ad4c30e | 108 | if (counter % 16 == 0) |
maetugr | 8:609a2ad4c30e | 109 | Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angles and get his advice to correct |
maetugr | 7:90f876d47862 | 110 | Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 8:609a2ad4c30e | 111 | Controller_Rate[i].compute(-Controller_Angle[i].Value, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 8:609a2ad4c30e | 112 | //Controller_Rate[i].compute(-Controller_Angle[i].Value, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 ); |
maetugr | 7:90f876d47862 | 113 | } |
maetugr | 7:90f876d47862 | 114 | } else { |
maetugr | 7:90f876d47862 | 115 | for(int i=0;i<2;i++) { // ACRO |
maetugr | 7:90f876d47862 | 116 | Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 8:609a2ad4c30e | 117 | Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 8:609a2ad4c30e | 118 | //Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 ); |
maetugr | 7:90f876d47862 | 119 | } |
maetugr | 7:90f876d47862 | 120 | } |
maetugr | 7:90f876d47862 | 121 | |
maetugr | 7:90f876d47862 | 122 | Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 7:90f876d47862 | 123 | if (RC[THROTTLE].read() > 20) |
maetugr | 7:90f876d47862 | 124 | Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 7:90f876d47862 | 125 | else |
maetugr | 7:90f876d47862 | 126 | Controller_Rate[2].compute(0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 7:90f876d47862 | 127 | |
maetugr | 7:90f876d47862 | 128 | float throttle = 100 + (RC[THROTTLE].read() * 500 / 1000); |
maetugr | 7:90f876d47862 | 129 | Times[4] = LoopTimer.read(); // 53us |
maetugr | 7:90f876d47862 | 130 | |
maetugr | 7:90f876d47862 | 131 | // Mixing |
maetugr | 7:90f876d47862 | 132 | Motor_speed[0] = throttle +SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // X Configuration |
maetugr | 7:90f876d47862 | 133 | Motor_speed[1] = throttle -SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 7:90f876d47862 | 134 | Motor_speed[2] = throttle -SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 7:90f876d47862 | 135 | Motor_speed[3] = throttle +SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 7:90f876d47862 | 136 | |
maetugr | 7:90f876d47862 | 137 | Motor_speed[0] -= Controller_Rate[YAW].Value; |
maetugr | 8:609a2ad4c30e | 138 | Motor_speed[1] += Controller_Rate[YAW].Value; |
maetugr | 7:90f876d47862 | 139 | Motor_speed[2] -= Controller_Rate[YAW].Value; |
maetugr | 7:90f876d47862 | 140 | Motor_speed[3] += Controller_Rate[YAW].Value; |
maetugr | 7:90f876d47862 | 141 | Times[5] = LoopTimer.read(); // 17us |
maetugr | 7:90f876d47862 | 142 | |
maetugr | 7:90f876d47862 | 143 | if (armed) // for SECURITY! |
maetugr | 7:90f876d47862 | 144 | { |
maetugr | 7:90f876d47862 | 145 | debug = false; |
maetugr | 7:90f876d47862 | 146 | // PITCH |
maetugr | 8:609a2ad4c30e | 147 | //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50; |
maetugr | 8:609a2ad4c30e | 148 | //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50; |
maetugr | 7:90f876d47862 | 149 | // ROLL |
maetugr | 7:90f876d47862 | 150 | //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50; |
maetugr | 7:90f876d47862 | 151 | //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50; |
maetugr | 8:609a2ad4c30e | 152 | for(int i=0;i<4;i++) // Set new motorspeeds |
maetugr | 8:609a2ad4c30e | 153 | ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100; |
maetugr | 7:90f876d47862 | 154 | |
maetugr | 7:90f876d47862 | 155 | } else { |
maetugr | 7:90f876d47862 | 156 | for(int i=0;i<4;i++) // for security reason, set every motor to zero speed |
maetugr | 7:90f876d47862 | 157 | ESC[i] = 0; |
maetugr | 7:90f876d47862 | 158 | debug = true; |
maetugr | 7:90f876d47862 | 159 | } |
maetugr | 7:90f876d47862 | 160 | Times[6] = LoopTimer.read(); // 6us |
maetugr | 7:90f876d47862 | 161 | |
maetugr | 7:90f876d47862 | 162 | LEDs.rollnext(); |
maetugr | 7:90f876d47862 | 163 | |
maetugr | 8:609a2ad4c30e | 164 | /*if(counter % divider == 0) { |
maetugr | 8:609a2ad4c30e | 165 | pc.printf("%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]); |
maetugr | 8:609a2ad4c30e | 166 | }*/ |
maetugr | 8:609a2ad4c30e | 167 | counter++; |
maetugr | 7:90f876d47862 | 168 | |
maetugr | 8:609a2ad4c30e | 169 | Times[7] = LoopTimer.read(); // 7us TOTAL 297us |
maetugr | 8:609a2ad4c30e | 170 | while(LoopTimer.read() < 1/control_frequency); // Kill the rest of the time TODO: make a better solution so we can do misc things with these cycles |
maetugr | 7:90f876d47862 | 171 | Times[8] = LoopTimer.read(); |
maetugr | 7:90f876d47862 | 172 | LoopTimer.stop(); |
maetugr | 7:90f876d47862 | 173 | LoopTimer.reset(); |
maetugr | 7:90f876d47862 | 174 | |
maetugr | 7:90f876d47862 | 175 | |
maetugr | 7:90f876d47862 | 176 | if (debug) { |
maetugr | 7:90f876d47862 | 177 | pc.printf("$STATE,%d,%d,%.f,%.3f,%.3f\r\n", armed, level, control_frequency, IMU.dt*1e3, IMU.dt_sensors*1e6); |
maetugr | 7:90f876d47862 | 178 | //pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read()); |
maetugr | 8:609a2ad4c30e | 179 | pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]); |
maetugr | 8:609a2ad4c30e | 180 | pc.printf("$GYRO2,%.3f,%.3f,%.3f\r\n", IMU.mpu2.data_gyro[ROLL], IMU.mpu2.data_gyro[PITCH], IMU.mpu2.data_gyro[YAW]); |
maetugr | 7:90f876d47862 | 181 | //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]); |
maetugr | 7:90f876d47862 | 182 | pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]); |
maetugr | 7:90f876d47862 | 183 | //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]); |
maetugr | 7:90f876d47862 | 184 | pc.printf("$CONTR,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R, PY); |
maetugr | 7:90f876d47862 | 185 | pc.printf("$CONTA,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Angle[ROLL].Value, Controller_Angle[PITCH].Value, Controller_Angle[YAW].Value, P_A, I_A, D_A); |
maetugr | 7:90f876d47862 | 186 | pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]); |
maetugr | 8:609a2ad4c30e | 187 | /*pc.printf("$TIMES"); |
maetugr | 7:90f876d47862 | 188 | for(int i = 1; i < 10; i++) |
maetugr | 7:90f876d47862 | 189 | pc.printf(",%.3f", (Times[i]-Times[i-1])*1e6); |
maetugr | 8:609a2ad4c30e | 190 | pc.printf("\r\n");*/ |
maetugr | 7:90f876d47862 | 191 | wait(0.1); |
maetugr | 7:90f876d47862 | 192 | } |
maetugr | 7:90f876d47862 | 193 | } |
maetugr | 7:90f876d47862 | 194 | |
maetugr | 0:37f0c1e8fa66 | 195 | void executer() { |
maetugr | 0:37f0c1e8fa66 | 196 | char command = pc.getc(); |
maetugr | 0:37f0c1e8fa66 | 197 | if (command == 'X') |
maetugr | 0:37f0c1e8fa66 | 198 | mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 199 | if (command == '-') |
maetugr | 0:37f0c1e8fa66 | 200 | debug = !debug; |
maetugr | 2:f0a9ecb4d049 | 201 | |
maetugr | 3:3709be130495 | 202 | if (command == ':') |
maetugr | 3:3709be130495 | 203 | armed = true; |
maetugr | 3:3709be130495 | 204 | if (command == ' ') |
maetugr | 3:3709be130495 | 205 | armed = false; |
maetugr | 3:3709be130495 | 206 | |
maetugr | 3:3709be130495 | 207 | if (command == 'q') |
maetugr | 3:3709be130495 | 208 | level = true; |
maetugr | 3:3709be130495 | 209 | if (command == 'a') |
maetugr | 3:3709be130495 | 210 | level = false; |
maetugr | 3:3709be130495 | 211 | |
maetugr | 1:60882db03b0f | 212 | if (command == 'w') |
maetugr | 1:60882db03b0f | 213 | P_R += 0.1; |
maetugr | 1:60882db03b0f | 214 | if (command == 's') |
maetugr | 1:60882db03b0f | 215 | P_R -= 0.1; |
maetugr | 0:37f0c1e8fa66 | 216 | |
maetugr | 3:3709be130495 | 217 | if (command == 'e') |
maetugr | 5:8ea99e98de73 | 218 | I_R += 0.1; |
maetugr | 3:3709be130495 | 219 | if (command == 'd') |
maetugr | 5:8ea99e98de73 | 220 | I_R -= 0.1; |
maetugr | 3:3709be130495 | 221 | |
maetugr | 8:609a2ad4c30e | 222 | if (command == 'x') |
maetugr | 8:609a2ad4c30e | 223 | D_R += 0.001; |
maetugr | 8:609a2ad4c30e | 224 | if (command == 'c') |
maetugr | 8:609a2ad4c30e | 225 | D_R -= 0.001; |
maetugr | 8:609a2ad4c30e | 226 | |
maetugr | 3:3709be130495 | 227 | if (command == 'r') |
maetugr | 5:8ea99e98de73 | 228 | P_A += 0.1; |
maetugr | 3:3709be130495 | 229 | if (command == 'f') |
maetugr | 5:8ea99e98de73 | 230 | P_A -= 0.1; |
maetugr | 2:f0a9ecb4d049 | 231 | |
maetugr | 4:b2efa7f03701 | 232 | if (command == 't') |
maetugr | 4:b2efa7f03701 | 233 | I_A += 0.1; |
maetugr | 4:b2efa7f03701 | 234 | if (command == 'g') |
maetugr | 4:b2efa7f03701 | 235 | I_A -= 0.1; |
maetugr | 4:b2efa7f03701 | 236 | |
maetugr | 5:8ea99e98de73 | 237 | if (command == 'z') |
maetugr | 5:8ea99e98de73 | 238 | PY += 0.1; |
maetugr | 5:8ea99e98de73 | 239 | if (command == 'h') |
maetugr | 5:8ea99e98de73 | 240 | PY -= 0.1; |
maetugr | 5:8ea99e98de73 | 241 | |
maetugr | 8:609a2ad4c30e | 242 | if (command == 'o') { |
maetugr | 7:90f876d47862 | 243 | control_frequency += 100; |
maetugr | 8:609a2ad4c30e | 244 | |
maetugr | 7:90f876d47862 | 245 | } |
maetugr | 7:90f876d47862 | 246 | if (command == 'l') { |
maetugr | 7:90f876d47862 | 247 | control_frequency -= 100; |
maetugr | 8:609a2ad4c30e | 248 | |
maetugr | 8:609a2ad4c30e | 249 | } |
maetugr | 8:609a2ad4c30e | 250 | |
maetugr | 7:90f876d47862 | 251 | |
maetugr | 0:37f0c1e8fa66 | 252 | pc.putc(command); |
maetugr | 0:37f0c1e8fa66 | 253 | LEDs.tilt(2); |
maetugr | 0:37f0c1e8fa66 | 254 | } |
maetugr | 0:37f0c1e8fa66 | 255 | |
maetugr | 0:37f0c1e8fa66 | 256 | int main() { |
maetugr | 0:37f0c1e8fa66 | 257 | pc.attach(&executer); |
maetugr | 0:37f0c1e8fa66 | 258 | while(1) { |
maetugr | 7:90f876d47862 | 259 | loop(); |
maetugr | 0:37f0c1e8fa66 | 260 | } |
maetugr | 0:37f0c1e8fa66 | 261 | } |