File content as of revision 7:23bb20892c8c:

#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.
    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);
    odom.fall(&it_odom);

    //init state machines
    pc.printf("init state machines\r\n");

    //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_esc();
        update_sm_servo();

        //output_sm_sections();
        output_sm_esc();
        output_sm_servo();
    }
}