Seunghee Jeong
/
Dronequadrotor
.
main.cpp@0:56ba69e447e3, 2022-07-18 (annotated)
- Committer:
- jjeong
- Date:
- Mon Jul 18 06:37:14 2022 +0000
- Revision:
- 0:56ba69e447e3
.
Who changed what in which revision?
User | Revision | Line number | New 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 | } |