Diff: main.cpp

Revision:

8:f23601373e8b

Parent:

7:23bb20892c8c

Child:

9:1b54bac6d9a7

--- a/main.cpp	Sun Sep 29 20:22:43 2019 +0000
+++ b/main.cpp	Wed Oct 02 22:25:12 2019 +0000
@@ -1,60 +1,43 @@
 #include "sm_esc.h"
 #include "sm_servo.h"
 #include "sm_sections.h"
-#include "MPU6050.h"
+#include "sm_mpu.h"
 
 
-MPU6050 mpu;
-int16_t ax, ay, az;
-float ax_g,ay_g,az_g;
-int16_t gx, gy, gz;
 
 int main()
 {
-    //on met le jumper sur le chassis le plus récent. Avec le jumper, chassisMode = 0.
+    //on met le jumper sur le chassis le plus récent. Avec le jumper, b_CHASSIS_Mode = 0.
     init_chassis_mode();
-
-    mpu.initialize();
-    odroid.printf("MPU6050 testConnection \r\n");
-
-    bool mpu6050TestResult = mpu.testConnection();
-    if(mpu6050TestResult) {
-        odroid.printf("MPU6050 test passed \r\n");
-    } else {
-        odroid.printf("MPU6050 test failed \r\n");
-    }
-
+   
     //init timers
-    timerSinceStart.start();
+    t_utils_timerSinceStart.start();
 
     //init_interruptions
-    button.fall(&it_pressed);
-    odom.fall(&it_odom);
+    b_UTILS_button.fall(&it_pressed);
+    b_ODOM_odom.fall(&it_odom);
+    b_UTILS_emergency_signal.rise(&it_emergency_signal_recieved);
 
     //init state machines
-    pc.printf("init state machines\r\n");
+    rs_UTILS_pc.printf("init state machines\r\n");
 
-    //init_sm_sections();
+    init_sm_mpu();
+    init_sm_sections();
     init_sm_esc();
     init_sm_servo();
 
     while(1) {
-        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-        //correction pour avoir une acceleration coef empirique
-        ax_g = (float)ax /1658.0 ;
-        ay_g = (float)ay /1658.0 ;
-        az_g = (float)az /1658.0 ;
-
-        if(e_stateESC == ESC_COMMAND)
-        {
-            odroid.printf("acc X =%.2f; acc Y =%.2f; acc Z=%.2f;gyr X =%d; gyr Y =%d; gyr Z=%d;odom %f\n\r",ax_g,ay_g,az_g,gx,gy,gz,d_ODOM_distFromSectionStart_m);
-        }
-        //update_sm_sections();
+        
+        update_sm_mpu();
+        update_sm_sections();
         update_sm_esc();
         update_sm_servo();
 
-        //output_sm_sections();
+        output_sm_mpu();
+        output_sm_sections();
         output_sm_esc();
         output_sm_servo();
+        
+        rs_UTILS_pc.printf("\r\n");
     }
 }