Tanu Tanu / Mbed 2 deprecated RC_QuadCopter

Radio controlled Quadcopter, FRDM K64f, ZIgbee Radio Modules, ESCs

Dependencies:   FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 kalman mbed-dsp mbed

Fork of Freescale_Multi-Sensor_Quadcopter by Julio Fajardo

main.cpp@8:8f3dfde6fa88, 2017-04-01 (annotated)

Committer:
impeccabletanu
Date:
Sat Apr 01 01:52:18 2017 +0000
Revision:
8:8f3dfde6fa88
Parent:
7:72e41680e5e0 
RC Controlled Quadcopter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
screamer 0:bfb567985c64 1 #include "mbed.h"
screamer 0:bfb567985c64 2 #include "FXOS8700Q.h"
screamer 0:bfb567985c64 3 #include "FXAS21000.h"
julioefajardo 3:58ebc00f1a68 4 #include "kalman.c"
julioefajardo 6:d868495c1936 5 #include "arm_math.h"
screamer 0:bfb567985c64 6
vijeetgoel 7:72e41680e5e0 7 #define PI 3.14159265358979f
julioefajardo 6:d868495c1936 8 #define Rad2Dree 57.295779513082320876798154814105f
julioefajardo 6:d868495c1936 9
julioefajardo 6:d868495c1936 10 #define PID_ROLL_KP 0.0245f /* Proporcional */ //0.015f
julioefajardo 6:d868495c1936 11 #define PID_ROLL_KI 0.000175f /* Integral */
julioefajardo 6:d868495c1936 12 #define PID_ROLL_KD 0.0f /* Derivative */
julioefajardo 6:d868495c1936 13
julioefajardo 6:d868495c1936 14 #define PID_PITCH_KP 0.0245f /* Proporcional */ //0.015f
julioefajardo 6:d868495c1936 15 #define PID_PITCH_KI 0.000175f /* Integral */
julioefajardo 6:d868495c1936 16 #define PID_PITCH_KD 0.0f /* Derivative */
julioefajardo 6:d868495c1936 17
vijeetgoel 7:72e41680e5e0 18 #define PID_YAW_KP 0.0245f /* Proporcional */ //0.015f
vijeetgoel 7:72e41680e5e0 19 #define PID_YAW_KI 0.000175f /* Integral */
vijeetgoel 7:72e41680e5e0 20 #define PID_YAW_KD 0.0f /* Derivative */
vijeetgoel 7:72e41680e5e0 21
julioefajardo 6:d868495c1936 22 #define ROLL_SP PI/2
julioefajardo 6:d868495c1936 23 #define PITCH_SP PI/2
vijeetgoel 7:72e41680e5e0 24 #define YAW_SP PI/2
vijeetgoel 7:72e41680e5e0 25 FXOS8700Q_acc combo_acc(D14,D15,FXOS8700CQ_SLAVE_ADDR0);
vijeetgoel 7:72e41680e5e0 26 FXOS8700Q_mag combo_mag(D14,D15,FXOS8700CQ_SLAVE_ADDR0);
vijeetgoel 7:72e41680e5e0 27 FXAS21000 gyro(D14,D15);
julioefajardo 5:74ca8be12359 28
vijeetgoel 7:72e41680e5e0 29 AnalogIn ultra(A0);
julioefajardo 3:58ebc00f1a68 30 Timer GlobalTime;
julioefajardo 3:58ebc00f1a68 31 Timer ProgramTimer;
julioefajardo 3:58ebc00f1a68 32
vijeetgoel 7:72e41680e5e0 33 PwmOut esc1(D10);
vijeetgoel 7:72e41680e5e0 34 PwmOut esc2(D11);
vijeetgoel 7:72e41680e5e0 35 PwmOut esc3(D12);
vijeetgoel 7:72e41680e5e0 36 PwmOut esc4(D13);
vijeetgoel 7:72e41680e5e0 37 DigitalOut reset1(D7);
vijeetgoel 7:72e41680e5e0 38
screamer 0:bfb567985c64 39
julioefajardo 3:58ebc00f1a68 40 kalman filter_pitch;
julioefajardo 3:58ebc00f1a68 41 kalman filter_roll;
vijeetgoel 7:72e41680e5e0 42 kalman filter_yaw;
vijeetgoel 7:72e41680e5e0 43
vijeetgoel 7:72e41680e5e0 44 Serial xbe(D1,D0);
vijeetgoel 7:72e41680e5e0 45
vijeetgoel 7:72e41680e5e0 46 Serial pc(USBTX, USBRX);
vijeetgoel 7:72e41680e5e0 47 // tx, rx
julioefajardo 3:58ebc00f1a68 48
julioefajardo 3:58ebc00f1a68 49 float R;
julioefajardo 3:58ebc00f1a68 50 double angle[3];
julioefajardo 3:58ebc00f1a68 51 unsigned long timer;
julioefajardo 3:58ebc00f1a68 52 long loopStartTime;
julioefajardo 5:74ca8be12359 53 char i;
julioefajardo 5:74ca8be12359 54 char command = ' ';
julioefajardo 6:d868495c1936 55 char sflag = 0;
vijeetgoel 7:72e41680e5e0 56 float high;
julioefajardo 5:74ca8be12359 57
vijeetgoel 7:72e41680e5e0 58 float roll_error;
vijeetgoel 7:72e41680e5e0 59 float pitch_error;
vijeetgoel 7:72e41680e5e0 60 float yaw_error;
vijeetgoel 7:72e41680e5e0 61
vijeetgoel 7:72e41680e5e0 62 float roll;
vijeetgoel 7:72e41680e5e0 63 float pitch;
vijeetgoel 7:72e41680e5e0 64 float yaw;
julioefajardo 6:d868495c1936 65 float ESC1 = 0.0006f; //pitch up
julioefajardo 6:d868495c1936 66 float ESC2 = 0.0006f; //roll up
julioefajardo 6:d868495c1936 67 float ESC3 = 0.0006f; //roll down
julioefajardo 6:d868495c1936 68 float ESC4 = 0.0006f; //pitch down
julioefajardo 6:d868495c1936 69
vijeetgoel 7:72e41680e5e0 70 int main() {
vijeetgoel 7:72e41680e5e0 71
vijeetgoel 7:72e41680e5e0 72 char c;
vijeetgoel 7:72e41680e5e0 73 float gyro_data[3];
screamer 0:bfb567985c64 74 MotionSensorDataUnits adata;
screamer 0:bfb567985c64 75 MotionSensorDataUnits mdata;
julioefajardo 2:4bc4e25328cc 76
vijeetgoel 7:72e41680e5e0 77 arm_pid_instance_f32 RPID;
julioefajardo 6:d868495c1936 78 arm_pid_instance_f32 PPID;
vijeetgoel 7:72e41680e5e0 79 arm_pid_instance_f32 YPID;
julioefajardo 6:d868495c1936 80
julioefajardo 6:d868495c1936 81 //Pitch
julioefajardo 6:d868495c1936 82 PPID.Kp = PID_PITCH_KP/1000.0f; /* Proporcional */
julioefajardo 6:d868495c1936 83 PPID.Ki = PID_PITCH_KI/1000.0f; /* Integral */
julioefajardo 6:d868495c1936 84 PPID.Kd = PID_PITCH_KD/1000.0f; /* Derivative */
julioefajardo 6:d868495c1936 85
julioefajardo 6:d868495c1936 86 //Roll
julioefajardo 6:d868495c1936 87 RPID.Kp = PID_ROLL_KP/1000.0f; /* Proporcional */
julioefajardo 6:d868495c1936 88 RPID.Ki = PID_ROLL_KI/1000.0f; /* Integral */
julioefajardo 6:d868495c1936 89 RPID.Kd = PID_ROLL_KD/1000.0f; /* Derivative */
julioefajardo 6:d868495c1936 90
vijeetgoel 7:72e41680e5e0 91 //yaw
vijeetgoel 7:72e41680e5e0 92 YPID.Kp = PID_YAW_KP/1000.0f; /* Proporcional */
vijeetgoel 7:72e41680e5e0 93 YPID.Ki = PID_YAW_KI/1000.0f; /* Integral */
vijeetgoel 7:72e41680e5e0 94 YPID.Kd = PID_YAW_KD/1000.0f; /* Derivative */
vijeetgoel 7:72e41680e5e0 95
vijeetgoel 7:72e41680e5e0 96
julioefajardo 6:d868495c1936 97 arm_pid_init_f32(&RPID, 1);
julioefajardo 6:d868495c1936 98 arm_pid_init_f32(&PPID, 1);
vijeetgoel 7:72e41680e5e0 99 arm_pid_init_f32(&YPID, 1);
julioefajardo 6:d868495c1936 100
vijeetgoel 7:72e41680e5e0 101
julioefajardo 3:58ebc00f1a68 102
vijeetgoel 7:72e41680e5e0 103 reset1=0;
vijeetgoel 7:72e41680e5e0 104 xbe.baud(9600);
vijeetgoel 7:72e41680e5e0 105 reset1=1;
vijeetgoel 7:72e41680e5e0 106
vijeetgoel 7:72e41680e5e0 107 GlobalTime.start();
vijeetgoel 7:72e41680e5e0 108
vijeetgoel 7:72e41680e5e0 109 esc1.period(0.02f);
vijeetgoel 7:72e41680e5e0 110 esc2.period(0.02f);
vijeetgoel 7:72e41680e5e0 111 esc3.period(0.02f);
vijeetgoel 7:72e41680e5e0 112 esc4.period(0.02f); //Comparten el mismo timer
vijeetgoel 7:72e41680e5e0 113 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 114 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 115 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 116 esc4.pulsewidth(ESC4);
julioefajardo 2:4bc4e25328cc 117
screamer 0:bfb567985c64 118 combo_acc.enable();
screamer 0:bfb567985c64 119 combo_mag.enable();
screamer 0:bfb567985c64 120 printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
screamer 0:bfb567985c64 121 printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
screamer 0:bfb567985c64 122
julioefajardo 3:58ebc00f1a68 123 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 3:58ebc00f1a68 124 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
vijeetgoel 7:72e41680e5e0 125 kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
vijeetgoel 7:72e41680e5e0 126
julioefajardo 3:58ebc00f1a68 127
vijeetgoel 7:72e41680e5e0 128
vijeetgoel 7:72e41680e5e0 129
vijeetgoel 7:72e41680e5e0 130 while(1)
vijeetgoel 7:72e41680e5e0 131 {
vijeetgoel 7:72e41680e5e0 132
vijeetgoel 7:72e41680e5e0 133 while(xbe.readable())
vijeetgoel 7:72e41680e5e0 134
vijeetgoel 7:72e41680e5e0 135 {
vijeetgoel 7:72e41680e5e0 136 c = xbe.getc();
vijeetgoel 7:72e41680e5e0 137
vijeetgoel 7:72e41680e5e0 138 pc.printf("Value %c",c);
vijeetgoel 7:72e41680e5e0 139 pc.printf("receving");
vijeetgoel 7:72e41680e5e0 140
vijeetgoel 7:72e41680e5e0 141
vijeetgoel 7:72e41680e5e0 142 if (c == 'u')
vijeetgoel 7:72e41680e5e0 143 {
vijeetgoel 7:72e41680e5e0 144 if (ESC1<0.001f)
vijeetgoel 7:72e41680e5e0 145 {
vijeetgoel 7:72e41680e5e0 146 ESC1 += 0.0001f;
vijeetgoel 7:72e41680e5e0 147 ESC2 += 0.0001f;
vijeetgoel 7:72e41680e5e0 148 ESC3 += 0.0001f;
vijeetgoel 7:72e41680e5e0 149 ESC4 += 0.0001f;
vijeetgoel 7:72e41680e5e0 150 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 151 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 152 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 153 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 154 wait_ms(1000);
vijeetgoel 7:72e41680e5e0 155 pc.printf("Up");
vijeetgoel 7:72e41680e5e0 156 }
vijeetgoel 7:72e41680e5e0 157 }
vijeetgoel 7:72e41680e5e0 158
vijeetgoel 7:72e41680e5e0 159
vijeetgoel 7:72e41680e5e0 160
vijeetgoel 7:72e41680e5e0 161 else if (c == 'd')
vijeetgoel 7:72e41680e5e0 162 {
vijeetgoel 7:72e41680e5e0 163 if ((ESC1 > 0.0006f)||(ESC2 > 0.0006f)||(ESC3 > 0.0006f)||(ESC4 > 0.0006f))
vijeetgoel 7:72e41680e5e0 164 {
vijeetgoel 7:72e41680e5e0 165 ESC1 -= 0.0001f;
vijeetgoel 7:72e41680e5e0 166 ESC2 -= 0.0001f;
vijeetgoel 7:72e41680e5e0 167 ESC3 -= 0.0001f;
vijeetgoel 7:72e41680e5e0 168 ESC4 -= 0.0001f;
vijeetgoel 7:72e41680e5e0 169 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 170 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 171 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 172 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 173
vijeetgoel 7:72e41680e5e0 174 pc.printf("down");
vijeetgoel 7:72e41680e5e0 175 }
vijeetgoel 7:72e41680e5e0 176
vijeetgoel 7:72e41680e5e0 177 printf("%f\r\n", ESC1);
vijeetgoel 7:72e41680e5e0 178
vijeetgoel 7:72e41680e5e0 179
vijeetgoel 7:72e41680e5e0 180 }
vijeetgoel 7:72e41680e5e0 181
vijeetgoel 7:72e41680e5e0 182 else if(c=='r')
vijeetgoel 7:72e41680e5e0 183
vijeetgoel 7:72e41680e5e0 184
vijeetgoel 7:72e41680e5e0 185 {
vijeetgoel 7:72e41680e5e0 186
vijeetgoel 7:72e41680e5e0 187 ESC1 = 0.0006f;
vijeetgoel 7:72e41680e5e0 188 ESC2 = 0.0006f;
vijeetgoel 7:72e41680e5e0 189 ESC3 = 0.0006f;
vijeetgoel 7:72e41680e5e0 190 ESC4 = 0.0006f;
vijeetgoel 7:72e41680e5e0 191 pc.printf("still");
vijeetgoel 7:72e41680e5e0 192
vijeetgoel 7:72e41680e5e0 193 printf("%f\r\n", ESC1);
vijeetgoel 7:72e41680e5e0 194
vijeetgoel 7:72e41680e5e0 195
vijeetgoel 7:72e41680e5e0 196 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 197 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 198 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 199 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 200
vijeetgoel 7:72e41680e5e0 201
vijeetgoel 7:72e41680e5e0 202 }
vijeetgoel 7:72e41680e5e0 203
vijeetgoel 7:72e41680e5e0 204
vijeetgoel 7:72e41680e5e0 205 else if (c == 's') {
vijeetgoel 7:72e41680e5e0 206
vijeetgoel 7:72e41680e5e0 207 pc.printf("flyingatSameSpeed");
vijeetgoel 7:72e41680e5e0 208
vijeetgoel 7:72e41680e5e0 209
vijeetgoel 7:72e41680e5e0 210 printf("%f\r\n", ESC1);
vijeetgoel 7:72e41680e5e0 211
vijeetgoel 7:72e41680e5e0 212 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 213 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 214 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 215 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 216 }
vijeetgoel 7:72e41680e5e0 217
vijeetgoel 7:72e41680e5e0 218
vijeetgoel 7:72e41680e5e0 219
vijeetgoel 7:72e41680e5e0 220
vijeetgoel 7:72e41680e5e0 221 else if (c =='g') {
vijeetgoel 7:72e41680e5e0 222
vijeetgoel 7:72e41680e5e0 223 pc.printf("Moving left");
vijeetgoel 7:72e41680e5e0 224
vijeetgoel 7:72e41680e5e0 225 if(ESC3<0.01f)
vijeetgoel 7:72e41680e5e0 226 {
vijeetgoel 7:72e41680e5e0 227 ESC3 += 0.0001f;
vijeetgoel 7:72e41680e5e0 228 ESC4 += 0.0001f;
vijeetgoel 7:72e41680e5e0 229
vijeetgoel 7:72e41680e5e0 230 }
vijeetgoel 7:72e41680e5e0 231
vijeetgoel 7:72e41680e5e0 232 printf("%f\r\n", ESC3);
vijeetgoel 7:72e41680e5e0 233
vijeetgoel 7:72e41680e5e0 234
vijeetgoel 7:72e41680e5e0 235 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 236 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 237 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 238 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 239 }
vijeetgoel 7:72e41680e5e0 240
vijeetgoel 7:72e41680e5e0 241
vijeetgoel 7:72e41680e5e0 242
vijeetgoel 7:72e41680e5e0 243 else if (c =='h') {
vijeetgoel 7:72e41680e5e0 244
vijeetgoel 7:72e41680e5e0 245 pc.printf("Moving roght");
vijeetgoel 7:72e41680e5e0 246
vijeetgoel 7:72e41680e5e0 247 if(ESC2<0.01f)
vijeetgoel 7:72e41680e5e0 248 {
vijeetgoel 7:72e41680e5e0 249 ESC1 += 0.0001f;
vijeetgoel 7:72e41680e5e0 250 ESC2 += 0.0001f;
vijeetgoel 7:72e41680e5e0 251
vijeetgoel 7:72e41680e5e0 252 }
vijeetgoel 7:72e41680e5e0 253
vijeetgoel 7:72e41680e5e0 254 printf("%f\r\n", ESC3);
vijeetgoel 7:72e41680e5e0 255
vijeetgoel 7:72e41680e5e0 256
vijeetgoel 7:72e41680e5e0 257 esc1.pulsewidth(ESC1);
vijeetgoel 7:72e41680e5e0 258 esc2.pulsewidth(ESC2);
vijeetgoel 7:72e41680e5e0 259 esc3.pulsewidth(ESC3);
vijeetgoel 7:72e41680e5e0 260 esc4.pulsewidth(ESC4);
vijeetgoel 7:72e41680e5e0 261 }
julioefajardo 3:58ebc00f1a68 262 ProgramTimer.start();
julioefajardo 3:58ebc00f1a68 263 loopStartTime = ProgramTimer.read_us();
julioefajardo 3:58ebc00f1a68 264 timer = loopStartTime;
vijeetgoel 7:72e41680e5e0 265 high= (float)(ultra.read_u16()*2.75f/512.0f)*2.54f;
julioefajardo 5:74ca8be12359 266
screamer 0:bfb567985c64 267 combo_acc.getAxis(adata);
vijeetgoel 7:72e41680e5e0 268 combo_mag.getAxis(mdata);;
julioefajardo 3:58ebc00f1a68 269 gyro.ReadXYZ(gyro_data);
julioefajardo 3:58ebc00f1a68 270
julioefajardo 3:58ebc00f1a68 271 R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
screamer 0:bfb567985c64 272
julioefajardo 3:58ebc00f1a68 273 kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer));
julioefajardo 3:58ebc00f1a68 274 kalman_update(&filter_pitch, acos(adata.x/R));
julioefajardo 3:58ebc00f1a68 275 kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer));
julioefajardo 3:58ebc00f1a68 276 kalman_update(&filter_roll, acos(adata.y/R));
vijeetgoel 7:72e41680e5e0 277 kalman_predict(&filter_yaw, gyro_data[2], (ProgramTimer.read_us() - timer));
vijeetgoel 7:72e41680e5e0 278 kalman_update(&filter_yaw, acos(adata.z/R));
vijeetgoel 7:72e41680e5e0 279
vijeetgoel 7:72e41680e5e0 280 angle[0] = kalman_get_angle(&filter_pitch);
julioefajardo 3:58ebc00f1a68 281 angle[1] = kalman_get_angle(&filter_roll);
vijeetgoel 7:72e41680e5e0 282 angle[2]=kalman_get_angle(&filter_yaw);
julioefajardo 3:58ebc00f1a68 283
julioefajardo 6:d868495c1936 284 if (angle[0]>PI) angle[0] = PI;
julioefajardo 6:d868495c1936 285 else if (angle[0]<0) angle[0] = 0.0f;
julioefajardo 6:d868495c1936 286 else angle[0] += 0.0f;
julioefajardo 6:d868495c1936 287
julioefajardo 6:d868495c1936 288 if (angle[1]>PI) angle[1] = PI;
julioefajardo 6:d868495c1936 289 else if (angle[1]<0) angle[1] = 0.0f;
julioefajardo 6:d868495c1936 290 else angle[1] += 0.0f;
vijeetgoel 7:72e41680e5e0 291
vijeetgoel 7:72e41680e5e0 292 if (angle[2]>PI) angle[2] = PI;
vijeetgoel 7:72e41680e5e0 293 else if (angle[2]<0) angle[2] = 0.0f;
vijeetgoel 7:72e41680e5e0 294 else angle[2] += 0.0f;
julioefajardo 6:d868495c1936 295
julioefajardo 6:d868495c1936 296 pitch_error = angle[0] - PITCH_SP;
julioefajardo 6:d868495c1936 297 roll_error = angle[1] - ROLL_SP;
vijeetgoel 7:72e41680e5e0 298 yaw_error = angle[2] - YAW_SP;
julioefajardo 6:d868495c1936 299
julioefajardo 6:d868495c1936 300 pitch = arm_pid_f32(&PPID, pitch_error);
julioefajardo 6:d868495c1936 301 roll = arm_pid_f32(&RPID, roll_error);
vijeetgoel 7:72e41680e5e0 302 yaw = arm_pid_f32(&YPID, yaw_error);
julioefajardo 3:58ebc00f1a68 303 timer = ProgramTimer.read_us();
julioefajardo 6:d868495c1936 304
vijeetgoel 7:72e41680e5e0 305 ESC1=ESC1-pitch+yaw;
vijeetgoel 7:72e41680e5e0 306 ESC2=ESC2-roll +yaw;
vijeetgoel 7:72e41680e5e0 307 ESC3=ESC3+roll-yaw;
vijeetgoel 7:72e41680e5e0 308 ESC4=ESC3+pitch-yaw;
vijeetgoel 7:72e41680e5e0 309
vijeetgoel 7:72e41680e5e0 310 if(ESC1>0.0014f)
vijeetgoel 7:72e41680e5e0 311 {
vijeetgoel 7:72e41680e5e0 312 ESC1=.00014f;
vijeetgoel 7:72e41680e5e0 313 }
vijeetgoel 7:72e41680e5e0 314 else if(ESC1<0.001f)
vijeetgoel 7:72e41680e5e0 315 {
vijeetgoel 7:72e41680e5e0 316 ESC1=0.001f;
vijeetgoel 7:72e41680e5e0 317 }
julioefajardo 6:d868495c1936 318
vijeetgoel 7:72e41680e5e0 319 else{
vijeetgoel 7:72e41680e5e0 320 pc.printf("no change");
vijeetgoel 7:72e41680e5e0 321 }
vijeetgoel 7:72e41680e5e0 322
vijeetgoel 7:72e41680e5e0 323
vijeetgoel 7:72e41680e5e0 324
vijeetgoel 7:72e41680e5e0 325
vijeetgoel 7:72e41680e5e0 326 if(ESC2>0.0014f)
vijeetgoel 7:72e41680e5e0 327 {
vijeetgoel 7:72e41680e5e0 328 ESC2=.00014f;
vijeetgoel 7:72e41680e5e0 329 }
vijeetgoel 7:72e41680e5e0 330 else if(ESC2<0.001f)
vijeetgoel 7:72e41680e5e0 331 {
vijeetgoel 7:72e41680e5e0 332 ESC2=0.001f;
vijeetgoel 7:72e41680e5e0 333 }
vijeetgoel 7:72e41680e5e0 334
vijeetgoel 7:72e41680e5e0 335 else{
vijeetgoel 7:72e41680e5e0 336 pc.printf("no change");
vijeetgoel 7:72e41680e5e0 337 }
vijeetgoel 7:72e41680e5e0 338
julioefajardo 6:d868495c1936 339
vijeetgoel 7:72e41680e5e0 340 if(ESC3>0.0014f)
vijeetgoel 7:72e41680e5e0 341 {
vijeetgoel 7:72e41680e5e0 342 ESC3=.00014f;
vijeetgoel 7:72e41680e5e0 343 }
vijeetgoel 7:72e41680e5e0 344 else if(ESC3<0.001f)
vijeetgoel 7:72e41680e5e0 345 {
vijeetgoel 7:72e41680e5e0 346 ESC3=0.001f;
vijeetgoel 7:72e41680e5e0 347 }
vijeetgoel 7:72e41680e5e0 348
vijeetgoel 7:72e41680e5e0 349 else{
vijeetgoel 7:72e41680e5e0 350 pc.printf("no change");
vijeetgoel 7:72e41680e5e0 351 }
vijeetgoel 7:72e41680e5e0 352
vijeetgoel 7:72e41680e5e0 353
vijeetgoel 7:72e41680e5e0 354 if(ESC4>0.0014f)
vijeetgoel 7:72e41680e5e0 355 {
vijeetgoel 7:72e41680e5e0 356 ESC4=.00014f;
vijeetgoel 7:72e41680e5e0 357 }
vijeetgoel 7:72e41680e5e0 358 else if(ESC4<0.001f)
vijeetgoel 7:72e41680e5e0 359 {
vijeetgoel 7:72e41680e5e0 360 ESC4=0.001f;
vijeetgoel 7:72e41680e5e0 361 }
vijeetgoel 7:72e41680e5e0 362
vijeetgoel 7:72e41680e5e0 363 else{
vijeetgoel 7:72e41680e5e0 364 pc.printf("no change");
vijeetgoel 7:72e41680e5e0 365 }
vijeetgoel 7:72e41680e5e0 366
vijeetgoel 7:72e41680e5e0 367
vijeetgoel 7:72e41680e5e0 368 pc.printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
vijeetgoel 7:72e41680e5e0 369 pc.printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
vijeetgoel 7:72e41680e5e0 370 printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
vijeetgoel 7:72e41680e5e0 371 pc.printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n YAW Angle Z: %.6f \r\n ", Rad2Dree * angle[1], Rad2Dree * angle[0], Rad2Dree * angle[2]);
vijeetgoel 7:72e41680e5e0 372 pc.printf("roll = %.6f pitch = %.6f \r\n yaw = %.6f \r\n",roll,pitch,yaw);
vijeetgoel 7:72e41680e5e0 373 pc.printf("ESC1 = %.6f ESC4 = %.6f \r\n", ESC1,ESC4);
vijeetgoel 7:72e41680e5e0 374 pc.printf("ESC2 = %.6f ESC3 = %.6f \r\n", ESC2,ESC3);
julioefajardo 5:74ca8be12359 375 //printf("dist = %.2f \r\n",dist);
screamer 0:bfb567985c64 376
julioefajardo 5:74ca8be12359 377 wait(0.02f);
vijeetgoel 7:72e41680e5e0 378
vijeetgoel 7:72e41680e5e0 379
vijeetgoel 7:72e41680e5e0 380 }
vijeetgoel 7:72e41680e5e0 381
vijeetgoel 7:72e41680e5e0 382 // 20ms is the default period of the ESC pwm
julioefajardo 5:74ca8be12359 383 }
julioefajardo 5:74ca8be12359 384 }