Seunghee Jeong / Mbed 2 deprecated Dronequadrotor

.

Dependencies:   mbed

main.cpp@0:56ba69e447e3, 23 months ago (annotated)

Committer:
jjeong
Date:
Mon Jul 18 06:37:14 2022 +0000
Revision:
0:56ba69e447e3
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jjeong 0:56ba69e447e3 1 #include "mbed.h"
jjeong 0:56ba69e447e3 2 #include "nRF24L01.h"
jjeong 0:56ba69e447e3 3 #include "RF24.h"
jjeong 0:56ba69e447e3 4 #include "RF24_config.h"
jjeong 0:56ba69e447e3 5
jjeong 0:56ba69e447e3 6 PwmOut PWM1(p21);
jjeong 0:56ba69e447e3 7 PwmOut PWM2(p22);
jjeong 0:56ba69e447e3 8 PwmOut PWM3(p23);
jjeong 0:56ba69e447e3 9 PwmOut PWM4(p24);
jjeong 0:56ba69e447e3 10 DigitalOut led1(LED1);
jjeong 0:56ba69e447e3 11 DigitalOut led_switch(LED4);
jjeong 0:56ba69e447e3 12 //DigitalIn switch_on(p5, PullDown);
jjeong 0:56ba69e447e3 13 Ticker loops; // 프로그램 무한루프
jjeong 0:56ba69e447e3 14
jjeong 0:56ba69e447e3 15 RF24 NRF24L01(p5, p6, p7, p15, p8);
jjeong 0:56ba69e447e3 16
jjeong 0:56ba69e447e3 17 I2C i2c(p28, p27); //(I2C_SDA,I2C_SCL);
jjeong 0:56ba69e447e3 18 Serial pc(USBTX, USBRX); //D1D0
jjeong 0:56ba69e447e3 19
jjeong 0:56ba69e447e3 20 int MPU9250_ADDRESS = (0x68 << 1), //0b11010000
jjeong 0:56ba69e447e3 21 WHO_AM_I_MPU9250 = 0x75,
jjeong 0:56ba69e447e3 22 FIFO_EN = 0x23,
jjeong 0:56ba69e447e3 23 SMPLRT_DIV = 0x19,
jjeong 0:56ba69e447e3 24 CONFIG = 0x1A,
jjeong 0:56ba69e447e3 25 GYRO_CONFIG = 0x1B,
jjeong 0:56ba69e447e3 26 ACCEL_CONFIG = 0x1C,
jjeong 0:56ba69e447e3 27 ACCEL_CONFIG2 = 0x1D,
jjeong 0:56ba69e447e3 28 ACCEL_XOUT_H = 0x3B,
jjeong 0:56ba69e447e3 29 TEMP_OUT_H = 0x41,
jjeong 0:56ba69e447e3 30 GYRO_XOUT_H = 0x43,
jjeong 0:56ba69e447e3 31 PWR_MGMT_1 = 0x6B,
jjeong 0:56ba69e447e3 32 PWR_MGMT_2 = 0x6C,
jjeong 0:56ba69e447e3 33 INT_PIN_CFG = 0x37,
jjeong 0:56ba69e447e3 34 AK8963_ADDRESS = (0x0C << 1), //0b00001100 0x0C<<1
jjeong 0:56ba69e447e3 35 WHO_AM_I_AK8963 = 0x00, // should return 0x48
jjeong 0:56ba69e447e3 36 AK8963_ST1 = 0x02,
jjeong 0:56ba69e447e3 37 AK8963_XOUT_L = 0x03,
jjeong 0:56ba69e447e3 38 AK8963_CNTL = 0x0A,
jjeong 0:56ba69e447e3 39 AK8963_ASAX = 0x10;
jjeong 0:56ba69e447e3 40
jjeong 0:56ba69e447e3 41 float gyro_bias[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 42 float accel_bias[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 43 int16_t gyro[3];
jjeong 0:56ba69e447e3 44 int16_t accel[3];
jjeong 0:56ba69e447e3 45
jjeong 0:56ba69e447e3 46 float accel_fu[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 47 float accel_f2[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 48 float accel_f1[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 49 float accel_f[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 50
jjeong 0:56ba69e447e3 51 float gyro_f[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 52 float gyro_angle[3] = { 0,0,0 };
jjeong 0:56ba69e447e3 53 float roll_c, roll_accel, roll_g_n, roll_g_old, roll_a_n, roll_a_old = 0;
jjeong 0:56ba69e447e3 54 float pitch_c, pitch_accel, pitch_g_n, pitch_g_old, pitch_a_n, pitch_a_old = 0;
jjeong 0:56ba69e447e3 55 float tau = 0.3;
jjeong 0:56ba69e447e3 56 float accel_tau = 1;
jjeong 0:56ba69e447e3 57
jjeong 0:56ba69e447e3 58 const uint64_t pipe = 0x1212121212LL;
jjeong 0:56ba69e447e3 59 int8_t recv[30];
jjeong 0:56ba69e447e3 60 int16_t PITCH = 0, ROLL = 0, YAW = 0, THROTTLE = 0;
jjeong 0:56ba69e447e3 61 int8_t ackData[30];
jjeong 0:56ba69e447e3 62 int8_t flip1 = 1;
jjeong 0:56ba69e447e3 63
jjeong 0:56ba69e447e3 64 // ETC
jjeong 0:56ba69e447e3 65 float roll_controller, pitch_controller, yaw_controller = 0.0;
jjeong 0:56ba69e447e3 66 float tilt_cont[4];
jjeong 0:56ba69e447e3 67 float checksum[2] = {0,0};
jjeong 0:56ba69e447e3 68 int BUT1, BUT2;
jjeong 0:56ba69e447e3 69
jjeong 0:56ba69e447e3 70 //char BUT1, BUT2, prev_BUT2;
jjeong 0:56ba69e447e3 71 int rf_fail_count = 0;
jjeong 0:56ba69e447e3 72
jjeong 0:56ba69e447e3 73 int l_count = 0;
jjeong 0:56ba69e447e3 74 float dt = 0.0025; // 0.0025
jjeong 0:56ba69e447e3 75 float roll_f, pitch_f, yaw_f, yaw_c;
jjeong 0:56ba69e447e3 76
jjeong 0:56ba69e447e3 77 // 적절하게 조절 0
jjeong 0:56ba69e447e3 78 float K_scale = 0.0001; // 1
jjeong 0:56ba69e447e3 79 float Kp_roll = 15, Kd_roll = 40 ; // 0.0015 0.0024
jjeong 0:56ba69e447e3 80 float Kp_pitch = 16, Kd_pitch = 40 ; // 0.0015 0.0024
jjeong 0:56ba69e447e3 81 float Kp_yaw = 50, Kd_yaw = 10 ; //
jjeong 0:56ba69e447e3 82
jjeong 0:56ba69e447e3 83 float roll_cont, pitch_cont, yaw_cont = 0;
jjeong 0:56ba69e447e3 84 float rollcompensate, pitchcompensate = 0;
jjeong 0:56ba69e447e3 85 float thrust_c = 0.0;
jjeong 0:56ba69e447e3 86
jjeong 0:56ba69e447e3 87 // 피치, 요 에러도 추가
jjeong 0:56ba69e447e3 88 float roll_err, roll_rate_err;
jjeong 0:56ba69e447e3 89 float pitch_err, pitch_rate_err;
jjeong 0:56ba69e447e3 90 float yaw_err, yaw_rate_err;
jjeong 0:56ba69e447e3 91
jjeong 0:56ba69e447e3 92 float thrust_in = 0.0;
jjeong 0:56ba69e447e3 93 float roll_ref = 0.0, roll_rate_ref = 0.0;
jjeong 0:56ba69e447e3 94 float pitch_ref = 0, pitch_rate_ref = 0;; // 추가
jjeong 0:56ba69e447e3 95 float yaw_ref = 0, yaw_rate_ref = 0;
jjeong 0:56ba69e447e3 96
jjeong 0:56ba69e447e3 97 float pwm1_pw, pwm2_pw, pwm3_pw, pwm4_pw; // 1이면 풀가동
jjeong 0:56ba69e447e3 98 float ROLL_ERR_MAX = 40.0, ROLL_RATE_ERR_MAX = 40.0; // maximum errors considered
jjeong 0:56ba69e447e3 99 float PITCH_ERR_MAX = 40.0, PITCH_RATE_ERR_MAX = 40.0; // maximum errors considered
jjeong 0:56ba69e447e3 100 float YAW_ERR_MAX = 30.0, YAW_RATE_ERR_MAX = 30.0; // maximum errors considered
jjeong 0:56ba69e447e3 101
jjeong 0:56ba69e447e3 102 void WHO_AM_I(void);
jjeong 0:56ba69e447e3 103 void MPU9250_INIT(void);
jjeong 0:56ba69e447e3 104 void MPU9250_RESET(void);
jjeong 0:56ba69e447e3 105 void MPU9250_GET_GYRO(int16_t * destination);
jjeong 0:56ba69e447e3 106 void MPU9250_GET_ACCEL(int16_t * destination);
jjeong 0:56ba69e447e3 107 void gyro_bias_f(void);
jjeong 0:56ba69e447e3 108 void control(void);
jjeong 0:56ba69e447e3 109 void pwm_drive(void);
jjeong 0:56ba69e447e3 110 void control_loop(void);
jjeong 0:56ba69e447e3 111 int constrain_int16(int16_t x, int min, int max);
jjeong 0:56ba69e447e3 112 float constrain_float(float x, float min, float max);
jjeong 0:56ba69e447e3 113 void RF_READ();
jjeong 0:56ba69e447e3 114 void yaw_reset();
jjeong 0:56ba69e447e3 115 void moving_cont();
jjeong 0:56ba69e447e3 116 float set_min(float x, float min);
jjeong 0:56ba69e447e3 117
jjeong 0:56ba69e447e3 118 void RF_READ()
jjeong 0:56ba69e447e3 119 {
jjeong 0:56ba69e447e3 120 if (NRF24L01.available())
jjeong 0:56ba69e447e3 121 {
jjeong 0:56ba69e447e3 122 NRF24L01.read(recv, 10);
jjeong 0:56ba69e447e3 123
jjeong 0:56ba69e447e3 124 // 스케일링 다른데서 곱하기
jjeong 0:56ba69e447e3 125 ROLL = *(int16_t*)(&recv[0])- 3; //ROLL = - ROLL; offset = 0
jjeong 0:56ba69e447e3 126 PITCH = *(int16_t*)(&recv[2])- 7;//flip pitch and roll offset = 6
jjeong 0:56ba69e447e3 127 YAW = *(int16_t*)(&recv[4]) - 0; // offset = 2
jjeong 0:56ba69e447e3 128 THROTTLE = *(int16_t*)(&recv[6]) - 0; // offset = 0
jjeong 0:56ba69e447e3 129 BUT1 = recv[8];
jjeong 0:56ba69e447e3 130 BUT2 = recv[9]; //should hold value here
jjeong 0:56ba69e447e3 131 //pc.printf("\r RF_READ : %d, %d, %d, %d \n\r", (int)ROLL, (int)PITCH, (int)YAW, (int)THROTTLE);
jjeong 0:56ba69e447e3 132 rf_fail_count = 0;
jjeong 0:56ba69e447e3 133 }
jjeong 0:56ba69e447e3 134
jjeong 0:56ba69e447e3 135 else
jjeong 0:56ba69e447e3 136 {
jjeong 0:56ba69e447e3 137 rf_fail_count = rf_fail_count + 1;
jjeong 0:56ba69e447e3 138
jjeong 0:56ba69e447e3 139 //printf(" rf_fail_count : %d\n\r", rf_fail_count);
jjeong 0:56ba69e447e3 140
jjeong 0:56ba69e447e3 141 if (rf_fail_count >= 20 && rf_fail_count < 100)
jjeong 0:56ba69e447e3 142 {
jjeong 0:56ba69e447e3 143 printf(" rf_fail_count : %d\n\r", rf_fail_count);
jjeong 0:56ba69e447e3 144 THROTTLE = THROTTLE - 2;
jjeong 0:56ba69e447e3 145 THROTTLE = constrain_int16(THROTTLE, 0, 1023);
jjeong 0:56ba69e447e3 146 }
jjeong 0:56ba69e447e3 147 if (rf_fail_count >= 50)
jjeong 0:56ba69e447e3 148 {
jjeong 0:56ba69e447e3 149 THROTTLE = 0;
jjeong 0:56ba69e447e3 150 }
jjeong 0:56ba69e447e3 151 if (rf_fail_count >= 100)
jjeong 0:56ba69e447e3 152 {
jjeong 0:56ba69e447e3 153 rf_fail_count = 100;
jjeong 0:56ba69e447e3 154 }
jjeong 0:56ba69e447e3 155 }
jjeong 0:56ba69e447e3 156 }
jjeong 0:56ba69e447e3 157
jjeong 0:56ba69e447e3 158 void yaw_reset()
jjeong 0:56ba69e447e3 159 {
jjeong 0:56ba69e447e3 160 if(set_min(yaw_ref,20)!= 0)
jjeong 0:56ba69e447e3 161 gyro_angle[2] = 0;
jjeong 0:56ba69e447e3 162 }
jjeong 0:56ba69e447e3 163
jjeong 0:56ba69e447e3 164 void control(void)
jjeong 0:56ba69e447e3 165 {
jjeong 0:56ba69e447e3 166 //pc.printf(" _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in);
jjeong 0:56ba69e447e3 167
jjeong 0:56ba69e447e3 168 roll_err = roll_ref - roll_f;
jjeong 0:56ba69e447e3 169 roll_rate_err = roll_rate_ref - gyro_f[0];
jjeong 0:56ba69e447e3 170
jjeong 0:56ba69e447e3 171 if (roll_err > ROLL_ERR_MAX)
jjeong 0:56ba69e447e3 172 roll_err = ROLL_ERR_MAX;
jjeong 0:56ba69e447e3 173 else if (roll_err < -(ROLL_ERR_MAX))
jjeong 0:56ba69e447e3 174 roll_err = -(ROLL_ERR_MAX);
jjeong 0:56ba69e447e3 175
jjeong 0:56ba69e447e3 176 if (roll_rate_err > ROLL_RATE_ERR_MAX)
jjeong 0:56ba69e447e3 177 roll_rate_err = ROLL_RATE_ERR_MAX;
jjeong 0:56ba69e447e3 178 else if (roll_rate_err < -(ROLL_RATE_ERR_MAX))
jjeong 0:56ba69e447e3 179 roll_rate_err = -(ROLL_RATE_ERR_MAX);
jjeong 0:56ba69e447e3 180
jjeong 0:56ba69e447e3 181 roll_cont = K_scale * (Kp_roll * roll_err + Kd_roll * roll_rate_err);
jjeong 0:56ba69e447e3 182
jjeong 0:56ba69e447e3 183 if (roll_cont > 0.33)
jjeong 0:56ba69e447e3 184 roll_cont = 0.33;
jjeong 0:56ba69e447e3 185 else if (roll_cont < -0.33)
jjeong 0:56ba69e447e3 186 roll_cont = -0.33;
jjeong 0:56ba69e447e3 187
jjeong 0:56ba69e447e3 188 /*-------------------------------------------------------------------*/
jjeong 0:56ba69e447e3 189
jjeong 0:56ba69e447e3 190 pitch_err = pitch_ref - pitch_f;
jjeong 0:56ba69e447e3 191 pitch_rate_err = pitch_rate_ref - gyro_f[1];
jjeong 0:56ba69e447e3 192
jjeong 0:56ba69e447e3 193 if (pitch_err > PITCH_ERR_MAX)
jjeong 0:56ba69e447e3 194 pitch_err = PITCH_ERR_MAX;
jjeong 0:56ba69e447e3 195 else if (pitch_err < -(PITCH_ERR_MAX))
jjeong 0:56ba69e447e3 196 pitch_err = -(PITCH_ERR_MAX);
jjeong 0:56ba69e447e3 197
jjeong 0:56ba69e447e3 198 if (pitch_rate_err > PITCH_RATE_ERR_MAX)
jjeong 0:56ba69e447e3 199 pitch_rate_err = PITCH_RATE_ERR_MAX;
jjeong 0:56ba69e447e3 200 else if (pitch_rate_err < -(PITCH_RATE_ERR_MAX))
jjeong 0:56ba69e447e3 201 pitch_rate_err = -(PITCH_RATE_ERR_MAX);
jjeong 0:56ba69e447e3 202
jjeong 0:56ba69e447e3 203 pitch_cont = K_scale * (Kp_pitch * pitch_err + Kd_pitch * pitch_rate_err);
jjeong 0:56ba69e447e3 204
jjeong 0:56ba69e447e3 205 if (pitch_cont > 0.33)
jjeong 0:56ba69e447e3 206 pitch_cont = 0.33;
jjeong 0:56ba69e447e3 207 else if (pitch_cont < -0.33)
jjeong 0:56ba69e447e3 208 pitch_cont = -0.33;
jjeong 0:56ba69e447e3 209
jjeong 0:56ba69e447e3 210
jjeong 0:56ba69e447e3 211 /*-------------------------------------------------------------------------*/
jjeong 0:56ba69e447e3 212
jjeong 0:56ba69e447e3 213 yaw_err = yaw_ref - gyro_angle[2];
jjeong 0:56ba69e447e3 214 yaw_rate_err = yaw_rate_ref - gyro_f[2];
jjeong 0:56ba69e447e3 215
jjeong 0:56ba69e447e3 216 if (yaw_err > YAW_ERR_MAX)
jjeong 0:56ba69e447e3 217 yaw_err = YAW_ERR_MAX;
jjeong 0:56ba69e447e3 218 else if (yaw_err < -(YAW_ERR_MAX))
jjeong 0:56ba69e447e3 219 yaw_err = -(YAW_ERR_MAX);
jjeong 0:56ba69e447e3 220
jjeong 0:56ba69e447e3 221 if (yaw_rate_err > YAW_RATE_ERR_MAX)
jjeong 0:56ba69e447e3 222 yaw_rate_err = YAW_RATE_ERR_MAX;
jjeong 0:56ba69e447e3 223 else if (yaw_rate_err < -(YAW_RATE_ERR_MAX))
jjeong 0:56ba69e447e3 224 yaw_rate_err = -(YAW_RATE_ERR_MAX);
jjeong 0:56ba69e447e3 225 if(yaw_ref == 0)
jjeong 0:56ba69e447e3 226 yaw_cont = K_scale * (Kp_yaw * yaw_err + Kd_yaw * yaw_rate_err);
jjeong 0:56ba69e447e3 227 else
jjeong 0:56ba69e447e3 228 {
jjeong 0:56ba69e447e3 229 yaw_cont = K_scale * (Kp_yaw * yaw_err);
jjeong 0:56ba69e447e3 230 yaw_ref = 0;
jjeong 0:56ba69e447e3 231 }
jjeong 0:56ba69e447e3 232
jjeong 0:56ba69e447e3 233 if (yaw_cont > 0.20)
jjeong 0:56ba69e447e3 234 yaw_cont = 0.20;
jjeong 0:56ba69e447e3 235 else if (yaw_cont < -0.20)
jjeong 0:56ba69e447e3 236 yaw_cont = -0.20;
jjeong 0:56ba69e447e3 237
jjeong 0:56ba69e447e3 238 thrust_c = thrust_in / (float)1654; //control_scale = 2000
jjeong 0:56ba69e447e3 239 if (thrust_c > 0.8)
jjeong 0:56ba69e447e3 240 thrust_c = 0.8;
jjeong 0:56ba69e447e3 241
jjeong 0:56ba69e447e3 242 }
jjeong 0:56ba69e447e3 243
jjeong 0:56ba69e447e3 244 //control tilted moving
jjeong 0:56ba69e447e3 245 void moving_cont()
jjeong 0:56ba69e447e3 246 {
jjeong 0:56ba69e447e3 247 checksum[0] = roll_ref + pitch_ref;
jjeong 0:56ba69e447e3 248 if(checksum[0] == checksum[1])
jjeong 0:56ba69e447e3 249 {
jjeong 0:56ba69e447e3 250 rollcompensate = accel_f[1]*(float)(0.5);
jjeong 0:56ba69e447e3 251 pitchcompensate = accel_f[0]*(float)(0.5);
jjeong 0:56ba69e447e3 252 rollcompensate = constrain_float(rollcompensate,-0.05,0.05);
jjeong 0:56ba69e447e3 253 pitchcompensate = constrain_float(pitchcompensate,-0.05,0.05);
jjeong 0:56ba69e447e3 254
jjeong 0:56ba69e447e3 255 }
jjeong 0:56ba69e447e3 256 else
jjeong 0:56ba69e447e3 257 {
jjeong 0:56ba69e447e3 258 rollcompensate = 0;
jjeong 0:56ba69e447e3 259 pitchcompensate = 0;
jjeong 0:56ba69e447e3 260
jjeong 0:56ba69e447e3 261 }
jjeong 0:56ba69e447e3 262 checksum[1] = checksum[0];
jjeong 0:56ba69e447e3 263 }
jjeong 0:56ba69e447e3 264
jjeong 0:56ba69e447e3 265
jjeong 0:56ba69e447e3 266 void pwm_drive(void)//roll+ = pitch-,
jjeong 0:56ba69e447e3 267 {
jjeong 0:56ba69e447e3 268 //pc.printf(" _c : %5.2f %5.2f %5.2f %5.2f \n\r", roll_c, pitch_c, yaw_c, thrust_c);
jjeong 0:56ba69e447e3 269 pwm1_pw = -(roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) -yaw_cont + thrust_c ; // 최댓값 1, 최솟값 0
jjeong 0:56ba69e447e3 270 pwm2_pw = (roll_cont + rollcompensate) - (pitch_cont + pitchcompensate) + yaw_cont + thrust_c ;
jjeong 0:56ba69e447e3 271 pwm3_pw = -(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) + yaw_cont + thrust_c ;
jjeong 0:56ba69e447e3 272 pwm4_pw = +(roll_cont + rollcompensate) + (pitch_cont + pitchcompensate) - yaw_cont + thrust_c ;
jjeong 0:56ba69e447e3 273
jjeong 0:56ba69e447e3 274 if(pwm1_pw >= 1)
jjeong 0:56ba69e447e3 275 pwm1_pw = 1;
jjeong 0:56ba69e447e3 276 else if(pwm1_pw < 0)
jjeong 0:56ba69e447e3 277 pwm1_pw = 0;
jjeong 0:56ba69e447e3 278 if(pwm2_pw >= 1)
jjeong 0:56ba69e447e3 279 pwm2_pw = 1;
jjeong 0:56ba69e447e3 280 else if(pwm2_pw < 0)
jjeong 0:56ba69e447e3 281 pwm2_pw = 0;
jjeong 0:56ba69e447e3 282 if(pwm3_pw >= 1)
jjeong 0:56ba69e447e3 283 pwm3_pw = 1;
jjeong 0:56ba69e447e3 284 else if(pwm3_pw < 0)
jjeong 0:56ba69e447e3 285 pwm3_pw = 0;
jjeong 0:56ba69e447e3 286 if(pwm4_pw >= 1)
jjeong 0:56ba69e447e3 287 pwm4_pw = 1;
jjeong 0:56ba69e447e3 288 else if(pwm4_pw < 0)
jjeong 0:56ba69e447e3 289 pwm4_pw = 0;
jjeong 0:56ba69e447e3 290
jjeong 0:56ba69e447e3 291 if (rf_fail_count>=50) // switch_on = 1;
jjeong 0:56ba69e447e3 292 {
jjeong 0:56ba69e447e3 293 pwm1_pw = 0.0;
jjeong 0:56ba69e447e3 294 pwm2_pw = 0.0;
jjeong 0:56ba69e447e3 295 pwm3_pw = 0.0;
jjeong 0:56ba69e447e3 296 pwm4_pw = 0.0;
jjeong 0:56ba69e447e3 297 }
jjeong 0:56ba69e447e3 298
jjeong 0:56ba69e447e3 299
jjeong 0:56ba69e447e3 300 PWM1 = pwm1_pw;
jjeong 0:56ba69e447e3 301 PWM2 = pwm2_pw;
jjeong 0:56ba69e447e3 302 PWM3 = pwm3_pw;
jjeong 0:56ba69e447e3 303 PWM4 = pwm4_pw;
jjeong 0:56ba69e447e3 304 }
jjeong 0:56ba69e447e3 305
jjeong 0:56ba69e447e3 306 void control_loop(void)
jjeong 0:56ba69e447e3 307 {
jjeong 0:56ba69e447e3 308
jjeong 0:56ba69e447e3 309 MPU9250_GET_GYRO(gyro);
jjeong 0:56ba69e447e3 310 gyro_f[0] = gyro[0] / 32.8 - gyro_bias[0];
jjeong 0:56ba69e447e3 311 gyro_f[1] = -(gyro[1] / 32.8 - gyro_bias[1]);
jjeong 0:56ba69e447e3 312 gyro_f[2] = -(gyro[2] / 32.8 - gyro_bias[2]);
jjeong 0:56ba69e447e3 313
jjeong 0:56ba69e447e3 314 MPU9250_GET_ACCEL(accel);
jjeong 0:56ba69e447e3 315 accel_f[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED) // 9.8m/s^2 ( G )
jjeong 0:56ba69e447e3 316 accel_f[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
jjeong 0:56ba69e447e3 317 accel_f[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);
jjeong 0:56ba69e447e3 318
jjeong 0:56ba69e447e3 319 /*
jjeong 0:56ba69e447e3 320 //accel low pass filter
jjeong 0:56ba69e447e3 321 accel_fu[0] = (accel[0] / 8192.0 - accel_bias[0]); //%Navigation frame reference (NED) // 9.8m/s^2 ( G )
jjeong 0:56ba69e447e3 322 accel_fu[1] = (accel[1] / 8192.0 - accel_bias[1]) * (-1); // 부호?
jjeong 0:56ba69e447e3 323 accel_fu[2] = (accel[2] / 8192.0 - accel_bias[2]) * (-1);
jjeong 0:56ba69e447e3 324
jjeong 0:56ba69e447e3 325 //accel low pass filter -----------
jjeong 0:56ba69e447e3 326 accel_f2[0]=(1-dt/accel_tau)*accel_f1[0]+dt/accel_tau*accel_fu[0];
jjeong 0:56ba69e447e3 327 accel_f2[1]=(1-dt/accel_tau)*accel_f1[1]+dt/accel_tau*accel_fu[1];
jjeong 0:56ba69e447e3 328 accel_f2[2]=(1-dt/accel_tau)*accel_f1[2]+dt/accel_tau*accel_fu[2];
jjeong 0:56ba69e447e3 329
jjeong 0:56ba69e447e3 330 accel_f1[0]=accel_f2[0];
jjeong 0:56ba69e447e3 331 accel_f1[1]=accel_f2[1];
jjeong 0:56ba69e447e3 332 accel_f1[2]=accel_f2[2];
jjeong 0:56ba69e447e3 333
jjeong 0:56ba69e447e3 334 accel_f[0]=accel_f2[0];
jjeong 0:56ba69e447e3 335 accel_f[1]=accel_f2[1];
jjeong 0:56ba69e447e3 336 accel_f[2]=accel_f2[2];
jjeong 0:56ba69e447e3 337 */
jjeong 0:56ba69e447e3 338 //----------------------------------
jjeong 0:56ba69e447e3 339
jjeong 0:56ba69e447e3 340 //get roll and pitch from Accelometer values
jjeong 0:56ba69e447e3 341 roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14;
jjeong 0:56ba69e447e3 342 //roll_accel = atan(accel_f[1] / accel_f[2]) * 180.0 / 3.14; //degree '
jjeong 0:56ba69e447e3 343 pitch_accel = asin(constrain_float(accel_f[0],(float)-1,(float)1)) * 180.0 / 3.14; // apply constrain to avoid NaN
jjeong 0:56ba69e447e3 344
jjeong 0:56ba69e447e3 345 //get Roll, Pitch, Yaw from Gyro
jjeong 0:56ba69e447e3 346 gyro_angle[0] = gyro_angle[0] + (gyro_f[0]) * dt; //Roll
jjeong 0:56ba69e447e3 347 gyro_angle[1] = gyro_angle[1] + (gyro_f[1]) * dt; //Pitch
jjeong 0:56ba69e447e3 348 gyro_angle[2] = gyro_angle[2] + (gyro_f[2]) * dt; //Yaw
jjeong 0:56ba69e447e3 349
jjeong 0:56ba69e447e3 350 // YAW 360도 넘길 때 빼는 방법
jjeong 0:56ba69e447e3 351
jjeong 0:56ba69e447e3 352 //get roll and pitch by applying Complementary Filter
jjeong 0:56ba69e447e3 353 roll_g_n = (1 - dt / tau) * roll_g_old + dt * gyro_f[0];
jjeong 0:56ba69e447e3 354 roll_a_n = (1 - dt / tau) * roll_a_old + dt / tau * roll_accel;
jjeong 0:56ba69e447e3 355
jjeong 0:56ba69e447e3 356 pitch_g_n = (1 - dt / tau) * pitch_g_old + dt * gyro_f[1];
jjeong 0:56ba69e447e3 357 pitch_a_n = (1 - dt / tau) * pitch_a_old + dt / tau * pitch_accel;
jjeong 0:56ba69e447e3 358
jjeong 0:56ba69e447e3 359 roll_g_old = roll_g_n;
jjeong 0:56ba69e447e3 360 roll_a_old = roll_a_n;
jjeong 0:56ba69e447e3 361
jjeong 0:56ba69e447e3 362 pitch_g_old = pitch_g_n;
jjeong 0:56ba69e447e3 363 pitch_a_old = pitch_a_n;
jjeong 0:56ba69e447e3 364 //pc.printf("\r%5.2f\t%5.2f",pitch_g_old, pitch_a_old);
jjeong 0:56ba69e447e3 365 roll_f = roll_g_old + roll_a_old;
jjeong 0:56ba69e447e3 366 pitch_f = pitch_g_old + pitch_a_old; //this value is weird
jjeong 0:56ba69e447e3 367
jjeong 0:56ba69e447e3 368 RF_READ();
jjeong 0:56ba69e447e3 369 /*
jjeong 0:56ba69e447e3 370 ROLL = ROLL - (int)offset[0];
jjeong 0:56ba69e447e3 371 PITCH = PITCH - (int)offset[1];
jjeong 0:56ba69e447e3 372 YAW = YAW - (int)offset[2];
jjeong 0:56ba69e447e3 373 THROTTLE = THROTTLE - (int)offset[3];
jjeong 0:56ba69e447e3 374 */
jjeong 0:56ba69e447e3 375
jjeong 0:56ba69e447e3 376 roll_ref = -(set_min((float)ROLL,6));
jjeong 0:56ba69e447e3 377 pitch_ref = set_min((float)PITCH,6);
jjeong 0:56ba69e447e3 378 yaw_ref = -(set_min((float)YAW,6));
jjeong 0:56ba69e447e3 379 thrust_in = set_min((float)THROTTLE,6);
jjeong 0:56ba69e447e3 380
jjeong 0:56ba69e447e3 381 yaw_reset();
jjeong 0:56ba69e447e3 382
jjeong 0:56ba69e447e3 383 control();
jjeong 0:56ba69e447e3 384
jjeong 0:56ba69e447e3 385 //moving_cont();
jjeong 0:56ba69e447e3 386
jjeong 0:56ba69e447e3 387 pwm_drive();
jjeong 0:56ba69e447e3 388
jjeong 0:56ba69e447e3 389
jjeong 0:56ba69e447e3 390 /*
jjeong 0:56ba69e447e3 391 if (l_count == 400 ) {
jjeong 0:56ba69e447e3 392 //pc.printf("l_count per 10")
jjeong 0:56ba69e447e3 393
jjeong 0:56ba69e447e3 394 //pc.printf("----------------------------------\n\r");
jjeong 0:56ba69e447e3 395 //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]); // 각속도
jjeong 0:56ba69e447e3 396 //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
jjeong 0:56ba69e447e3 397 //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
jjeong 0:56ba69e447e3 398 //pc.printf("\r Accel to angle :\t %5.2f\t %5.2f\n\r", roll_accel, pitch_accel);
jjeong 0:56ba69e447e3 399
jjeong 0:56ba69e447e3 400 //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 각도
jjeong 0:56ba69e447e3 401
jjeong 0:56ba69e447e3 402 // pc.printf("\r control input : %d, %d, %d, %f \n\r\n", (int)ROLL, (int)PITCH, (int)YAW, (float)THROTTLE); //RF_READ
jjeong 0:56ba69e447e3 403 //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); // 모터에 들어가는 값
jjeong 0:56ba69e447e3 404 //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);
jjeong 0:56ba69e447e3 405 //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
jjeong 0:56ba69e447e3 406 //pc.printf("\r RPYT_err :\t%5.3f\t%5.3f\t%5.3f \n\r", roll_err, pitch_err, yaw_err);
jjeong 0:56ba69e447e3 407 //pc.printf("\t\tRoll\tPitch\tYaw\tThrust\t\r\n");
jjeong 0:56ba69e447e3 408 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 각도 표 출력
jjeong 0:56ba69e447e3 409 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 표 출력
jjeong 0:56ba69e447e3 410
jjeong 0:56ba69e447e3 411
jjeong 0:56ba69e447e3 412 //pc.printf("\r _ref : %5.2f %5.2f %5.2f %5.2f \n\r", roll_ref, pitch_ref, yaw_ref, thrust_in); // 제어에 쓰이는 값
jjeong 0:56ba69e447e3 413
jjeong 0:56ba69e447e3 414 l_count = 1;
jjeong 0:56ba69e447e3 415 }
jjeong 0:56ba69e447e3 416
jjeong 0:56ba69e447e3 417 l_count = l_count + 1;
jjeong 0:56ba69e447e3 418 */
jjeong 0:56ba69e447e3 419
jjeong 0:56ba69e447e3 420 //wait(dt);
jjeong 0:56ba69e447e3 421
jjeong 0:56ba69e447e3 422 }
jjeong 0:56ba69e447e3 423 float set_min(float x, float min)
jjeong 0:56ba69e447e3 424 {
jjeong 0:56ba69e447e3 425 if ( x<min && x> -min)
jjeong 0:56ba69e447e3 426 x = 0;
jjeong 0:56ba69e447e3 427 return x;
jjeong 0:56ba69e447e3 428 }
jjeong 0:56ba69e447e3 429
jjeong 0:56ba69e447e3 430 int constrain_int16(int16_t x, int min, int max)
jjeong 0:56ba69e447e3 431 {
jjeong 0:56ba69e447e3 432 if (x > max)
jjeong 0:56ba69e447e3 433 x = max;
jjeong 0:56ba69e447e3 434 else if (x < min)
jjeong 0:56ba69e447e3 435 x = min;
jjeong 0:56ba69e447e3 436 return x;
jjeong 0:56ba69e447e3 437 }
jjeong 0:56ba69e447e3 438
jjeong 0:56ba69e447e3 439 float constrain_float(float x, float min, float max)
jjeong 0:56ba69e447e3 440 {
jjeong 0:56ba69e447e3 441 if (x > max)
jjeong 0:56ba69e447e3 442 x = max;
jjeong 0:56ba69e447e3 443 else if (x < min)
jjeong 0:56ba69e447e3 444 x = min;
jjeong 0:56ba69e447e3 445 return x;
jjeong 0:56ba69e447e3 446 }
jjeong 0:56ba69e447e3 447
jjeong 0:56ba69e447e3 448 void MPU9250_RESET()
jjeong 0:56ba69e447e3 449 {
jjeong 0:56ba69e447e3 450 // reset device
jjeong 0:56ba69e447e3 451 char cmd[2];
jjeong 0:56ba69e447e3 452 cmd[0] = PWR_MGMT_1; //status
jjeong 0:56ba69e447e3 453 cmd[1] = 0x80;
jjeong 0:56ba69e447e3 454 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 455 wait(0.1);
jjeong 0:56ba69e447e3 456 }
jjeong 0:56ba69e447e3 457
jjeong 0:56ba69e447e3 458 void MPU9250_INIT()
jjeong 0:56ba69e447e3 459 {
jjeong 0:56ba69e447e3 460 char cmd[3];
jjeong 0:56ba69e447e3 461 cmd[0] = PWR_MGMT_1; //reset
jjeong 0:56ba69e447e3 462 cmd[1] = 0x80;
jjeong 0:56ba69e447e3 463 i2c.write(MPU9250_ADDRESS, cmd, 2);
jjeong 0:56ba69e447e3 464 pc.printf("MPU 1 \n\r");
jjeong 0:56ba69e447e3 465 wait(0.1);
jjeong 0:56ba69e447e3 466
jjeong 0:56ba69e447e3 467 cmd[0] = PWR_MGMT_1; // Auto selects the best available clock source PLL if ready,
jjeong 0:56ba69e447e3 468 cmd[1] = 0x01; // else use the Internal oscillator
jjeong 0:56ba69e447e3 469 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 470 pc.printf("MPU 2 \n\r");
jjeong 0:56ba69e447e3 471 wait(0.1);
jjeong 0:56ba69e447e3 472
jjeong 0:56ba69e447e3 473 // Select Bandwidth of Gyroscopes BW 41Hz 1KHz internal sampling
jjeong 0:56ba69e447e3 474 cmd[0] = CONFIG;
jjeong 0:56ba69e447e3 475 cmd[1] = 0x03;
jjeong 0:56ba69e447e3 476 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 477 pc.printf("MPU 3 \n\r");
jjeong 0:56ba69e447e3 478 wait(0.1);
jjeong 0:56ba69e447e3 479
jjeong 0:56ba69e447e3 480 // sample rate = (internal sample rate) /(1+4) = 1000/5=200 Hz
jjeong 0:56ba69e447e3 481 cmd[0] = SMPLRT_DIV;
jjeong 0:56ba69e447e3 482 cmd[1] = 0x00;//0x04
jjeong 0:56ba69e447e3 483 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 484 pc.printf("MPU 4 \n\r");
jjeong 0:56ba69e447e3 485 wait(0.1);
jjeong 0:56ba69e447e3 486
jjeong 0:56ba69e447e3 487
jjeong 0:56ba69e447e3 488 cmd[0] = GYRO_CONFIG;
jjeong 0:56ba69e447e3 489 cmd[1] = 0b00010000;// Gyro full scale 1000 deg/sec; Gyro DLPF Enable
jjeong 0:56ba69e447e3 490 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 491 pc.printf("MPU 6 \n\r");
jjeong 0:56ba69e447e3 492 wait(0.1);
jjeong 0:56ba69e447e3 493
jjeong 0:56ba69e447e3 494 // Set accelerometer configuration
jjeong 0:56ba69e447e3 495 // Accel fulll sacle range +/- 4g
jjeong 0:56ba69e447e3 496 cmd[0] = ACCEL_CONFIG;
jjeong 0:56ba69e447e3 497 cmd[1] = 0b00001000;// Accel
jjeong 0:56ba69e447e3 498 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 499 pc.printf("MPU 8 \n\r");
jjeong 0:56ba69e447e3 500 wait(0.1);
jjeong 0:56ba69e447e3 501
jjeong 0:56ba69e447e3 502 cmd[0] = ACCEL_CONFIG2;
jjeong 0:56ba69e447e3 503 cmd[1] = 0b00000101; // 218 Hz bandwidth, 1kHz Sampling for accelerometer
jjeong 0:56ba69e447e3 504 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 505 pc.printf("MPU 10 \n\r");
jjeong 0:56ba69e447e3 506 wait(0.1);
jjeong 0:56ba69e447e3 507
jjeong 0:56ba69e447e3 508 //XYZ Gyro acel enable
jjeong 0:56ba69e447e3 509 cmd[0] = PWR_MGMT_2;
jjeong 0:56ba69e447e3 510 cmd[1] = 0x00;
jjeong 0:56ba69e447e3 511 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 512 pc.printf("MPU 11 \n\r");
jjeong 0:56ba69e447e3 513 wait(0.1);
jjeong 0:56ba69e447e3 514
jjeong 0:56ba69e447e3 515 cmd[0] = INT_PIN_CFG;
jjeong 0:56ba69e447e3 516 cmd[1] = 0x22; //0x02
jjeong 0:56ba69e447e3 517 i2c.write(MPU9250_ADDRESS, cmd, 2, 0);
jjeong 0:56ba69e447e3 518 pc.printf("MPU 12 \n\r");
jjeong 0:56ba69e447e3 519 wait(0.1);
jjeong 0:56ba69e447e3 520 }
jjeong 0:56ba69e447e3 521
jjeong 0:56ba69e447e3 522 void WHO_AM_I()
jjeong 0:56ba69e447e3 523 {
jjeong 0:56ba69e447e3 524 char cmd[2];
jjeong 0:56ba69e447e3 525 char rvd[2];
jjeong 0:56ba69e447e3 526
jjeong 0:56ba69e447e3 527 cmd[0] = WHO_AM_I_MPU9250;
jjeong 0:56ba69e447e3 528 i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
jjeong 0:56ba69e447e3 529 i2c.read(MPU9250_ADDRESS | 1, rvd, 1, 0);
jjeong 0:56ba69e447e3 530
jjeong 0:56ba69e447e3 531 pc.printf(" IMU device id is 0x%x \n\r", rvd[0]);
jjeong 0:56ba69e447e3 532 wait(1);
jjeong 0:56ba69e447e3 533 }
jjeong 0:56ba69e447e3 534
jjeong 0:56ba69e447e3 535 void MPU9250_GET_GYRO(int16_t * destination)
jjeong 0:56ba69e447e3 536 {
jjeong 0:56ba69e447e3 537 char cmd[6];
jjeong 0:56ba69e447e3 538 cmd[0] = GYRO_XOUT_H;
jjeong 0:56ba69e447e3 539 i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
jjeong 0:56ba69e447e3 540 i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
jjeong 0:56ba69e447e3 541 destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
jjeong 0:56ba69e447e3 542 destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
jjeong 0:56ba69e447e3 543 destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);
jjeong 0:56ba69e447e3 544
jjeong 0:56ba69e447e3 545 //pc.printf(" get_gyro : %d, %d, %d\n\r", destination[0], destination[1], destination[2]);
jjeong 0:56ba69e447e3 546 }
jjeong 0:56ba69e447e3 547
jjeong 0:56ba69e447e3 548 void MPU9250_GET_ACCEL(int16_t * destination)
jjeong 0:56ba69e447e3 549 {
jjeong 0:56ba69e447e3 550 char cmd[6];
jjeong 0:56ba69e447e3 551 cmd[0] = ACCEL_XOUT_H;
jjeong 0:56ba69e447e3 552 i2c.write(MPU9250_ADDRESS, cmd, 1, 1);
jjeong 0:56ba69e447e3 553 i2c.read(MPU9250_ADDRESS | 1, cmd, 6, 0);
jjeong 0:56ba69e447e3 554 destination[0] = (int16_t)(((int16_t)cmd[0] << 8) | cmd[1]);
jjeong 0:56ba69e447e3 555 destination[1] = (int16_t)(((int16_t)cmd[2] << 8) | cmd[3]);
jjeong 0:56ba69e447e3 556 destination[2] = (int16_t)(((int16_t)cmd[4] << 8) | cmd[5]);
jjeong 0:56ba69e447e3 557
jjeong 0:56ba69e447e3 558 //pc.printf(" get_accel : %d, %d, %d \n\r", destination[0], destination[1], destination[2]);
jjeong 0:56ba69e447e3 559 }
jjeong 0:56ba69e447e3 560
jjeong 0:56ba69e447e3 561 void gyro_bias_f(void)
jjeong 0:56ba69e447e3 562 {
jjeong 0:56ba69e447e3 563 int16_t gyro1[3];
jjeong 0:56ba69e447e3 564 //int16_t gyro_bias[3];
jjeong 0:56ba69e447e3 565
jjeong 0:56ba69e447e3 566 pc.printf(" Please keep still 5 seconds\n\r");
jjeong 0:56ba69e447e3 567 for (int i = 0; i < 200; i++)
jjeong 0:56ba69e447e3 568 {
jjeong 0:56ba69e447e3 569 MPU9250_GET_GYRO(gyro1); //정지상태 각속도를 gyro1에 측정, 32.8로 스케일링, 100번 측정한 값을 모두 더함
jjeong 0:56ba69e447e3 570 gyro_bias[0] = gyro_bias[0] + gyro1[0] / 32.8;
jjeong 0:56ba69e447e3 571 gyro_bias[1] = gyro_bias[1] + gyro1[1] / 32.8;
jjeong 0:56ba69e447e3 572 gyro_bias[2] = gyro_bias[2] + gyro1[2] / 32.8;
jjeong 0:56ba69e447e3 573 //pc.printf(" gyro_bias finding i= %d\n\r", i);
jjeong 0:56ba69e447e3 574
jjeong 0:56ba69e447e3 575 }
jjeong 0:56ba69e447e3 576 gyro_bias[0] = gyro_bias[0] / 200.0; //500으로 나누어 평균 도출
jjeong 0:56ba69e447e3 577 gyro_bias[1] = gyro_bias[1] / 200.0;
jjeong 0:56ba69e447e3 578 gyro_bias[2] = gyro_bias[2] / 200.0;
jjeong 0:56ba69e447e3 579 //pc.printf(" Gyro_Bias finding completed %\n\r");
jjeong 0:56ba69e447e3 580 }
jjeong 0:56ba69e447e3 581
jjeong 0:56ba69e447e3 582 void accel_bias_f(void)
jjeong 0:56ba69e447e3 583 {
jjeong 0:56ba69e447e3 584 int16_t accel1[3];
jjeong 0:56ba69e447e3 585 //int16_t accel_bias[3];
jjeong 0:56ba69e447e3 586
jjeong 0:56ba69e447e3 587 pc.printf(" Please keep still 5 seconds\n\r");
jjeong 0:56ba69e447e3 588 for (int i = 0; i < 100; i++)
jjeong 0:56ba69e447e3 589 {
jjeong 0:56ba69e447e3 590 MPU9250_GET_ACCEL(accel1); //정지상태 가속도를 accel1에 측정, 8192.0로 스케일링, 100번 측정한 값을 모두 더함
jjeong 0:56ba69e447e3 591 accel_bias[0] = accel_bias[0] + accel1[0] / 8192.0;
jjeong 0:56ba69e447e3 592 accel_bias[1] = accel_bias[1] + accel1[1] / 8192.0;
jjeong 0:56ba69e447e3 593 accel_bias[2] = accel_bias[2] + accel1[2] / 8192.0;
jjeong 0:56ba69e447e3 594 pc.printf(" accel_bias finding i= %d\n\r", i);
jjeong 0:56ba69e447e3 595 }
jjeong 0:56ba69e447e3 596 accel_bias[0] = accel_bias[0] / 100.0; //100으로 나누어 평균 도출
jjeong 0:56ba69e447e3 597 accel_bias[1] = accel_bias[1] / 100.0;
jjeong 0:56ba69e447e3 598 accel_bias[2] = accel_bias[2] / 100.0 - 1; //중력값 보정
jjeong 0:56ba69e447e3 599 pc.printf(" Accel_Bias finding completed %\n\r");
jjeong 0:56ba69e447e3 600 }
jjeong 0:56ba69e447e3 601
jjeong 0:56ba69e447e3 602
jjeong 0:56ba69e447e3 603
jjeong 0:56ba69e447e3 604 int main()
jjeong 0:56ba69e447e3 605 {
jjeong 0:56ba69e447e3 606 PWM1.period(0.0001);// 10 kHz PWM for PWM1~PWM4
jjeong 0:56ba69e447e3 607
jjeong 0:56ba69e447e3 608 wait(3);
jjeong 0:56ba69e447e3 609 pc.printf("------------------------\n\r");
jjeong 0:56ba69e447e3 610 NRF24L01.begin();
jjeong 0:56ba69e447e3 611 NRF24L01.setDataRate(RF24_2MBPS); //RF24_2MBPS
jjeong 0:56ba69e447e3 612 NRF24L01.setChannel(90); // set channel 10 20 30
jjeong 0:56ba69e447e3 613 NRF24L01.setPayloadSize(28);
jjeong 0:56ba69e447e3 614 NRF24L01.setAddressWidth(5);
jjeong 0:56ba69e447e3 615 NRF24L01.setRetries(2, 4); //1,3 2,8 1,8
jjeong 0:56ba69e447e3 616 NRF24L01.enableAckPayload();
jjeong 0:56ba69e447e3 617 NRF24L01.openReadingPipe(0, pipe);
jjeong 0:56ba69e447e3 618 NRF24L01.startListening();
jjeong 0:56ba69e447e3 619
jjeong 0:56ba69e447e3 620 MPU9250_RESET();
jjeong 0:56ba69e447e3 621 MPU9250_INIT();
jjeong 0:56ba69e447e3 622 pc.baud(38400);
jjeong 0:56ba69e447e3 623 //pc.printf("%d th Who am I?\n\r", count);
jjeong 0:56ba69e447e3 624 WHO_AM_I();
jjeong 0:56ba69e447e3 625
jjeong 0:56ba69e447e3 626
jjeong 0:56ba69e447e3 627
jjeong 0:56ba69e447e3 628 //pc.printf(" count and gyro outputs(x,y,z)\n\r");
jjeong 0:56ba69e447e3 629 //pc.printf(" count and accel outputs(x,y,z)\n\r");
jjeong 0:56ba69e447e3 630
jjeong 0:56ba69e447e3 631 gyro_bias_f();
jjeong 0:56ba69e447e3 632 accel_bias_f();
jjeong 0:56ba69e447e3 633 pc.printf(" \ngyro biases(deg/sec) %5.2f %5.2f %5.2f \n\r", gyro_bias[0], gyro_bias[1], gyro_bias[2]); // 초기화 됐는지 확인
jjeong 0:56ba69e447e3 634 pc.printf(" accel biases(G) %5.2f %5.2f %5.2f \n\n\r", accel_bias[0], accel_bias[1], accel_bias[2]); // 초기화 됐는지 확인
jjeong 0:56ba69e447e3 635 wait(1);
jjeong 0:56ba69e447e3 636
jjeong 0:56ba69e447e3 637
jjeong 0:56ba69e447e3 638 loops.attach_us(&control_loop, 2500); // ticker 2500
jjeong 0:56ba69e447e3 639 }