Diff: main.cpp

Revision:

5:74ca8be12359

Parent:

4:1bc3ca07a412

Child:

6:d868495c1936

--- 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