Freescale Quadcopter with Freedom K64F Board
Dependencies: FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp
Fork of Freescale_Multi-Sensor_Shield by
Quadcopter based on Freescale FRDM-K64F, Freescale FRDM-FXS-9AXIS, Hobbypower X525 V3 Quadcopter Foldable Kit with 1000KV Motors and SimonK 30A ESC
Diff: main.cpp
- Revision:
- 5:74ca8be12359
- Parent:
- 4:1bc3ca07a412
- Child:
- 6:d868495c1936
diff -r 1bc3ca07a412 -r 74ca8be12359 main.cpp --- a/main.cpp Tue Sep 01 18:21:17 2015 +0000 +++ b/main.cpp Fri Sep 11 22:24:22 2015 +0000 @@ -24,6 +24,9 @@ #define PI 3.1415926535897932384626433832795 #define Rad2Dree 57.295779513082320876798154814105 +DigitalOut red(LED_RED); +DigitalOut green(LED_GREEN); + FXOS8700Q_acc combo_acc(A5, A4, FXOS8700CQ_SLAVE_ADDR0); FXOS8700Q_mag combo_mag(A5, A4, FXOS8700CQ_SLAVE_ADDR0); FXAS21000 gyro(A5, A4); @@ -38,6 +41,9 @@ PwmOut M4(D10); Serial pc(USBTX, USBRX); +Serial bt(D1,D0); + +AnalogIn ultra(A0); kalman filter_pitch; kalman filter_roll; @@ -46,6 +52,18 @@ double angle[3]; unsigned long timer; long loopStartTime; +char i; +char command = ' '; +float high; + +float ESC1 = 0.0006f; +float ESC2 = 0.0006f; +float ESC3 = 0.0006f; +float ESC4 = 0.0006f; + +void bt_callback(void); +void esc_start(void); +void esc_stop(void); int main() { @@ -53,15 +71,18 @@ MotionSensorDataUnits adata; MotionSensorDataUnits mdata; + bt.attach(&bt_callback); + printf("\r\nStarting\r\n\r\n"); + red = 0; green= 1; GlobalTime.start(); M1.period(0.02f); //Comparten el mismo timer - M1.pulsewidth(0.0006f); - M2.pulsewidth(0.0006f); - M3.pulsewidth(0.0006f); - M4.pulsewidth(0.0006f); + M1.pulsewidth(ESC1); + M2.pulsewidth(ESC2); + M3.pulsewidth(ESC3); + M4.pulsewidth(ESC4); combo_acc.enable(); combo_mag.enable(); @@ -71,14 +92,18 @@ kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); - wait(3); - + esc_start(); + wait(2.0f); + red = 1; green= 0; ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; while(1) { + + high = (float)(ultra.read_u16()*2.75f/512.0f)*2.54f; + combo_acc.getAxis(adata); combo_mag.getAxis(mdata); gyro.ReadXYZ(gyro_data); @@ -95,12 +120,63 @@ timer = ProgramTimer.read_us(); - printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z); - printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z); - printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]); - printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]); - printf("\r\n"); + //printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z); + //printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z); + //printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]); + printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]); + //printf("dist = %.2f \r\n",dist); - wait(0.5); + wait(0.02f); } } + +void bt_callback(void) { + // Note: you need to actually read from the serial to clear the RX interrupt + command = bt.getc(); + //if (command == 'z') bt.printf("start\n\r"); + if (command == 'x') { + bt.printf("stop\n\r"); + esc_stop(); + } + else if (command == 'w') bt.printf("up\n\r"); + else if (command == 's') bt.printf("down\n\r"); + else if (command == 'h') bt.printf("dist = %.2f \r\n",high); + else bt.printf("%c\n\r", command); +} + +void esc_start(void) { + ESC1 = 0.0006f; + ESC2 = 0.0006f; + ESC3 = 0.0006f; + ESC4 = 0.0006f; + for (i = 0; i < 4; i++){ + ESC1 += 0.0001f; + ESC2 += 0.0001f; + ESC3 += 0.0001f; + ESC4 += 0.0001f; + M1.pulsewidth(ESC1); + M2.pulsewidth(ESC2); + M3.pulsewidth(ESC3); + M4.pulsewidth(ESC4); + wait_ms(1000); + } +} + +void esc_stop(void){ + red = 0; green= 1; + while ((ESC1 > 0.0006f)||(ESC2 > 0.0006f)||(ESC3 > 0.0006f)||(ESC4 > 0.0006f)){ + ESC1 -= 0.0001f; + ESC2 -= 0.0001f; + ESC3 -= 0.0001f; + ESC4 -= 0.0001f; + M1.pulsewidth(ESC1); + M2.pulsewidth(ESC2); + M3.pulsewidth(ESC3); + M4.pulsewidth(ESC4); + wait_ms(1000); + } + if (ESC1 < 0.0006f) ESC1 = 0.0006f; + if (ESC2 < 0.0006f) ESC2 = 0.0006f; + if (ESC3 < 0.0006f) ESC3 = 0.0006f; + if (ESC4 < 0.0006f) ESC4 = 0.0006f; +} \ No newline at end of file