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@40:2ca410923691, 2014-02-14 (annotated)

Committer:
maetugr
Date:
Fri Feb 14 14:17:32 2014 +0000
Revision:
40:2ca410923691
Parent:
39:9fd3f4439978 
now with MPU6050 before taking it too FlyBed2

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 40:2ca410923691 4 #include "MPU6050.h" // Combined Gyro and Acc
maetugr 40:2ca410923691 5 //#include "L3G4200D.h" // Gyro (Gyroscope)
maetugr 40:2ca410923691 6 //#include "ADXL345.h" // Acc (Accelerometer)
maetugr 40:2ca410923691 7 //#include "HMC5883.h" // Comp (Compass)
maetugr 40:2ca410923691 8 //#include "BMP085_old.h" // Alt (Altitude sensor)
maetugr 30:021e13b62575 9 #include "RC_Channel.h" // RemoteControl Channels with PPM
maetugr 15:753c5d6a63b3 10 #include "Servo_PWM.h" // Motor PPM using PwmOut
maetugr 39:9fd3f4439978 11 #include "PID.h" // PID Library (slim, self written)
maetugr 26:96a072233d7a 12 #include "IMU_Filter.h" // Class to calculate position angles
maetugr 26:96a072233d7a 13 #include "Mixer.h" // Class to calculate motorspeeds from Angles, Regulation and RC-Signals
maetugr 0:0c4fafa398b4 14
maetugr 30:021e13b62575 15 #define RATE 0.002 // speed of the interrupt for Sensors and PID
maetugr 30:021e13b62575 16 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 33:fd98776b6cc7 17 #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for
maetugr 36:128c55793728 18 #define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate
maetugr 37:34917f7c10ae 19 #define INTEGRAL_MAX 300
maetugr 15:753c5d6a63b3 20
maetugr 34:3aa1cbcde59d 21 // RC
maetugr 34:3aa1cbcde59d 22 #define AILERON 0
maetugr 34:3aa1cbcde59d 23 #define ELEVATOR 1
maetugr 34:3aa1cbcde59d 24 #define RUDDER 2
maetugr 34:3aa1cbcde59d 25 #define THROTTLE 3
maetugr 34:3aa1cbcde59d 26 // Axes
maetugr 34:3aa1cbcde59d 27 #define ROLL 0
maetugr 34:3aa1cbcde59d 28 #define PITCH 1
maetugr 34:3aa1cbcde59d 29 #define YAW 2
maetugr 25:0498d3041afa 30
maetugr 40:2ca410923691 31 #define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC
maetugr 2:93f703d2c4d7 32
maetugr 34:3aa1cbcde59d 33 // Global variables
maetugr 34:3aa1cbcde59d 34 bool armed = false; // this variable is for security (when false no motor rotates any more)
maetugr 37:34917f7c10ae 35 bool RC_present = false; // this variable shows if an RC is present
maetugr 34:3aa1cbcde59d 36 float dt = 0;
maetugr 34:3aa1cbcde59d 37 float time_for_dt = 0;
maetugr 34:3aa1cbcde59d 38 float dt_read_sensors = 0;
maetugr 34:3aa1cbcde59d 39 float time_read_sensors = 0;
maetugr 34:3aa1cbcde59d 40 float controller_value[] = {0,0,0}; // The calculated answer form the Controller
maetugr 34:3aa1cbcde59d 41 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
maetugr 37:34917f7c10ae 42 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
maetugr 34:3aa1cbcde59d 43
maetugr 35:2a9465fedb99 44 float P = 4.0; // PID values
maetugr 34:3aa1cbcde59d 45 float I = 0;
maetugr 38:ff95fd524c9e 46 float D = 0.1;
maetugr 38:ff95fd524c9e 47
maetugr 38:ff95fd524c9e 48 float PY = 0; // PID values for YAW
maetugr 38:ff95fd524c9e 49 float IY = 0;
maetugr 38:ff95fd524c9e 50 float DY = 0;
maetugr 34:3aa1cbcde59d 51
maetugr 14:cf260677ecde 52 Timer GlobalTimer; // global time to calculate processing speed
maetugr 34:3aa1cbcde59d 53 Ticker Dutycycler; // timecontrolled interrupt for exact timed control loop
maetugr 14:cf260677ecde 54
maetugr 34:3aa1cbcde59d 55 // Initialisation of hardware (see includes for more info)
maetugr 5:818c0668fd2d 56 LED LEDs;
maetugr 21:c2a2e7cbabdd 57 #ifdef PC_CONNECTED
maetugr 40:2ca410923691 58 PC pc(USBTX, USBRX, 115200); // USB
maetugr 40:2ca410923691 59 //PC pc(p9, p10, 115200); // Bluetooth
maetugr 21:c2a2e7cbabdd 60 #endif
maetugr 40:2ca410923691 61 MPU6050 Sensor(p28, p27);
maetugr 40:2ca410923691 62 //L3G4200D Gyro(p28, p27);
maetugr 40:2ca410923691 63 //ADXL345 Acc(p28, p27);
maetugr 40:2ca410923691 64 //HMC5883 Comp(p28, p27);
maetugr 40:2ca410923691 65 //BMP085_old Alt(p28, p27);
maetugr 34:3aa1cbcde59d 66 RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 !
maetugr 34:3aa1cbcde59d 67 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 34:3aa1cbcde59d 68 IMU_Filter IMU; // (don't write () after constructor for no arguments!)
maetugr 34:3aa1cbcde59d 69 Mixer MIX(1); // 0 for +-Formation, 1 for X-Formation
maetugr 38:ff95fd524c9e 70 PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 21:c2a2e7cbabdd 71
maetugr 39:9fd3f4439978 72 void dutycycle() { // method which is called by the Ticker Dutycycler every RATE seconds
maetugr 33:fd98776b6cc7 73 time_read_sensors = GlobalTimer.read(); // start time measure for sensors
maetugr 12:67a06c9b69d5 74
maetugr 15:753c5d6a63b3 75 // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
maetugr 40:2ca410923691 76 Sensor.read();
maetugr 40:2ca410923691 77 //Gyro.read();
maetugr 40:2ca410923691 78 //Acc.read();
maetugr 34:3aa1cbcde59d 79 //Comp.read(); // TODO: not every loop every sensor? altitude not that important
maetugr 34:3aa1cbcde59d 80 //Alt.Update(); // TODO needs very long to read because of waits
maetugr 12:67a06c9b69d5 81
maetugr 37:34917f7c10ae 82 //pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 35:2a9465fedb99 83
maetugr 38:ff95fd524c9e 84
maetugr 8:d25ecdcdbeb5 85
maetugr 33:fd98776b6cc7 86 // meassure dt for the filter
maetugr 33:fd98776b6cc7 87 dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
maetugr 33:fd98776b6cc7 88 time_for_dt = GlobalTimer.read(); // set new time for next measurement
maetugr 12:67a06c9b69d5 89
maetugr 40:2ca410923691 90 IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc);
maetugr 40:2ca410923691 91 //IMU.compute(dt, Gyro.data, Acc.data);
maetugr 35:2a9465fedb99 92 //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors);
maetugr 35:2a9465fedb99 93
maetugr 37:34917f7c10ae 94 if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100)
maetugr 37:34917f7c10ae 95 RC_present = false;
maetugr 37:34917f7c10ae 96 else
maetugr 37:34917f7c10ae 97 RC_present = true;
maetugr 37:34917f7c10ae 98
maetugr 21:c2a2e7cbabdd 99 // Arming / disarming
maetugr 34:3aa1cbcde59d 100 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
maetugr 21:c2a2e7cbabdd 101 armed = true;
maetugr 38:ff95fd524c9e 102 RC_angle[YAW] = IMU.angle[YAW];
maetugr 25:0498d3041afa 103 }
maetugr 37:34917f7c10ae 104 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
maetugr 20:e116e596e540 105 armed = false;
maetugr 25:0498d3041afa 106 }
maetugr 20:e116e596e540 107
maetugr 37:34917f7c10ae 108 // RC Angle ROLL-PITCH-Part
maetugr 33:fd98776b6cc7 109 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
maetugr 37:34917f7c10ae 110 if (RC_present)
maetugr 37:34917f7c10ae 111 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
maetugr 37:34917f7c10ae 112 else
maetugr 33:fd98776b6cc7 113 RC_angle[i] = 0;
maetugr 33:fd98776b6cc7 114 }
maetugr 37:34917f7c10ae 115
maetugr 37:34917f7c10ae 116 // RC Angle YAW-Part
maetugr 37:34917f7c10ae 117 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 37:34917f7c10ae 118 RC_yaw_adding = (RC[RUDDER].read()-500)*YAWSPEED/500;
maetugr 37:34917f7c10ae 119 else
maetugr 37:34917f7c10ae 120 RC_yaw_adding = 0;
maetugr 37:34917f7c10ae 121
maetugr 37:34917f7c10ae 122 while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180
maetugr 37:34917f7c10ae 123 if(RC_angle[YAW] + RC_yaw_adding < -180)
maetugr 37:34917f7c10ae 124 RC_yaw_adding += 360;
maetugr 37:34917f7c10ae 125 if(RC_angle[YAW] + RC_yaw_adding > 180)
maetugr 37:34917f7c10ae 126 RC_yaw_adding -= 360;
maetugr 37:34917f7c10ae 127 }
maetugr 37:34917f7c10ae 128 RC_angle[YAW] += RC_yaw_adding; // for yaw angle it's integrated
maetugr 30:021e13b62575 129
maetugr 34:3aa1cbcde59d 130 // PID controlling
maetugr 37:34917f7c10ae 131 for(int i=0;i<2;i++) {
maetugr 29:8b7362a2ee14 132 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 37:34917f7c10ae 133 controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
maetugr 30:021e13b62575 134 }
maetugr 37:34917f7c10ae 135 Controller[YAW].setIntegrate(armed); // same for YAW
maetugr 37:34917f7c10ae 136 if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180
maetugr 37:34917f7c10ae 137 if (RC_angle[YAW] > IMU.angle[YAW])
maetugr 37:34917f7c10ae 138 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW]);
maetugr 37:34917f7c10ae 139 else
maetugr 37:34917f7c10ae 140 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW]);
maetugr 37:34917f7c10ae 141 else
maetugr 37:34917f7c10ae 142 controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW]);
maetugr 29:8b7362a2ee14 143
maetugr 21:c2a2e7cbabdd 144 if (armed) // for SECURITY!
maetugr 22:d301b455a1ad 145 {
maetugr 34:3aa1cbcde59d 146 MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
maetugr 28:ba6ca9f4def4 147 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 26:96a072233d7a 148 ESC[i] = (int)MIX.Motor_speed[i];
maetugr 25:0498d3041afa 149
maetugr 15:753c5d6a63b3 150 } else {
maetugr 26:96a072233d7a 151 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 28:ba6ca9f4def4 152 ESC[i] = 0;
maetugr 21:c2a2e7cbabdd 153 }
maetugr 40:2ca410923691 154 /*pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n",
maetugr 38:ff95fd524c9e 155 armed,
maetugr 38:ff95fd524c9e 156 dt,
maetugr 38:ff95fd524c9e 157 dt_read_sensors,
maetugr 38:ff95fd524c9e 158 IMU.angle[ROLL],
maetugr 38:ff95fd524c9e 159 IMU.angle[PITCH],
maetugr 38:ff95fd524c9e 160 IMU.angle[YAW],
maetugr 38:ff95fd524c9e 161 RC_angle[ROLL],
maetugr 38:ff95fd524c9e 162 RC_angle[PITCH],
maetugr 38:ff95fd524c9e 163 RC_angle[YAW],
maetugr 38:ff95fd524c9e 164 controller_value[ROLL],
maetugr 38:ff95fd524c9e 165 controller_value[PITCH],
maetugr 38:ff95fd524c9e 166 controller_value[YAW],
maetugr 38:ff95fd524c9e 167 MIX.Motor_speed[0],
maetugr 38:ff95fd524c9e 168 MIX.Motor_speed[1],
maetugr 38:ff95fd524c9e 169 MIX.Motor_speed[2],
maetugr 40:2ca410923691 170 MIX.Motor_speed[3]);*/
maetugr 40:2ca410923691 171
maetugr 40:2ca410923691 172 pc.printf("%f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n",
maetugr 40:2ca410923691 173 IMU.angle[ROLL],
maetugr 40:2ca410923691 174 IMU.angle[PITCH],
maetugr 40:2ca410923691 175 IMU.angle[YAW],
maetugr 40:2ca410923691 176 Sensor.data_gyro[0],
maetugr 40:2ca410923691 177 Sensor.data_gyro[1],
maetugr 40:2ca410923691 178 Sensor.data_gyro[2],
maetugr 40:2ca410923691 179 Sensor.data_acc[0],
maetugr 40:2ca410923691 180 Sensor.data_acc[1],
maetugr 40:2ca410923691 181 Sensor.data_acc[2],
maetugr 40:2ca410923691 182 controller_value[ROLL],
maetugr 40:2ca410923691 183 controller_value[PITCH],
maetugr 40:2ca410923691 184 controller_value[YAW],
maetugr 40:2ca410923691 185 MIX.Motor_speed[0],
maetugr 40:2ca410923691 186 MIX.Motor_speed[1],
maetugr 40:2ca410923691 187 MIX.Motor_speed[2],
maetugr 38:ff95fd524c9e 188 MIX.Motor_speed[3]);
maetugr 38:ff95fd524c9e 189
maetugr 38:ff95fd524c9e 190 dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop
maetugr 8:d25ecdcdbeb5 191 }
maetugr 5:818c0668fd2d 192
maetugr 33:fd98776b6cc7 193 void commandexecuter(char* command) { // take new PID values on the fly
maetugr 31:872d8b8c7812 194 if (command[0] == 'p')
maetugr 38:ff95fd524c9e 195 if (command[1] == 'y')
maetugr 38:ff95fd524c9e 196 PY = atof(&command[2]);
maetugr 38:ff95fd524c9e 197 else
maetugr 38:ff95fd524c9e 198 P = atof(&command[1]);
maetugr 31:872d8b8c7812 199 if (command[0] == 'i')
maetugr 38:ff95fd524c9e 200 if (command[1] == 'y')
maetugr 38:ff95fd524c9e 201 IY = atof(&command[2]);
maetugr 38:ff95fd524c9e 202 else
maetugr 38:ff95fd524c9e 203 I = atof(&command[1]);
maetugr 31:872d8b8c7812 204 if (command[0] == 'd')
maetugr 38:ff95fd524c9e 205 if (command[1] == 'y')
maetugr 38:ff95fd524c9e 206 DY = atof(&command[2]);
maetugr 38:ff95fd524c9e 207 else
maetugr 38:ff95fd524c9e 208 D = atof(&command[1]);
maetugr 33:fd98776b6cc7 209 for(int i=0;i<2;i++) {
maetugr 33:fd98776b6cc7 210 Controller[i].setPID(P,I,D); // give the controller the new PID values
maetugr 31:872d8b8c7812 211 }
maetugr 38:ff95fd524c9e 212 Controller[YAW].setPID(PY,IY,DY); // give the controller the new PID values
maetugr 31:872d8b8c7812 213 }
maetugr 31:872d8b8c7812 214
maetugr 26:96a072233d7a 215 int main() { // main programm for initialisation and debug output
maetugr 26:96a072233d7a 216 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 217
maetugr 21:c2a2e7cbabdd 218 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 219 // init screen
maetugr 12:67a06c9b69d5 220 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 221 pc.printf("Flybed v0.2");
maetugr 12:67a06c9b69d5 222 #endif
maetugr 1:5a64632b1eb9 223 LEDs.roll(2);
maetugr 5:818c0668fd2d 224
maetugr 40:2ca410923691 225 Sensor.calibrate(50, 0.02);
maetugr 40:2ca410923691 226 //Gyro.calibrate(50, 0.02);
maetugr 40:2ca410923691 227 //Acc.calibrate(50, 0.02);
maetugr 33:fd98776b6cc7 228
maetugr 21:c2a2e7cbabdd 229 // Start!
maetugr 2:93f703d2c4d7 230 GlobalTimer.start();
maetugr 39:9fd3f4439978 231 Dutycycler.attach(&dutycycle, RATE); // start to process all RATE seconds
maetugr 12:67a06c9b69d5 232
maetugr 12:67a06c9b69d5 233 while(1) {
maetugr 35:2a9465fedb99 234 #ifdef PC_CONNECTED
maetugr 35:2a9465fedb99 235 if (pc.readable()) // Get Serial input (polled because interrupts disturb I2C)
maetugr 35:2a9465fedb99 236 pc.readcommand(&commandexecuter);
maetugr 35:2a9465fedb99 237 //pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU
maetugr 35:2a9465fedb99 238 //pc.printf("%f,%f,%f,%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]);
maetugr 38:ff95fd524c9e 239 #if 0 //pc.cls();
maetugr 35:2a9465fedb99 240 pc.locate(20,0); // PC output
maetugr 35:2a9465fedb99 241 pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 35:2a9465fedb99 242 pc.locate(5,1);
maetugr 35:2a9465fedb99 243 if(armed)
maetugr 35:2a9465fedb99 244 pc.printf("ARMED!!!!!!!!!!!!!");
maetugr 35:2a9465fedb99 245 else
maetugr 35:2a9465fedb99 246 pc.printf("DIS_ARMED ");
maetugr 35:2a9465fedb99 247 pc.locate(5,3);
maetugr 35:2a9465fedb99 248 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
maetugr 35:2a9465fedb99 249 pc.locate(5,4);
maetugr 35:2a9465fedb99 250 pc.printf("q0:%6.1f q1:%6.1f q2:%6.1f q3:%6.1f ", IMU.q0, IMU.q1, IMU.q2, IMU.q3);
maetugr 35:2a9465fedb99 251 pc.locate(5,5);
maetugr 35:2a9465fedb99 252 pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
maetugr 35:2a9465fedb99 253 pc.locate(5,6);
maetugr 35:2a9465fedb99 254 pc.printf("Acc.data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 35:2a9465fedb99 255
maetugr 35:2a9465fedb99 256 pc.locate(5,8);
maetugr 38:ff95fd524c9e 257 pc.printf("P :%6.1f I :%6.1f D :%6.1f ", P, I, D);
maetugr 38:ff95fd524c9e 258 pc.locate(5,9);
maetugr 38:ff95fd524c9e 259 pc.printf("PY:%6.1f IY:%6.1f DY:%6.1f ", PY, IY, DY);
maetugr 35:2a9465fedb99 260
maetugr 35:2a9465fedb99 261 pc.locate(5,11);
maetugr 35:2a9465fedb99 262 pc.printf("PID Result:");
maetugr 35:2a9465fedb99 263 for(int i=0;i<3;i++)
maetugr 35:2a9465fedb99 264 pc.printf(" %d: %6.1f", i, controller_value[i]);
maetugr 35:2a9465fedb99 265 pc.locate(5,14);
maetugr 35:2a9465fedb99 266 pc.printf("RC angle: roll: %f pitch: %f yaw: %f ", RC_angle[0], RC_angle[1], RC_angle[2]);
maetugr 35:2a9465fedb99 267 pc.locate(5,16);
maetugr 35:2a9465fedb99 268 pc.printf("Motor: 0:%d 1:%d 2:%d 3:%d ", (int)MIX.Motor_speed[0], (int)MIX.Motor_speed[1], (int)MIX.Motor_speed[2], (int)MIX.Motor_speed[3]);
maetugr 35:2a9465fedb99 269
maetugr 35:2a9465fedb99 270 // RC
maetugr 35:2a9465fedb99 271 pc.locate(10,19);
maetugr 35:2a9465fedb99 272 pc.printf("RC0: %4d RC1: %4d RC2: %4d RC3: %4d ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
maetugr 35:2a9465fedb99 273
maetugr 35:2a9465fedb99 274 pc.locate(10,21);
maetugr 35:2a9465fedb99 275 pc.printf("Commandline: %s ", pc.command);
maetugr 35:2a9465fedb99 276 #endif
maetugr 21:c2a2e7cbabdd 277 #endif
maetugr 21:c2a2e7cbabdd 278 if(armed){
maetugr 21:c2a2e7cbabdd 279 LEDs.rollnext();
maetugr 21:c2a2e7cbabdd 280 } else {
maetugr 26:96a072233d7a 281 for(int i=1;i<=4;i++)
maetugr 26:96a072233d7a 282 LEDs.set(i);
maetugr 21:c2a2e7cbabdd 283 }
maetugr 29:8b7362a2ee14 284 wait(0.05);
maetugr 0:0c4fafa398b4 285 }
maetugr 28:ba6ca9f4def4 286 }