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.
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?
User | Revision | Line number | New 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 | } |