Seunghee Jeong
.
Diff: main.cpp
- Revision:
- 0:56ba69e447e3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jul 18 06:37:14 2022 +0000
@@ -0,0 +1,639 @@
+#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
+}
\ No newline at end of file