Diff: main.cpp

Revision:

6:ab9f3695633f

Parent:

5:8bbe640528bc

Child:

7:23bb20892c8c

--- a/main.cpp	Sun Sep 29 17:09:48 2019 +0000
+++ b/main.cpp	Sun Sep 29 20:07:54 2019 +0000
@@ -1,30 +1,59 @@
 #include "sm_esc.h"
 #include "sm_servo.h"
 #include "sm_sections.h"
+#include "MPU6050.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.
-    chassisMode.mode(PullUp);
-    
+    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();
 
     //init_interruptions
-    button.fall(&it_pressed);  
+    button.fall(&it_pressed);
     odom.fall(&it_odom);
 
     //init state machines
     pc.printf("init state machines\r\n");
 
-    output_sm_esc();
+    //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;\n\r",ax_g,ay_g,az_g,gx,gy,gz);
+        }
+        //update_sm_sections();
         update_sm_esc();
         update_sm_servo();
 
+        //output_sm_sections();
         output_sm_esc();
         output_sm_servo();
     }