File content as of revision 0:56ba69e447e3:

#include "mbed.h"
#include "nRF24L01.h"
#include "RF24.h"
#include "RF24_config.h"

PwmOut PWM1(p21);
PwmOut PWM2(p22);
PwmOut PWM3(p23);
PwmOut PWM4(p24);
DigitalOut led1(LED1);
DigitalOut led_switch(LED4);
//DigitalIn switch_on(p5, PullDown);
Ticker loops; // 프로그램 무한루프

RF24 NRF24L01(p5, p6, p7, p15, p8);

I2C i2c(p28, p27); //(I2C_SDA,I2C_SCL); 
Serial pc(USBTX, USBRX); //D1D0

int MPU9250_ADDRESS = (0x68 << 1), //0b11010000
WHO_AM_I_MPU9250 = 0x75,
FIFO_EN = 0x23,
SMPLRT_DIV = 0x19,
CONFIG = 0x1A,
GYRO_CONFIG = 0x1B,
ACCEL_CONFIG = 0x1C,
ACCEL_CONFIG2 = 0x1D,
ACCEL_XOUT_H = 0x3B,
TEMP_OUT_H = 0x41,
GYRO_XOUT_H = 0x43,
PWR_MGMT_1 = 0x6B,
PWR_MGMT_2 = 0x6C,
INT_PIN_CFG = 0x37,
AK8963_ADDRESS = (0x0C << 1), //0b00001100 0x0C<<1
WHO_AM_I_AK8963 = 0x00, // should return 0x48
AK8963_ST1 = 0x02,
AK8963_XOUT_L = 0x03,
AK8963_CNTL = 0x0A,
AK8963_ASAX = 0x10;

float gyro_bias[3] = { 0,0,0 };
float accel_bias[3] = { 0,0,0 };
int16_t gyro[3];
int16_t accel[3];

float accel_fu[3] = { 0,0,0 };
float accel_f2[3] = { 0,0,0 };
float accel_f1[3] = { 0,0,0 };
float accel_f[3] = { 0,0,0 };

float gyro_f[3] = { 0,0,0 };
float gyro_angle[3] = { 0,0,0 };
float roll_c, roll_accel, roll_g_n, roll_g_old, roll_a_n, roll_a_old = 0;
float pitch_c, pitch_accel, pitch_g_n, pitch_g_old, pitch_a_n, pitch_a_old = 0;
float tau = 0.3;
float accel_tau = 1;

const uint64_t pipe = 0x1212121212LL;
int8_t recv[30];
int16_t PITCH = 0, ROLL = 0, YAW = 0, THROTTLE = 0;
int8_t ackData[30];
int8_t flip1 = 1;

// ETC
float roll_controller, pitch_controller, yaw_controller = 0.0;
float tilt_cont[4];
float checksum[2] = {0,0};
int BUT1, BUT2;

//char BUT1, BUT2, prev_BUT2;
int rf_fail_count = 0;

int l_count = 0;
float dt = 0.0025; // 0.0025
float roll_f, pitch_f, yaw_f, yaw_c;

// 적절하게 조절 0
float K_scale = 0.0001;                             //    1
float Kp_roll   = 15,     Kd_roll   = 40    ;    //     0.0015 0.0024
float Kp_pitch  = 16,     Kd_pitch  = 40    ;    //     0.0015 0.0024
float Kp_yaw    = 50,   Kd_yaw    = 10  ;  //

float roll_cont, pitch_cont, yaw_cont = 0;
float rollcompensate, pitchcompensate = 0;
float thrust_c = 0.0;

// 피치, 요 에러도 추가
float roll_err, roll_rate_err;
float pitch_err, pitch_rate_err;
float yaw_err, yaw_rate_err;

float thrust_in = 0.0;
float roll_ref = 0.0, roll_rate_ref = 0.0;
float pitch_ref = 0, pitch_rate_ref = 0;; // 추가
float yaw_ref = 0, yaw_rate_ref = 0;

float pwm1_pw, pwm2_pw, pwm3_pw, pwm4_pw; // 1이면 풀가동
float ROLL_ERR_MAX = 40.0, ROLL_RATE_ERR_MAX = 40.0; // maximum errors considered
float PITCH_ERR_MAX = 40.0, PITCH_RATE_ERR_MAX = 40.0; // maximum errors considered
float YAW_ERR_MAX = 30.0, YAW_RATE_ERR_MAX = 30.0; // maximum errors considered

void WHO_AM_I(void);
void MPU9250_INIT(void);
void MPU9250_RESET(void);
void MPU9250_GET_GYRO(int16_t * destination);
void MPU9250_GET_ACCEL(int16_t * destination);
void gyro_bias_f(void);
void control(void);
void pwm_drive(void);
void control_loop(void);
int constrain_int16(int16_t x, int min, int max);
float constrain_float(float x, float min, float max);
void RF_READ();
void yaw_reset();
void moving_cont();
float set_min(float x, float min);

void RF_READ()
{
    if (NRF24L01.available())
    {
        NRF24L01.read(recv, 10);

        // 스케일링 다른데서 곱하기
        ROLL = *(int16_t*)(&recv[0])- 3; //ROLL = - ROLL;           offset = 0
        PITCH = *(int16_t*)(&recv[2])- 7;//flip pitch and roll      offset = 6
        YAW = *(int16_t*)(&recv[4]) - 0; //                         offset = 2
        THROTTLE = *(int16_t*)(&recv[6]) - 0; //                    offset = 0
        BUT1 = recv[8];
        BUT2 = recv[9]; //should hold value here
        //pc.printf("\r RF_READ : %d, %d, %d, %d \n\r", (int)ROLL, (int)PITCH, (int)YAW, (int)THROTTLE);
        rf_fail_count = 0;
    }

    else
    {
        rf_fail_count = rf_fail_count + 1;
                
        //printf(" rf_fail_count : %d\n\r", rf_fail_count);
        
        if (rf_fail_count >= 20 && rf_fail_count < 100)
        {
            printf(" rf_fail_count : %d\n\r", rf_fail_count);
            THROTTLE = THROTTLE - 2;
            THROTTLE = constrain_int16(THROTTLE, 0, 1023);
        }
        if (rf_fail_count >= 50)
        {
            THROTTLE = 0;
        }
        if (rf_fail_count >= 100)
        {
            rf_fail_count = 100;
        }
    }
}

void yaw_reset()
{
    if(set_min(yaw_ref,20)!= 0)
    gyro_angle[2] = 0;
}

void control(void)
{
    //pc.printf(" _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in);

    roll_err = roll_ref - roll_f;
    roll_rate_err = roll_rate_ref - gyro_f[0];

    if (roll_err > ROLL_ERR_MAX)
        roll_err = ROLL_ERR_MAX;
    else if (roll_err < -(ROLL_ERR_MAX))
        roll_err = -(ROLL_ERR_MAX);

    if (roll_rate_err > ROLL_RATE_ERR_MAX)
        roll_rate_err = ROLL_RATE_ERR_MAX;
    else if (roll_rate_err < -(ROLL_RATE_ERR_MAX))
        roll_rate_err = -(ROLL_RATE_ERR_MAX);

    roll_cont = K_scale * (Kp_roll * roll_err + Kd_roll * roll_rate_err);

    if (roll_cont > 0.33)
        roll_cont = 0.33;
    else if (roll_cont < -0.33)
        roll_cont = -0.33;

    /*-------------------------------------------------------------------*/

    pitch_err = pitch_ref - pitch_f;
    pitch_rate_err = pitch_rate_ref - gyro_f[1];

    if (pitch_err > PITCH_ERR_MAX)
        pitch_err = PITCH_ERR_MAX;
    else if (pitch_err < -(PITCH_ERR_MAX))
        pitch_err = -(PITCH_ERR_MAX);

    if (pitch_rate_err > PITCH_RATE_ERR_MAX)
        pitch_rate_err = PITCH_RATE_ERR_MAX;
    else if (pitch_rate_err < -(PITCH_RATE_ERR_MAX))
        pitch_rate_err = -(PITCH_RATE_ERR_MAX);

    pitch_cont = K_scale * (Kp_pitch * pitch_err + Kd_pitch * pitch_rate_err);

    if (pitch_cont > 0.33)
        pitch_cont = 0.33;
    else if (pitch_cont < -0.33)
        pitch_cont = -0.33;


    /*-------------------------------------------------------------------------*/

    yaw_err = yaw_ref - gyro_angle[2];
    yaw_rate_err = yaw_rate_ref - gyro_f[2];

    if (yaw_err > YAW_ERR_MAX)
        yaw_err = YAW_ERR_MAX;
    else if (yaw_err < -(YAW_ERR_MAX))
        yaw_err = -(YAW_ERR_MAX);

    if (yaw_rate_err > YAW_RATE_ERR_MAX)
        yaw_rate_err = YAW_RATE_ERR_MAX;
    else if (yaw_rate_err < -(YAW_RATE_ERR_MAX))
        yaw_rate_err = -(YAW_RATE_ERR_MAX);
    if(yaw_ref == 0)
        yaw_cont = K_scale * (Kp_yaw * yaw_err + Kd_yaw * yaw_rate_err);
    else
    {
        yaw_cont = K_scale * (Kp_yaw * yaw_err);
        yaw_ref = 0;
    }
    
    if (yaw_cont > 0.20)
        yaw_cont = 0.20;
    else if (yaw_cont < -0.20)
        yaw_cont = -0.20;
    
    thrust_c = thrust_in / (float)1654;  //control_scale = 2000
    if (thrust_c > 0.8)
        thrust_c = 0.8;
        
}

//control tilted moving
void moving_cont()
{
    checksum[0] = roll_ref + pitch_ref;
    if(checksum[0] == checksum[1])
    {
        rollcompensate = accel_f[1]*(float)(0.5);
        pitchcompensate = accel_f[0]*(float)(0.5);
        rollcompensate = constrain_float(rollcompensate,-0.05,0.05);
        pitchcompensate = constrain_float(pitchcompensate,-0.05,0.05);
        
    }
    else
    {
        rollcompensate = 0;
        pitchcompensate = 0;
        
    }
    checksum[1] = checksum[0];
}    


void pwm_drive(void)//roll+ = pitch-, 
{
    //pc.printf(" _c :  %5.2f  %5.2f  %5.2f  %5.2f \n\r", roll_c, pitch_c, yaw_c, thrust_c);
    pwm1_pw = -(roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) -yaw_cont + thrust_c  ;  // 최댓값 1, 최솟값 0
    pwm2_pw = (roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) + yaw_cont + thrust_c  ;
    pwm3_pw = -(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) + yaw_cont + thrust_c  ;
    pwm4_pw = +(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) - yaw_cont + thrust_c ;
    
    if(pwm1_pw >= 1)
        pwm1_pw = 1;
    else if(pwm1_pw < 0)
        pwm1_pw = 0;
    if(pwm2_pw >= 1)
        pwm2_pw = 1;
    else if(pwm2_pw < 0)
        pwm2_pw = 0;
    if(pwm3_pw >= 1)
        pwm3_pw = 1;
    else if(pwm3_pw < 0)
        pwm3_pw = 0;
    if(pwm4_pw >= 1)
        pwm4_pw = 1;
    else if(pwm4_pw < 0)
        pwm4_pw = 0;
    
    if (rf_fail_count>=50) // switch_on = 1;
    {
       pwm1_pw = 0.0;
       pwm2_pw = 0.0;
       pwm3_pw = 0.0;
       pwm4_pw = 0.0;
    }

      
    PWM1 = pwm1_pw;
    PWM2 = pwm2_pw;
    PWM3 = pwm3_pw;
    PWM4 = pwm4_pw;   
}

void control_loop(void)
{

    MPU9250_GET_GYRO(gyro);
    gyro_f[0] = gyro[0] / 32.8 - gyro_bias[0];
    gyro_f[1] = -(gyro[1] / 32.8 - gyro_bias[1]);
    gyro_f[2] = -(gyro[2] / 32.8 - gyro_bias[2]);

    MPU9250_GET_ACCEL(accel);
    accel_f[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED)   // 9.8m/s^2 ( G )
    accel_f[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
    accel_f[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);

    /*
    //accel low pass filter
    accel_fu[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED)   // 9.8m/s^2 ( G )
    accel_fu[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
    accel_fu[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);

    //accel low pass filter -----------
    accel_f2[0]=(1-dt/accel_tau)*accel_f1[0]+dt/accel_tau*accel_fu[0];
    accel_f2[1]=(1-dt/accel_tau)*accel_f1[1]+dt/accel_tau*accel_fu[1];
    accel_f2[2]=(1-dt/accel_tau)*accel_f1[2]+dt/accel_tau*accel_fu[2];
    
    accel_f1[0]=accel_f2[0];
    accel_f1[1]=accel_f2[1];
    accel_f1[2]=accel_f2[2];
    
    accel_f[0]=accel_f2[0];
    accel_f[1]=accel_f2[1];
    accel_f[2]=accel_f2[2];
    */
    //----------------------------------
    
    //get roll and pitch from Accelometer values
    roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14;
    //roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14;         //degree '
    pitch_accel = asin(constrain_float(accel_f[0],(float)-1,(float)1)) * 180.0 / 3.14; // apply constrain to avoid NaN

    //get Roll, Pitch, Yaw from Gyro
    gyro_angle[0] = gyro_angle[0] + (gyro_f[0]) * dt;   //Roll
    gyro_angle[1] = gyro_angle[1] + (gyro_f[1]) * dt;   //Pitch
    gyro_angle[2] = gyro_angle[2] + (gyro_f[2]) * dt;   //Yaw

    // YAW 360도 넘길 때 빼는 방법

   //get roll and pitch by applying Complementary Filter
    roll_g_n = (1 - dt / tau) * roll_g_old + dt * gyro_f[0];
    roll_a_n = (1 - dt / tau) * roll_a_old + dt / tau * roll_accel;

    pitch_g_n = (1 - dt / tau) * pitch_g_old + dt * gyro_f[1];
    pitch_a_n = (1 - dt / tau) * pitch_a_old + dt / tau * pitch_accel;

    roll_g_old = roll_g_n;
    roll_a_old = roll_a_n;

    pitch_g_old = pitch_g_n;
    pitch_a_old = pitch_a_n;
    //pc.printf("\r%5.2f\t%5.2f",pitch_g_old, pitch_a_old);
    roll_f = roll_g_old + roll_a_old;
    pitch_f = pitch_g_old + pitch_a_old;        //this value is weird 

    RF_READ();
    /*
    ROLL = ROLL - (int)offset[0];
    PITCH = PITCH - (int)offset[1];
    YAW = YAW - (int)offset[2];
    THROTTLE = THROTTLE - (int)offset[3];
    */
    
    roll_ref = -(set_min((float)ROLL,6));
    pitch_ref = set_min((float)PITCH,6);
    yaw_ref = -(set_min((float)YAW,6));
    thrust_in = set_min((float)THROTTLE,6);
    
    yaw_reset();
    
    control();
    
    //moving_cont();
    
    pwm_drive();

    
  /*
    if (l_count == 400 ) {
        //pc.printf("l_count per 10")
    
        //pc.printf("----------------------------------\n\r");
        //pc.printf("\r Gyro data deg/s  :\t %5.2f\t %5.2f\t %5.2f\n\r", gyro_f[0], gyro_f[1], gyro_f[2]);  // 각속도
        //pc.printf("\r Gyro to angle   :\t %5.2f\t %5.2f\t %5.2f\n\r", gyro_angle[0], gyro_angle[1], gyro_angle[2]);    //Gyro print
        //pc.printf("\r Accel data /s^2  :\t %5.2f\t %5.2f\t %5.2f\n\r", accel_f[0], accel_f[1], accel_f[2]);             //Accel print
        //pc.printf("\r Accel to angle  :\t %5.2f\t %5.2f\n\r", roll_accel, pitch_accel);
        
        //pc.printf("\r R_f P_f Y angle  :\t %5.2f\t %5.2f\t %5.2f\n\r", roll_f, pitch_f, gyro_angle[2]);         //Roll, Pitch, Yaw 각도
    
       // pc.printf("\r control input    : %d, %d, %d, %f \n\r\n", (int)ROLL, (int)PITCH, (int)YAW, (float)THROTTLE);    //RF_READ
        //pc.printf("\r control value   :\t %5.2f\t %5.2f\t %5.2f\t %5.2f\n\r", roll_c, pitch_c, yaw_c, thrust_c);    // 모터에 들어가는 값
        //pc.printf("\r RPYT_control     :\t %5.2f\t%5.2f\t%5.2f\t%5.2f \n\r", roll_cont, pitch_cont, yaw_cont, thrust_c);
        //pc.printf("\r PWM Output       :\t %5.2f\t%5.2f\t%5.2f\t%5.2f\n\r", pwm1_pw, pwm2_pw, pwm3_pw, pwm4_pw);       // 모터가 내는 추력 0~1
        //pc.printf("\r RPYT_err   :\t%5.3f\t%5.3f\t%5.3f \n\r", roll_err, pitch_err, yaw_err);
        //pc.printf("\t\tRoll\tPitch\tYaw\tThrust\t\r\n");
        pc.printf("\r Angle     :\t%5.2f\t%5.2f\t%5.2f\n\r", roll_f, pitch_f, gyro_angle[2]);         //Roll, Pitch, Yaw 각도 표 출력
        pc.printf("\r RF input  :\t%d\t%d\t%d\t%d \n\r\n", (int)ROLL, (int)PITCH, (int)YAW, (int)THROTTLE); //RF intput 표 출력
        
        
        //pc.printf("\r _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in); // 제어에 쓰이는 값
        
        l_count = 1;
    }
    
    l_count = l_count + 1;
    */

    //wait(dt);
    
}
float set_min(float x, float min)
{
    if ( x<min && x> -min)
    x = 0;  
    return x;  
}

int constrain_int16(int16_t x, int min, int max)
{
    if (x > max)
        x = max;
    else if (x < min)
        x = min;
    return x;
}

float constrain_float(float x, float min, float max)
{
    if (x > max)
        x = max;
    else if (x < min)
        x = min;
    return x;
}

void MPU9250_RESET()
{
    // reset device
    char cmd[2];
    cmd[0] = PWR_MGMT_1; //status
    cmd[1] = 0x80;
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    wait(0.1);
}

void MPU9250_INIT()
{
    char cmd[3];
    cmd[0] = PWR_MGMT_1; //reset
    cmd[1] = 0x80;
    i2c.write(MPU9250_ADDRESS, cmd, 2);
    pc.printf("MPU 1 \n\r");
    wait(0.1);

    cmd[0] = PWR_MGMT_1; // Auto selects the best available clock source PLL if ready,
    cmd[1] = 0x01; // else use the Internal oscillator
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 2 \n\r");
    wait(0.1);

    // Select Bandwidth of Gyroscopes BW 41Hz 1KHz internal sampling
    cmd[0] = CONFIG;
    cmd[1] = 0x03;
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 3 \n\r");
    wait(0.1);

    // sample rate = (internal sample rate) /(1+4) = 1000/5=200 Hz 
    cmd[0] = SMPLRT_DIV;
    cmd[1] = 0x00;//0x04 
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 4 \n\r");
    wait(0.1);


    cmd[0] = GYRO_CONFIG;
    cmd[1] = 0b00010000;// Gyro full scale 1000 deg/sec; Gyro DLPF Enable
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 6 \n\r");
    wait(0.1);

    // Set accelerometer configuration
    // Accel fulll sacle range +/- 4g
    cmd[0] = ACCEL_CONFIG;
    cmd[1] = 0b00001000;// Accel 
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 8 \n\r");
    wait(0.1);

    cmd[0] = ACCEL_CONFIG2;
    cmd[1] = 0b00000101; // 218 Hz bandwidth, 1kHz Sampling for accelerometer
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 10 \n\r");
    wait(0.1);

    //XYZ Gyro acel enable
    cmd[0] = PWR_MGMT_2;
    cmd[1] = 0x00;
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 11 \n\r");
    wait(0.1);

    cmd[0] = INT_PIN_CFG;
    cmd[1] = 0x22; //0x02 
    i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
    pc.printf("MPU 12 \n\r");
    wait(0.1);
}

void WHO_AM_I()
{
    char cmd[2];
    char rvd[2];

    cmd[0] = WHO_AM_I_MPU9250;
    i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
    i2c.read(MPU9250_ADDRESS | 1, rvd, 1, 0);

    pc.printf(" IMU device id is 0x%x \n\r", rvd[0]);
    wait(1);
}

void MPU9250_GET_GYRO(int16_t * destination)
{
    char cmd[6];
    cmd[0] = GYRO_XOUT_H;
    i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
    i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
    destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
    destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
    destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);

    //pc.printf(" get_gyro : %d, %d, %d\n\r", destination[0], destination[1], destination[2]);
}

void MPU9250_GET_ACCEL(int16_t * destination)
{
    char cmd[6];
    cmd[0] = ACCEL_XOUT_H;
    i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
    i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
    destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
    destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
    destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);

    //pc.printf(" get_accel : %d, %d, %d \n\r", destination[0], destination[1], destination[2]); 
}

void gyro_bias_f(void)
{
    int16_t gyro1[3];
    //int16_t gyro_bias[3];

    pc.printf(" Please keep still 5 seconds\n\r");
    for (int i = 0; i < 200; i++)
    {
        MPU9250_GET_GYRO(gyro1); //정지상태 각속도를 gyro1에 측정, 32.8로 스케일링, 100번 측정한 값을 모두 더함
        gyro_bias[0] = gyro_bias[0] + gyro1[0] / 32.8;
        gyro_bias[1] = gyro_bias[1] + gyro1[1] / 32.8;
        gyro_bias[2] = gyro_bias[2] + gyro1[2] / 32.8;
        //pc.printf(" gyro_bias finding i= %d\n\r", i);
        
    }
    gyro_bias[0] = gyro_bias[0] / 200.0; //500으로 나누어 평균 도출
    gyro_bias[1] = gyro_bias[1] / 200.0;
    gyro_bias[2] = gyro_bias[2] / 200.0;
    //pc.printf(" Gyro_Bias finding completed %\n\r");
}

void accel_bias_f(void)
{
    int16_t accel1[3];
    //int16_t accel_bias[3];

    pc.printf(" Please keep still 5 seconds\n\r");
    for (int i = 0; i < 100; i++)
    {
        MPU9250_GET_ACCEL(accel1); //정지상태 가속도를 accel1에 측정, 8192.0로 스케일링, 100번 측정한 값을 모두 더함
        accel_bias[0] = accel_bias[0] + accel1[0] / 8192.0;
        accel_bias[1] = accel_bias[1] + accel1[1] / 8192.0;
        accel_bias[2] = accel_bias[2] + accel1[2] / 8192.0;
        pc.printf(" accel_bias finding i= %d\n\r", i);
    }
    accel_bias[0] = accel_bias[0] / 100.0; //100으로 나누어 평균 도출
    accel_bias[1] = accel_bias[1] / 100.0;
    accel_bias[2] = accel_bias[2] / 100.0 - 1; //중력값 보정
    pc.printf(" Accel_Bias finding completed %\n\r");
}



int main()
{
    PWM1.period(0.0001);// 10 kHz PWM for PWM1~PWM4
    
    wait(3);
    pc.printf("------------------------\n\r");  
    NRF24L01.begin();
    NRF24L01.setDataRate(RF24_2MBPS); //RF24_2MBPS
    NRF24L01.setChannel(90);  // set channel 10 20 30
    NRF24L01.setPayloadSize(28);
    NRF24L01.setAddressWidth(5);
    NRF24L01.setRetries(2, 4); //1,3 2,8 1,8
    NRF24L01.enableAckPayload();
    NRF24L01.openReadingPipe(0, pipe);
    NRF24L01.startListening();

    MPU9250_RESET();
    MPU9250_INIT();
    pc.baud(38400);
    //pc.printf("%d th Who am I?\n\r", count);
    WHO_AM_I();
    


    //pc.printf(" count and gyro outputs(x,y,z)\n\r");  
    //pc.printf(" count and accel outputs(x,y,z)\n\r");

    gyro_bias_f();
    accel_bias_f();
    pc.printf(" \ngyro biases(deg/sec) %5.2f %5.2f %5.2f \n\r", gyro_bias[0], gyro_bias[1], gyro_bias[2]); // 초기화 됐는지 확인
    pc.printf(" accel biases(G) %5.2f %5.2f %5.2f \n\n\r", accel_bias[0], accel_bias[1], accel_bias[2]); // 초기화 됐는지 확인
    wait(1);
        

    loops.attach_us(&control_loop, 2500); // ticker 2500
}