main.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01.h"
00003 #include "RF24.h"
00004 #include "RF24_config.h"
00005 
00006 PwmOut PWM1(p21);
00007 PwmOut PWM2(p22);
00008 PwmOut PWM3(p23);
00009 PwmOut PWM4(p24);
00010 DigitalOut led1(LED1);
00011 DigitalOut led_switch(LED4);
00012 //DigitalIn switch_on(p5, PullDown);
00013 Ticker loops; // 프로그램 무한루프
00014 
00015 RF24 NRF24L01(p5, p6, p7, p15, p8);
00016 
00017 I2C i2c(p28, p27); //(I2C_SDA,I2C_SCL); 
00018 Serial pc(USBTX, USBRX); //D1D0
00019 
00020 int MPU9250_ADDRESS = (0x68 << 1), //0b11010000
00021 WHO_AM_I_MPU9250 = 0x75,
00022 FIFO_EN = 0x23,
00023 SMPLRT_DIV = 0x19,
00024 CONFIG = 0x1A,
00025 GYRO_CONFIG = 0x1B,
00026 ACCEL_CONFIG = 0x1C,
00027 ACCEL_CONFIG2 = 0x1D,
00028 ACCEL_XOUT_H = 0x3B,
00029 TEMP_OUT_H = 0x41,
00030 GYRO_XOUT_H = 0x43,
00031 PWR_MGMT_1 = 0x6B,
00032 PWR_MGMT_2 = 0x6C,
00033 INT_PIN_CFG = 0x37,
00034 AK8963_ADDRESS = (0x0C << 1), //0b00001100 0x0C<<1
00035 WHO_AM_I_AK8963 = 0x00, // should return 0x48
00036 AK8963_ST1 = 0x02,
00037 AK8963_XOUT_L = 0x03,
00038 AK8963_CNTL = 0x0A,
00039 AK8963_ASAX = 0x10;
00040 
00041 float gyro_bias[3] = { 0,0,0 };
00042 float accel_bias[3] = { 0,0,0 };
00043 int16_t gyro[3];
00044 int16_t accel[3];
00045 
00046 float accel_fu[3] = { 0,0,0 };
00047 float accel_f2[3] = { 0,0,0 };
00048 float accel_f1[3] = { 0,0,0 };
00049 float accel_f[3] = { 0,0,0 };
00050 
00051 float gyro_f[3] = { 0,0,0 };
00052 float gyro_angle[3] = { 0,0,0 };
00053 float roll_c, roll_accel, roll_g_n, roll_g_old, roll_a_n, roll_a_old = 0;
00054 float pitch_c, pitch_accel, pitch_g_n, pitch_g_old, pitch_a_n, pitch_a_old = 0;
00055 float tau = 0.3;
00056 float accel_tau = 1;
00057 
00058 const uint64_t pipe = 0x1212121212LL;
00059 int8_t recv[30];
00060 int16_t PITCH = 0, ROLL = 0, YAW = 0, THROTTLE = 0;
00061 int8_t ackData[30];
00062 int8_t flip1 = 1;
00063 
00064 // ETC
00065 float roll_controller, pitch_controller, yaw_controller = 0.0;
00066 float tilt_cont[4];
00067 float checksum[2] = {0,0};
00068 int BUT1, BUT2;
00069 
00070 //char BUT1, BUT2, prev_BUT2;
00071 int rf_fail_count = 0;
00072 
00073 int l_count = 0;
00074 float dt = 0.0025; // 0.0025
00075 float roll_f, pitch_f, yaw_f, yaw_c;
00076 
00077 // 적절하게 조절 0
00078 float K_scale = 0.0001;                             //    1
00079 float Kp_roll   = 15,     Kd_roll   = 40    ;    //     0.0015 0.0024
00080 float Kp_pitch  = 16,     Kd_pitch  = 40    ;    //     0.0015 0.0024
00081 float Kp_yaw    = 50,   Kd_yaw    = 10  ;  //
00082 
00083 float roll_cont, pitch_cont, yaw_cont = 0;
00084 float rollcompensate, pitchcompensate = 0;
00085 float thrust_c = 0.0;
00086 
00087 // 피치, 요 에러도 추가
00088 float roll_err, roll_rate_err;
00089 float pitch_err, pitch_rate_err;
00090 float yaw_err, yaw_rate_err;
00091 
00092 float thrust_in = 0.0;
00093 float roll_ref = 0.0, roll_rate_ref = 0.0;
00094 float pitch_ref = 0, pitch_rate_ref = 0;; // 추가
00095 float yaw_ref = 0, yaw_rate_ref = 0;
00096 
00097 float pwm1_pw, pwm2_pw, pwm3_pw, pwm4_pw; // 1이면 풀가동
00098 float ROLL_ERR_MAX = 40.0, ROLL_RATE_ERR_MAX = 40.0; // maximum errors considered
00099 float PITCH_ERR_MAX = 40.0, PITCH_RATE_ERR_MAX = 40.0; // maximum errors considered
00100 float YAW_ERR_MAX = 30.0, YAW_RATE_ERR_MAX = 30.0; // maximum errors considered
00101 
00102 void WHO_AM_I(void);
00103 void MPU9250_INIT(void);
00104 void MPU9250_RESET(void);
00105 void MPU9250_GET_GYRO(int16_t * destination);
00106 void MPU9250_GET_ACCEL(int16_t * destination);
00107 void gyro_bias_f(void);
00108 void control(void);
00109 void pwm_drive(void);
00110 void control_loop(void);
00111 int constrain_int16(int16_t x, int min, int max);
00112 float constrain_float(float x, float min, float max);
00113 void RF_READ();
00114 void yaw_reset();
00115 void moving_cont();
00116 float set_min(float x, float min);
00117 
00118 void RF_READ()
00119 {
00120     if (NRF24L01.available())
00121     {
00122         NRF24L01.read(recv, 10);
00123 
00124         // 스케일링 다른데서 곱하기
00125         ROLL = *(int16_t*)(&recv[0])- 3; //ROLL = - ROLL;           offset = 0
00126         PITCH = *(int16_t*)(&recv[2])- 7;//flip pitch and roll      offset = 6
00127         YAW = *(int16_t*)(&recv[4]) - 0; //                         offset = 2
00128         THROTTLE = *(int16_t*)(&recv[6]) - 0; //                    offset = 0
00129         BUT1 = recv[8];
00130         BUT2 = recv[9]; //should hold value here
00131         //pc.printf("\r RF_READ : %d, %d, %d, %d \n\r", (int)ROLL, (int)PITCH, (int)YAW, (int)THROTTLE);
00132         rf_fail_count = 0;
00133     }
00134 
00135     else
00136     {
00137         rf_fail_count = rf_fail_count + 1;
00138                 
00139         //printf(" rf_fail_count : %d\n\r", rf_fail_count);
00140         
00141         if (rf_fail_count >= 20 && rf_fail_count < 100)
00142         {
00143             printf(" rf_fail_count : %d\n\r", rf_fail_count);
00144             THROTTLE = THROTTLE - 2;
00145             THROTTLE = constrain_int16(THROTTLE, 0, 1023);
00146         }
00147         if (rf_fail_count >= 50)
00148         {
00149             THROTTLE = 0;
00150         }
00151         if (rf_fail_count >= 100)
00152         {
00153             rf_fail_count = 100;
00154         }
00155     }
00156 }
00157 
00158 void yaw_reset()
00159 {
00160     if(set_min(yaw_ref,20)!= 0)
00161     gyro_angle[2] = 0;
00162 }
00163 
00164 void control(void)
00165 {
00166     //pc.printf(" _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in);
00167 
00168     roll_err = roll_ref - roll_f;
00169     roll_rate_err = roll_rate_ref - gyro_f[0];
00170 
00171     if (roll_err > ROLL_ERR_MAX)
00172         roll_err = ROLL_ERR_MAX;
00173     else if (roll_err < -(ROLL_ERR_MAX))
00174         roll_err = -(ROLL_ERR_MAX);
00175 
00176     if (roll_rate_err > ROLL_RATE_ERR_MAX)
00177         roll_rate_err = ROLL_RATE_ERR_MAX;
00178     else if (roll_rate_err < -(ROLL_RATE_ERR_MAX))
00179         roll_rate_err = -(ROLL_RATE_ERR_MAX);
00180 
00181     roll_cont = K_scale * (Kp_roll * roll_err + Kd_roll * roll_rate_err);
00182 
00183     if (roll_cont > 0.33)
00184         roll_cont = 0.33;
00185     else if (roll_cont < -0.33)
00186         roll_cont = -0.33;
00187 
00188     /*-------------------------------------------------------------------*/
00189 
00190     pitch_err = pitch_ref - pitch_f;
00191     pitch_rate_err = pitch_rate_ref - gyro_f[1];
00192 
00193     if (pitch_err > PITCH_ERR_MAX)
00194         pitch_err = PITCH_ERR_MAX;
00195     else if (pitch_err < -(PITCH_ERR_MAX))
00196         pitch_err = -(PITCH_ERR_MAX);
00197 
00198     if (pitch_rate_err > PITCH_RATE_ERR_MAX)
00199         pitch_rate_err = PITCH_RATE_ERR_MAX;
00200     else if (pitch_rate_err < -(PITCH_RATE_ERR_MAX))
00201         pitch_rate_err = -(PITCH_RATE_ERR_MAX);
00202 
00203     pitch_cont = K_scale * (Kp_pitch * pitch_err + Kd_pitch * pitch_rate_err);
00204 
00205     if (pitch_cont > 0.33)
00206         pitch_cont = 0.33;
00207     else if (pitch_cont < -0.33)
00208         pitch_cont = -0.33;
00209 
00210 
00211     /*-------------------------------------------------------------------------*/
00212 
00213     yaw_err = yaw_ref - gyro_angle[2];
00214     yaw_rate_err = yaw_rate_ref - gyro_f[2];
00215 
00216     if (yaw_err > YAW_ERR_MAX)
00217         yaw_err = YAW_ERR_MAX;
00218     else if (yaw_err < -(YAW_ERR_MAX))
00219         yaw_err = -(YAW_ERR_MAX);
00220 
00221     if (yaw_rate_err > YAW_RATE_ERR_MAX)
00222         yaw_rate_err = YAW_RATE_ERR_MAX;
00223     else if (yaw_rate_err < -(YAW_RATE_ERR_MAX))
00224         yaw_rate_err = -(YAW_RATE_ERR_MAX);
00225     if(yaw_ref == 0)
00226         yaw_cont = K_scale * (Kp_yaw * yaw_err + Kd_yaw * yaw_rate_err);
00227     else
00228     {
00229         yaw_cont = K_scale * (Kp_yaw * yaw_err);
00230         yaw_ref = 0;
00231     }
00232     
00233     if (yaw_cont > 0.20)
00234         yaw_cont = 0.20;
00235     else if (yaw_cont < -0.20)
00236         yaw_cont = -0.20;
00237     
00238     thrust_c = thrust_in / (float)1654;  //control_scale = 2000
00239     if (thrust_c > 0.8)
00240         thrust_c = 0.8;
00241         
00242 }
00243 
00244 //control tilted moving
00245 void moving_cont()
00246 {
00247     checksum[0] = roll_ref + pitch_ref;
00248     if(checksum[0] == checksum[1])
00249     {
00250         rollcompensate = accel_f[1]*(float)(0.5);
00251         pitchcompensate = accel_f[0]*(float)(0.5);
00252         rollcompensate = constrain_float(rollcompensate,-0.05,0.05);
00253         pitchcompensate = constrain_float(pitchcompensate,-0.05,0.05);
00254         
00255     }
00256     else
00257     {
00258         rollcompensate = 0;
00259         pitchcompensate = 0;
00260         
00261     }
00262     checksum[1] = checksum[0];
00263 }    
00264 
00265 
00266 void pwm_drive(void)//roll+ = pitch-, 
00267 {
00268     //pc.printf(" _c :  %5.2f  %5.2f  %5.2f  %5.2f \n\r", roll_c, pitch_c, yaw_c, thrust_c);
00269     pwm1_pw = -(roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) -yaw_cont + thrust_c  ;  // 최댓값 1, 최솟값 0
00270     pwm2_pw = (roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) + yaw_cont + thrust_c  ;
00271     pwm3_pw = -(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) + yaw_cont + thrust_c  ;
00272     pwm4_pw = +(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) - yaw_cont + thrust_c ;
00273     
00274     if(pwm1_pw >= 1)
00275         pwm1_pw = 1;
00276     else if(pwm1_pw < 0)
00277         pwm1_pw = 0;
00278     if(pwm2_pw >= 1)
00279         pwm2_pw = 1;
00280     else if(pwm2_pw < 0)
00281         pwm2_pw = 0;
00282     if(pwm3_pw >= 1)
00283         pwm3_pw = 1;
00284     else if(pwm3_pw < 0)
00285         pwm3_pw = 0;
00286     if(pwm4_pw >= 1)
00287         pwm4_pw = 1;
00288     else if(pwm4_pw < 0)
00289         pwm4_pw = 0;
00290     
00291     if (rf_fail_count>=50) // switch_on = 1;
00292     {
00293        pwm1_pw = 0.0;
00294        pwm2_pw = 0.0;
00295        pwm3_pw = 0.0;
00296        pwm4_pw = 0.0;
00297     }
00298 
00299       
00300     PWM1 = pwm1_pw;
00301     PWM2 = pwm2_pw;
00302     PWM3 = pwm3_pw;
00303     PWM4 = pwm4_pw;   
00304 }
00305 
00306 void control_loop(void)
00307 {
00308 
00309     MPU9250_GET_GYRO(gyro);
00310     gyro_f[0] = gyro[0] / 32.8 - gyro_bias[0];
00311     gyro_f[1] = -(gyro[1] / 32.8 - gyro_bias[1]);
00312     gyro_f[2] = -(gyro[2] / 32.8 - gyro_bias[2]);
00313 
00314     MPU9250_GET_ACCEL(accel);
00315     accel_f[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED)   // 9.8m/s^2 ( G )
00316     accel_f[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
00317     accel_f[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);
00318 
00319     /*
00320     //accel low pass filter
00321     accel_fu[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED)   // 9.8m/s^2 ( G )
00322     accel_fu[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
00323     accel_fu[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);
00324 
00325     //accel low pass filter -----------
00326     accel_f2[0]=(1-dt/accel_tau)*accel_f1[0]+dt/accel_tau*accel_fu[0];
00327     accel_f2[1]=(1-dt/accel_tau)*accel_f1[1]+dt/accel_tau*accel_fu[1];
00328     accel_f2[2]=(1-dt/accel_tau)*accel_f1[2]+dt/accel_tau*accel_fu[2];
00329     
00330     accel_f1[0]=accel_f2[0];
00331     accel_f1[1]=accel_f2[1];
00332     accel_f1[2]=accel_f2[2];
00333     
00334     accel_f[0]=accel_f2[0];
00335     accel_f[1]=accel_f2[1];
00336     accel_f[2]=accel_f2[2];
00337     */
00338     //----------------------------------
00339     
00340     //get roll and pitch from Accelometer values
00341     roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14;
00342     //roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14;         //degree '
00343     pitch_accel = asin(constrain_float(accel_f[0],(float)-1,(float)1)) * 180.0 / 3.14; // apply constrain to avoid NaN
00344 
00345     //get Roll, Pitch, Yaw from Gyro
00346     gyro_angle[0] = gyro_angle[0] + (gyro_f[0]) * dt;   //Roll
00347     gyro_angle[1] = gyro_angle[1] + (gyro_f[1]) * dt;   //Pitch
00348     gyro_angle[2] = gyro_angle[2] + (gyro_f[2]) * dt;   //Yaw
00349 
00350     // YAW 360도 넘길 때 빼는 방법
00351 
00352    //get roll and pitch by applying Complementary Filter
00353     roll_g_n = (1 - dt / tau) * roll_g_old + dt * gyro_f[0];
00354     roll_a_n = (1 - dt / tau) * roll_a_old + dt / tau * roll_accel;
00355 
00356     pitch_g_n = (1 - dt / tau) * pitch_g_old + dt * gyro_f[1];
00357     pitch_a_n = (1 - dt / tau) * pitch_a_old + dt / tau * pitch_accel;
00358 
00359     roll_g_old = roll_g_n;
00360     roll_a_old = roll_a_n;
00361 
00362     pitch_g_old = pitch_g_n;
00363     pitch_a_old = pitch_a_n;
00364     //pc.printf("\r%5.2f\t%5.2f",pitch_g_old, pitch_a_old);
00365     roll_f = roll_g_old + roll_a_old;
00366     pitch_f = pitch_g_old + pitch_a_old;        //this value is weird 
00367 
00368     RF_READ();
00369     /*
00370     ROLL = ROLL - (int)offset[0];
00371     PITCH = PITCH - (int)offset[1];
00372     YAW = YAW - (int)offset[2];
00373     THROTTLE = THROTTLE - (int)offset[3];
00374     */
00375     
00376     roll_ref = -(set_min((float)ROLL,6));
00377     pitch_ref = set_min((float)PITCH,6);
00378     yaw_ref = -(set_min((float)YAW,6));
00379     thrust_in = set_min((float)THROTTLE,6);
00380     
00381     yaw_reset();
00382     
00383     control();
00384     
00385     //moving_cont();
00386     
00387     pwm_drive();
00388 
00389     
00390   /*
00391     if (l_count == 400 ) {
00392         //pc.printf("l_count per 10")
00393     
00394         //pc.printf("----------------------------------\n\r");
00395         //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]);  // 각속도
00396         //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
00397         //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
00398         //pc.printf("\r Accel to angle  :\t %5.2f\t %5.2f\n\r", roll_accel, pitch_accel);
00399         
00400         //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 각도
00401     
00402        // pc.printf("\r control input    : %d, %d, %d, %f \n\r\n", (int)ROLL, (int)PITCH, (int)YAW, (float)THROTTLE);    //RF_READ
00403         //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);    // 모터에 들어가는 값
00404         //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);
00405         //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
00406         //pc.printf("\r RPYT_err   :\t%5.3f\t%5.3f\t%5.3f \n\r", roll_err, pitch_err, yaw_err);
00407         //pc.printf("\t\tRoll\tPitch\tYaw\tThrust\t\r\n");
00408         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 각도 표 출력
00409         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 표 출력
00410         
00411         
00412         //pc.printf("\r _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in); // 제어에 쓰이는 값
00413         
00414         l_count = 1;
00415     }
00416     
00417     l_count = l_count + 1;
00418     */
00419 
00420     //wait(dt);
00421     
00422 }
00423 float set_min(float x, float min)
00424 {
00425     if ( x<min && x> -min)
00426     x = 0;  
00427     return x;  
00428 }
00429 
00430 int constrain_int16(int16_t x, int min, int max)
00431 {
00432     if (x > max)
00433         x = max;
00434     else if (x < min)
00435         x = min;
00436     return x;
00437 }
00438 
00439 float constrain_float(float x, float min, float max)
00440 {
00441     if (x > max)
00442         x = max;
00443     else if (x < min)
00444         x = min;
00445     return x;
00446 }
00447 
00448 void MPU9250_RESET()
00449 {
00450     // reset device
00451     char cmd[2];
00452     cmd[0] = PWR_MGMT_1; //status
00453     cmd[1] = 0x80;
00454     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00455     wait(0.1);
00456 }
00457 
00458 void MPU9250_INIT()
00459 {
00460     char cmd[3];
00461     cmd[0] = PWR_MGMT_1; //reset
00462     cmd[1] = 0x80;
00463     i2c.write(MPU9250_ADDRESS, cmd, 2);
00464     pc.printf("MPU 1 \n\r");
00465     wait(0.1);
00466 
00467     cmd[0] = PWR_MGMT_1; // Auto selects the best available clock source PLL if ready,
00468     cmd[1] = 0x01; // else use the Internal oscillator
00469     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00470     pc.printf("MPU 2 \n\r");
00471     wait(0.1);
00472 
00473     // Select Bandwidth of Gyroscopes BW 41Hz 1KHz internal sampling
00474     cmd[0] = CONFIG;
00475     cmd[1] = 0x03;
00476     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00477     pc.printf("MPU 3 \n\r");
00478     wait(0.1);
00479 
00480     // sample rate = (internal sample rate) /(1+4) = 1000/5=200 Hz 
00481     cmd[0] = SMPLRT_DIV;
00482     cmd[1] = 0x00;//0x04 
00483     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00484     pc.printf("MPU 4 \n\r");
00485     wait(0.1);
00486 
00487 
00488     cmd[0] = GYRO_CONFIG;
00489     cmd[1] = 0b00010000;// Gyro full scale 1000 deg/sec; Gyro DLPF Enable
00490     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00491     pc.printf("MPU 6 \n\r");
00492     wait(0.1);
00493 
00494     // Set accelerometer configuration
00495     // Accel fulll sacle range +/- 4g
00496     cmd[0] = ACCEL_CONFIG;
00497     cmd[1] = 0b00001000;// Accel 
00498     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00499     pc.printf("MPU 8 \n\r");
00500     wait(0.1);
00501 
00502     cmd[0] = ACCEL_CONFIG2;
00503     cmd[1] = 0b00000101; // 218 Hz bandwidth, 1kHz Sampling for accelerometer
00504     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00505     pc.printf("MPU 10 \n\r");
00506     wait(0.1);
00507 
00508     //XYZ Gyro acel enable
00509     cmd[0] = PWR_MGMT_2;
00510     cmd[1] = 0x00;
00511     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00512     pc.printf("MPU 11 \n\r");
00513     wait(0.1);
00514 
00515     cmd[0] = INT_PIN_CFG;
00516     cmd[1] = 0x22; //0x02 
00517     i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
00518     pc.printf("MPU 12 \n\r");
00519     wait(0.1);
00520 }
00521 
00522 void WHO_AM_I()
00523 {
00524     char cmd[2];
00525     char rvd[2];
00526 
00527     cmd[0] = WHO_AM_I_MPU9250;
00528     i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
00529     i2c.read(MPU9250_ADDRESS | 1, rvd, 1, 0);
00530 
00531     pc.printf(" IMU device id is 0x%x \n\r", rvd[0]);
00532     wait(1);
00533 }
00534 
00535 void MPU9250_GET_GYRO(int16_t * destination)
00536 {
00537     char cmd[6];
00538     cmd[0] = GYRO_XOUT_H;
00539     i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
00540     i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
00541     destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
00542     destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
00543     destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);
00544 
00545     //pc.printf(" get_gyro : %d, %d, %d\n\r", destination[0], destination[1], destination[2]);
00546 }
00547 
00548 void MPU9250_GET_ACCEL(int16_t * destination)
00549 {
00550     char cmd[6];
00551     cmd[0] = ACCEL_XOUT_H;
00552     i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
00553     i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
00554     destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
00555     destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
00556     destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);
00557 
00558     //pc.printf(" get_accel : %d, %d, %d \n\r", destination[0], destination[1], destination[2]); 
00559 }
00560 
00561 void gyro_bias_f(void)
00562 {
00563     int16_t gyro1[3];
00564     //int16_t gyro_bias[3];
00565 
00566     pc.printf(" Please keep still 5 seconds\n\r");
00567     for (int i = 0; i < 200; i++)
00568     {
00569         MPU9250_GET_GYRO(gyro1); //정지상태 각속도를 gyro1에 측정, 32.8로 스케일링, 100번 측정한 값을 모두 더함
00570         gyro_bias[0] = gyro_bias[0] + gyro1[0] / 32.8;
00571         gyro_bias[1] = gyro_bias[1] + gyro1[1] / 32.8;
00572         gyro_bias[2] = gyro_bias[2] + gyro1[2] / 32.8;
00573         //pc.printf(" gyro_bias finding i= %d\n\r", i);
00574         
00575     }
00576     gyro_bias[0] = gyro_bias[0] / 200.0; //500으로 나누어 평균 도출
00577     gyro_bias[1] = gyro_bias[1] / 200.0;
00578     gyro_bias[2] = gyro_bias[2] / 200.0;
00579     //pc.printf(" Gyro_Bias finding completed %\n\r");
00580 }
00581 
00582 void accel_bias_f(void)
00583 {
00584     int16_t accel1[3];
00585     //int16_t accel_bias[3];
00586 
00587     pc.printf(" Please keep still 5 seconds\n\r");
00588     for (int i = 0; i < 100; i++)
00589     {
00590         MPU9250_GET_ACCEL(accel1); //정지상태 가속도를 accel1에 측정, 8192.0로 스케일링, 100번 측정한 값을 모두 더함
00591         accel_bias[0] = accel_bias[0] + accel1[0] / 8192.0;
00592         accel_bias[1] = accel_bias[1] + accel1[1] / 8192.0;
00593         accel_bias[2] = accel_bias[2] + accel1[2] / 8192.0;
00594         pc.printf(" accel_bias finding i= %d\n\r", i);
00595     }
00596     accel_bias[0] = accel_bias[0] / 100.0; //100으로 나누어 평균 도출
00597     accel_bias[1] = accel_bias[1] / 100.0;
00598     accel_bias[2] = accel_bias[2] / 100.0 - 1; //중력값 보정
00599     pc.printf(" Accel_Bias finding completed %\n\r");
00600 }
00601 
00602 
00603 
00604 int main()
00605 {
00606     PWM1.period(0.0001);// 10 kHz PWM for PWM1~PWM4
00607     
00608     wait(3);
00609     pc.printf("------------------------\n\r");  
00610     NRF24L01.begin();
00611     NRF24L01.setDataRate(RF24_2MBPS); //RF24_2MBPS
00612     NRF24L01.setChannel(90);  // set channel 10 20 30
00613     NRF24L01.setPayloadSize(28);
00614     NRF24L01.setAddressWidth(5);
00615     NRF24L01.setRetries(2, 4); //1,3 2,8 1,8
00616     NRF24L01.enableAckPayload();
00617     NRF24L01.openReadingPipe(0, pipe);
00618     NRF24L01.startListening();
00619 
00620     MPU9250_RESET();
00621     MPU9250_INIT();
00622     pc.baud(38400);
00623     //pc.printf("%d th Who am I?\n\r", count);
00624     WHO_AM_I();
00625     
00626 
00627 
00628     //pc.printf(" count and gyro outputs(x,y,z)\n\r");  
00629     //pc.printf(" count and accel outputs(x,y,z)\n\r");
00630 
00631     gyro_bias_f();
00632     accel_bias_f();
00633     pc.printf(" \ngyro biases(deg/sec) %5.2f %5.2f %5.2f \n\r", gyro_bias[0], gyro_bias[1], gyro_bias[2]); // 초기화 됐는지 확인
00634     pc.printf(" accel biases(G) %5.2f %5.2f %5.2f \n\n\r", accel_bias[0], accel_bias[1], accel_bias[2]); // 초기화 됐는지 확인
00635     wait(1);
00636         
00637 
00638     loops.attach_us(&control_loop, 2500); // ticker 2500
00639 }