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@25:0498d3041afa, 2012-11-26 (annotated)

Committer:
maetugr
Date:
Mon Nov 26 16:11:28 2012 +0000
Revision:
25:0498d3041afa
Parent:
24:c5a3cba48498
Child:
26:96a072233d7a
vor restrukturierung

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 7:9d4313510646 4 #include "L3G4200D.h" // Gyro (Gyroscope)
maetugr 7:9d4313510646 5 #include "ADXL345.h" // Acc (Accelerometer)
maetugr 7:9d4313510646 6 #include "HMC5883.h" // Comp (Compass)
maetugr 14:cf260677ecde 7 #include "BMP085_old.h" // Alt (Altitude sensor)
maetugr 7:9d4313510646 8 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
maetugr 15:753c5d6a63b3 9 #include "Servo_PWM.h" // Motor PPM using PwmOut
maetugr 13:4737ee9ebfee 10 #include "PID.h" // PID Library by Aaron Berk
maetugr 0:0c4fafa398b4 11
maetugr 21:c2a2e7cbabdd 12 //#define RAD2DEG 57.295779513082320876798154814105 // ratio between radians and degree (360/2Pi) //TODO not needed??
maetugr 15:753c5d6a63b3 13 #define RATE 0.02 // speed of the interrupt for Sensors and PID
maetugr 15:753c5d6a63b3 14
maetugr 24:c5a3cba48498 15
maetugr 25:0498d3041afa 16 #define P_VALUE 0.02 // PID values
maetugr 25:0498d3041afa 17 #define I_VALUE 20 // Werte die bis jetzt am besten funktioniert haben
maetugr 25:0498d3041afa 18 #define D_VALUE 0.004
maetugr 25:0498d3041afa 19
maetugr 24:c5a3cba48498 20
maetugr 24:c5a3cba48498 21 /*
maetugr 24:c5a3cba48498 22 // agressive Werte
maetugr 23:955a7c7ddf8b 23 #define P_VALUE 0.035 // PID values
maetugr 23:955a7c7ddf8b 24 #define I_VALUE 3.5
maetugr 23:955a7c7ddf8b 25 #define D_VALUE 0.04
maetugr 24:c5a3cba48498 26 */
maetugr 12:67a06c9b69d5 27
maetugr 14:cf260677ecde 28 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
maetugr 21:c2a2e7cbabdd 29 #define PC_CONNECTED // decoment if you want to debug per USB and your PC
maetugr 2:93f703d2c4d7 30
maetugr 14:cf260677ecde 31 Timer GlobalTimer; // global time to calculate processing speed
maetugr 14:cf260677ecde 32 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
maetugr 14:cf260677ecde 33
maetugr 14:cf260677ecde 34 // initialisation of hardware (see includes for more info)
maetugr 5:818c0668fd2d 35 LED LEDs;
maetugr 21:c2a2e7cbabdd 36 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 37 PC pc(USBTX, USBRX, 115200);
maetugr 21:c2a2e7cbabdd 38 #endif
maetugr 25:0498d3041afa 39 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
maetugr 25:0498d3041afa 40 FILE *Logger;
maetugr 5:818c0668fd2d 41 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 42 ADXL345 Acc(p28, p27);
maetugr 11:9bf69bc6df45 43 HMC5883 Comp(p28, p27);
maetugr 14:cf260677ecde 44 BMP085_old Alt(p28, p27);
maetugr 21:c2a2e7cbabdd 45 RC_Channel RC[] = {p11, p12, p13, p14}; // noooo p19/p20 !
maetugr 21:c2a2e7cbabdd 46 Servo_PWM Motor[] = {p21, p22, p23, p24}; // p21 - p26 only !
maetugr 8:d25ecdcdbeb5 47
maetugr 21:c2a2e7cbabdd 48 // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 22:d301b455a1ad 49 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
maetugr 14:cf260677ecde 50
maetugr 22:d301b455a1ad 51 // global variables
maetugr 20:e116e596e540 52 bool armed = false; // this variable is for security
maetugr 14:cf260677ecde 53 unsigned long dt_get_data = 0; // TODO: dt namen
maetugr 12:67a06c9b69d5 54 unsigned long time_get_data = 0;
maetugr 12:67a06c9b69d5 55 unsigned long dt_read_sensors = 0;
maetugr 12:67a06c9b69d5 56 unsigned long time_read_sensors = 0;
maetugr 14:cf260677ecde 57 float angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
maetugr 14:cf260677ecde 58 float tempangle = 0; // temporärer winkel für yaw ohne kompass
maetugr 20:e116e596e540 59 float Gyro_angle[3] ={0,0,0};
maetugr 21:c2a2e7cbabdd 60 float controller_value[] = {0,0,0};
maetugr 21:c2a2e7cbabdd 61 float motor_value[] = {0,0,0,0};
maetugr 21:c2a2e7cbabdd 62
maetugr 14:cf260677ecde 63 void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
maetugr 8:d25ecdcdbeb5 64 {
maetugr 12:67a06c9b69d5 65 time_read_sensors = GlobalTimer.read_us();
maetugr 12:67a06c9b69d5 66
maetugr 15:753c5d6a63b3 67 // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
maetugr 14:cf260677ecde 68 Gyro.read();
maetugr 15:753c5d6a63b3 69 Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig
maetugr 21:c2a2e7cbabdd 70 //Comp.read();
maetugr 13:4737ee9ebfee 71 //Alt.Update(); TODO braucht zu lange zum auslesen!
maetugr 12:67a06c9b69d5 72
maetugr 12:67a06c9b69d5 73 dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
maetugr 8:d25ecdcdbeb5 74
maetugr 8:d25ecdcdbeb5 75 // meassure dt
maetugr 12:67a06c9b69d5 76 dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop
maetugr 12:67a06c9b69d5 77 time_get_data = GlobalTimer.read_us(); // set new time for next measurement
maetugr 8:d25ecdcdbeb5 78
maetugr 24:c5a3cba48498 79 for(int i = 0; i < 3; i++)
maetugr 24:c5a3cba48498 80 Gyro_angle[i] += Gyro.data[i] *dt_get_data/15000000.0;
maetugr 20:e116e596e540 81
maetugr 8:d25ecdcdbeb5 82 // calculate angles for roll, pitch an yaw
maetugr 24:c5a3cba48498 83 #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
maetugr 24:c5a3cba48498 84 angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
maetugr 25:0498d3041afa 85 angle[1] += (Acc.angle[1] - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen
maetugr 24:c5a3cba48498 86 //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
maetugr 24:c5a3cba48498 87 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 24:c5a3cba48498 88 #endif
maetugr 12:67a06c9b69d5 89
maetugr 24:c5a3cba48498 90 #if 1 // neuer Test 1 (Formel von http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
maetugr 24:c5a3cba48498 91 angle[0] = (0.98*(angle[0]+(Gyro.data[0] *dt_get_data/15000000.0)))+(0.02*(Acc.angle[0]));
maetugr 24:c5a3cba48498 92 angle[1] = (0.98*(angle[1]+(Gyro.data[1] *dt_get_data/15000000.0)))+(0.02*(Acc.angle[1] + 3)); // TODO Offset accelerometer einstellen
maetugr 24:c5a3cba48498 93 //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
maetugr 24:c5a3cba48498 94 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 24:c5a3cba48498 95 #endif
maetugr 24:c5a3cba48498 96
maetugr 24:c5a3cba48498 97 #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
maetugr 24:c5a3cba48498 98 angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
maetugr 24:c5a3cba48498 99 angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
maetugr 24:c5a3cba48498 100 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 24:c5a3cba48498 101 #endif
maetugr 24:c5a3cba48498 102
maetugr 24:c5a3cba48498 103 #if 0 // rein Gyro
maetugr 24:c5a3cba48498 104 for(int i = 0; i < 3; i++)
maetugr 24:c5a3cba48498 105 angle[i] = Gyro_angle[i];
maetugr 24:c5a3cba48498 106 #endif
maetugr 8:d25ecdcdbeb5 107
maetugr 21:c2a2e7cbabdd 108 // Arming / disarming
maetugr 25:0498d3041afa 109 if(RC[2].read() < 20 && RC[3].read() > 850) {
maetugr 21:c2a2e7cbabdd 110 armed = true;
maetugr 25:0498d3041afa 111 #ifdef LOGGER
maetugr 25:0498d3041afa 112 if(Logger == NULL)
maetugr 25:0498d3041afa 113 Logger = fopen("/local/log.csv", "a");
maetugr 25:0498d3041afa 114 #endif
maetugr 25:0498d3041afa 115 }
maetugr 25:0498d3041afa 116 if((RC[2].read() < 30 && RC[3].read() < 30) || RC[3].read() < -10 || RC[2].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) {
maetugr 20:e116e596e540 117 armed = false;
maetugr 25:0498d3041afa 118 #ifdef LOGGER
maetugr 25:0498d3041afa 119 if(Logger != NULL) {
maetugr 25:0498d3041afa 120 fclose(Logger);
maetugr 25:0498d3041afa 121 Logger = NULL;
maetugr 25:0498d3041afa 122 }
maetugr 25:0498d3041afa 123 #endif
maetugr 25:0498d3041afa 124 }
maetugr 20:e116e596e540 125
maetugr 21:c2a2e7cbabdd 126 if (armed) // for SECURITY!
maetugr 22:d301b455a1ad 127 {
maetugr 24:c5a3cba48498 128 // PID controlling
maetugr 24:c5a3cba48498 129 if (!(RC[0].read() == -100)) {
maetugr 25:0498d3041afa 130 //Controller[0].setSetPoint(-(int)((RC[0].read()-440)/440.0*90.0));
maetugr 25:0498d3041afa 131 //Controller[1].setSetPoint(-(int)((RC[1].read()-430)/430.0*90.0));
maetugr 24:c5a3cba48498 132 //Controller[2].setSetPoint(-(int)((RC[3].read()-424)/424.0*180.0)); // TODO: muss später += werden
maetugr 24:c5a3cba48498 133 }
maetugr 24:c5a3cba48498 134 for(int i=0;i<3;i++) {
maetugr 24:c5a3cba48498 135 Controller[i].setProcessValue(angle[i]);
maetugr 24:c5a3cba48498 136 controller_value[i] = Controller[i].compute() - 1000;
maetugr 24:c5a3cba48498 137 }
maetugr 24:c5a3cba48498 138
maetugr 24:c5a3cba48498 139 // Calculate new motorspeeds
maetugr 21:c2a2e7cbabdd 140 // Pitch
maetugr 25:0498d3041afa 141 motor_value[0] = RC[2].read() +controller_value[1];
maetugr 25:0498d3041afa 142 motor_value[2] = RC[2].read() -controller_value[1];
maetugr 21:c2a2e7cbabdd 143
maetugr 24:c5a3cba48498 144 #if 1
maetugr 22:d301b455a1ad 145 // Roll
maetugr 25:0498d3041afa 146 motor_value[1] = RC[2].read() +controller_value[0];
maetugr 25:0498d3041afa 147 motor_value[3] = RC[2].read() -controller_value[0];
maetugr 22:d301b455a1ad 148
maetugr 25:0498d3041afa 149 /*// Yaw
maetugr 22:d301b455a1ad 150 motor_value[0] -= controller_value[2];
maetugr 22:d301b455a1ad 151 motor_value[2] -= controller_value[2];
maetugr 22:d301b455a1ad 152
maetugr 22:d301b455a1ad 153 motor_value[1] += controller_value[2];
maetugr 25:0498d3041afa 154 motor_value[3] += controller_value[2];*/
maetugr 22:d301b455a1ad 155 #endif
maetugr 21:c2a2e7cbabdd 156
maetugr 25:0498d3041afa 157 for(int i = 0; i < 4; i++) // make shure no motor stands still
maetugr 25:0498d3041afa 158 motor_value[i] = motor_value[i] > 50 ? motor_value[i] : 50;
maetugr 25:0498d3041afa 159
maetugr 25:0498d3041afa 160 #ifdef LOGGER
maetugr 25:0498d3041afa 161 // Writing Log
maetugr 25:0498d3041afa 162 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 163 fprintf(Logger, "%f;", angle[i]);
maetugr 25:0498d3041afa 164 fprintf(Logger, "%f;", controller_value[i]);
maetugr 25:0498d3041afa 165 }
maetugr 25:0498d3041afa 166 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 167 #endif
maetugr 15:753c5d6a63b3 168 } else {
maetugr 24:c5a3cba48498 169 for(int i=0;i<3;i++)
maetugr 24:c5a3cba48498 170 Controller[i].reset();
maetugr 21:c2a2e7cbabdd 171 for(int i=0;i<4;i++)
maetugr 21:c2a2e7cbabdd 172 motor_value[i] = 0;
maetugr 21:c2a2e7cbabdd 173 }
maetugr 21:c2a2e7cbabdd 174 // Set new motorspeeds
maetugr 21:c2a2e7cbabdd 175 for(int i=0;i<4;i++)
maetugr 22:d301b455a1ad 176 Motor[i] = (int)motor_value[i];
maetugr 8:d25ecdcdbeb5 177 }
maetugr 5:818c0668fd2d 178
maetugr 14:cf260677ecde 179 int main() { // main programm only used for initialisation and debug output
maetugr 16:74a6531350b5 180 NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)
maetugr 15:753c5d6a63b3 181
maetugr 25:0498d3041afa 182 #ifdef LOGGER
maetugr 25:0498d3041afa 183 Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
maetugr 25:0498d3041afa 184 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 185 fprintf(Logger, "angle[%d];", i);
maetugr 25:0498d3041afa 186 fprintf(Logger, "controller_value[%d];", i);
maetugr 25:0498d3041afa 187 }
maetugr 25:0498d3041afa 188 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 189 fclose(Logger);
maetugr 25:0498d3041afa 190 Logger = NULL;
maetugr 25:0498d3041afa 191 #endif
maetugr 25:0498d3041afa 192
maetugr 25:0498d3041afa 193 // Prepare PID Controllers
maetugr 21:c2a2e7cbabdd 194 for(int i=0;i<3;i++) {
maetugr 21:c2a2e7cbabdd 195 Controller[i].setInputLimits(-90.0, 90.0);
maetugr 21:c2a2e7cbabdd 196 Controller[i].setOutputLimits(0.0, 2000.0);
maetugr 21:c2a2e7cbabdd 197 Controller[i].setBias(1000);
maetugr 21:c2a2e7cbabdd 198 Controller[i].setMode(MANUAL_MODE);//AUTO_MODE);
maetugr 21:c2a2e7cbabdd 199 Controller[i].setSetPoint(0);
maetugr 21:c2a2e7cbabdd 200 }
maetugr 24:c5a3cba48498 201 Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad
maetugr 12:67a06c9b69d5 202
maetugr 21:c2a2e7cbabdd 203 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 204 #ifdef COMPASSCALIBRATE
maetugr 21:c2a2e7cbabdd 205 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 206 pc.printf("CALIBRATING");
maetugr 21:c2a2e7cbabdd 207 Comp.calibrate(60);
maetugr 21:c2a2e7cbabdd 208 #endif
maetugr 21:c2a2e7cbabdd 209
maetugr 21:c2a2e7cbabdd 210 // init screen
maetugr 12:67a06c9b69d5 211 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 212 pc.printf("Flybed v0.2");
maetugr 12:67a06c9b69d5 213 #endif
maetugr 1:5a64632b1eb9 214 LEDs.roll(2);
maetugr 5:818c0668fd2d 215
maetugr 21:c2a2e7cbabdd 216 // Start!
maetugr 2:93f703d2c4d7 217 GlobalTimer.start();
maetugr 12:67a06c9b69d5 218 Datagetter.attach(&get_Data, RATE); // start to get data all RATEms
maetugr 12:67a06c9b69d5 219
maetugr 12:67a06c9b69d5 220 while(1) {
maetugr 21:c2a2e7cbabdd 221 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 222 pc.locate(30,0); // PC output
maetugr 21:c2a2e7cbabdd 223 pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 21:c2a2e7cbabdd 224 pc.locate(5,1);
maetugr 21:c2a2e7cbabdd 225 if(armed)
maetugr 21:c2a2e7cbabdd 226 pc.printf("ARMED!!!!!!!!!!!!!");
maetugr 21:c2a2e7cbabdd 227 else
maetugr 21:c2a2e7cbabdd 228 pc.printf("DIS_ARMED ");
maetugr 21:c2a2e7cbabdd 229 pc.locate(5,3);
maetugr 21:c2a2e7cbabdd 230 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
maetugr 21:c2a2e7cbabdd 231 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));
maetugr 21:c2a2e7cbabdd 232
maetugr 21:c2a2e7cbabdd 233 pc.locate(5,5);
maetugr 21:c2a2e7cbabdd 234 pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
maetugr 21:c2a2e7cbabdd 235 pc.locate(5,6);
maetugr 21:c2a2e7cbabdd 236 pc.printf("Gyro_angle: X:%6.1f Y:%6.1f Z:%6.1f", Gyro_angle[0], Gyro_angle[1], Gyro_angle[2]);
maetugr 21:c2a2e7cbabdd 237
maetugr 21:c2a2e7cbabdd 238 pc.locate(5,8);
maetugr 21:c2a2e7cbabdd 239 pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 21:c2a2e7cbabdd 240 pc.locate(5,9);
maetugr 21:c2a2e7cbabdd 241 pc.printf("Acc.angle: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
maetugr 21:c2a2e7cbabdd 242
maetugr 21:c2a2e7cbabdd 243 pc.locate(5,11);
maetugr 21:c2a2e7cbabdd 244 pc.printf("PID Result:");
maetugr 21:c2a2e7cbabdd 245 for(int i=0;i<3;i++)
maetugr 21:c2a2e7cbabdd 246 pc.printf(" %d: %6.1f", i, controller_value[i]);
maetugr 21:c2a2e7cbabdd 247
maetugr 23:955a7c7ddf8b 248 pc.locate(5,12);
maetugr 22:d301b455a1ad 249 pc.printf("Motor Result:");
maetugr 22:d301b455a1ad 250 for(int i=0;i<4;i++)
maetugr 22:d301b455a1ad 251 pc.printf(" %d: %6.1f", i, motor_value[i]);
maetugr 21:c2a2e7cbabdd 252
maetugr 23:955a7c7ddf8b 253 pc.locate(5,14);
maetugr 23:955a7c7ddf8b 254 pc.printf(" roll: %d pitch: %d ", -(int)((RC[0].read()-440)/440.0*90.0), -(int)((RC[1].read()-430)/430.0*90.0));
maetugr 23:955a7c7ddf8b 255
maetugr 21:c2a2e7cbabdd 256 pc.locate(10,15);
maetugr 21:c2a2e7cbabdd 257 pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle);
maetugr 21:c2a2e7cbabdd 258 pc.locate(10,16);
maetugr 21:c2a2e7cbabdd 259 pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
maetugr 21:c2a2e7cbabdd 260 pc.locate(10,17);
maetugr 21:c2a2e7cbabdd 261 //pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private
maetugr 21:c2a2e7cbabdd 262 pc.locate(10,18);
maetugr 21:c2a2e7cbabdd 263 pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
maetugr 21:c2a2e7cbabdd 264
maetugr 21:c2a2e7cbabdd 265 pc.locate(10,19);
maetugr 21:c2a2e7cbabdd 266 pc.printf("RC0: %4d :[", RC[0].read());
maetugr 21:c2a2e7cbabdd 267 for (int i = 0; i < RC[0].read()/17; i++)
maetugr 21:c2a2e7cbabdd 268 pc.printf("=");
maetugr 21:c2a2e7cbabdd 269 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 270 pc.locate(10,20);
maetugr 21:c2a2e7cbabdd 271 pc.printf("RC1: %4d :[", RC[1].read());
maetugr 21:c2a2e7cbabdd 272 for (int i = 0; i < RC[1].read()/17; i++)
maetugr 21:c2a2e7cbabdd 273 pc.printf("=");
maetugr 21:c2a2e7cbabdd 274 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 275 pc.locate(10,21);
maetugr 21:c2a2e7cbabdd 276 pc.printf("RC2: %4d :[", RC[2].read());
maetugr 21:c2a2e7cbabdd 277 for (int i = 0; i < RC[2].read()/17; i++)
maetugr 21:c2a2e7cbabdd 278 pc.printf("=");
maetugr 21:c2a2e7cbabdd 279 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 280 pc.locate(10,22);
maetugr 21:c2a2e7cbabdd 281 pc.printf("RC3: %4d :[", RC[3].read());
maetugr 21:c2a2e7cbabdd 282 for (int i = 0; i < RC[3].read()/17; i++)
maetugr 21:c2a2e7cbabdd 283 pc.printf("=");
maetugr 21:c2a2e7cbabdd 284 pc.printf(" ");
maetugr 21:c2a2e7cbabdd 285 #endif
maetugr 21:c2a2e7cbabdd 286 wait(0.01);
maetugr 21:c2a2e7cbabdd 287 if(armed){
maetugr 21:c2a2e7cbabdd 288 LEDs.rollnext();
maetugr 21:c2a2e7cbabdd 289 } else {
maetugr 21:c2a2e7cbabdd 290 LEDs.set(1);
maetugr 21:c2a2e7cbabdd 291 LEDs.set(2);
maetugr 21:c2a2e7cbabdd 292 LEDs.set(3);
maetugr 21:c2a2e7cbabdd 293 LEDs.set(4);
maetugr 21:c2a2e7cbabdd 294 }
maetugr 0:0c4fafa398b4 295 }
maetugr 0:0c4fafa398b4 296 }