From 2dffcf04f0a8763513066663c32210c04f7ade35 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Fri, 22 Dec 2023 15:24:39 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E9=99=80=E8=9E=BA=E4=BB=AA=E7=9A=84?= =?UTF-8?q?=E5=88=9D=E7=BA=A7=E6=95=B0=E6=8D=AE=E8=8E=B7=E5=8F=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/by_imu.c | 26 ++++++++++++++ app/by_imu.h | 15 ++++++++ app/by_pt_button.c | 35 +++++++++++++++--- app/by_pt_button.h | 4 ++- app/isr.c | 4 ++- app/main.c | 50 +++++++++++++------------- libraries/zf_device/zf_device_ips200.h | 8 ++--- 7 files changed, 106 insertions(+), 36 deletions(-) create mode 100644 app/by_imu.c create mode 100644 app/by_imu.h diff --git a/app/by_imu.c b/app/by_imu.c new file mode 100644 index 0000000..11f945a --- /dev/null +++ b/app/by_imu.c @@ -0,0 +1,26 @@ +#include "by_imu.h" +#include "zf_common_headfile.h" + +float imu_acc_x; +float imu_acc_y; +float imu_acc_z; +float imu_gyro_x; +float imu_gyro_y; +float imu_gyro_z; + +void by_imu_data_get(void) +{ + imu660ra_get_acc(); + imu660ra_get_gyro(); + imu660ra_get_temperature(); + + imu_acc_x = imu660ra_acc_transition(imu660ra_acc_x); + imu_acc_y = imu660ra_acc_transition(imu660ra_acc_y); + imu_acc_z = imu660ra_acc_transition(imu660ra_acc_z); + + imu_gyro_x = imu660ra_gyro_transition(imu660ra_gyro_x); + imu_gyro_y = imu660ra_gyro_transition(imu660ra_gyro_y); + imu_gyro_z = imu660ra_gyro_transition(imu660ra_gyro_z); + + +} diff --git a/app/by_imu.h b/app/by_imu.h new file mode 100644 index 0000000..0e81a20 --- /dev/null +++ b/app/by_imu.h @@ -0,0 +1,15 @@ +#ifndef _BY_IMU_H__ +#define _BY_IMU_H__ + +#include "stdio.h" +#include "ch32v30x.h" + +extern float imu_acc_x; +extern float imu_acc_y; +extern float imu_acc_z; +extern float imu_gyro_x; +extern float imu_gyro_y; +extern float imu_gyro_z; + +extern void by_imu_data_get(void); +#endif \ No newline at end of file diff --git a/app/by_pt_button.c b/app/by_pt_button.c index fc0a5ce..97f5ab1 100644 --- a/app/by_pt_button.c +++ b/app/by_pt_button.c @@ -1,6 +1,8 @@ #include "by_pt_button.h" #include "zf_common_headfile.h" +#include "by_imu.h" uint8_t potate_button; + void by_gpio_init(void) { gpio_init(E10, GPI, GPIO_HIGH, GPI_PULL_UP); @@ -21,7 +23,32 @@ uint8_t by_get_pb_statu(void) void by_ips_show(void) { - ips114_show_string(0, 0, "button statu:"); - ips114_show_uint(104, 0, by_get_pb_statu(), 1); - -} \ No newline at end of file + ips200_show_string(0, 0, "button statu:"); + // ips200_show_uint(104, 0, by_get_pb_statu(), 1); + switch (by_get_pb_statu()) { + case 1: + ips200_show_string(104, 0, "press"); + break; + case 2: + ips200_show_string(104, 0, "up "); + break; + case 3: + ips200_show_string(104, 0, "down "); + break; + + default: + ips200_show_string(104, 0, " "); + break; + } + // 鎸夐挳 + ips200_show_string(0, 16, "imu:"); + ips200_show_float(46, 16, imu_acc_x, 2, 2); + ips200_show_float(106, 16, imu_acc_y, 2, 2); + ips200_show_float(166, 16, imu_acc_z, 2, 2); + ips200_show_float(46, 32, imu_gyro_x, 2, 2); + ips200_show_float(106, 32, imu_gyro_y, 2, 2); + ips200_show_float(166, 32, imu_gyro_z, 2, 2); + ips200_show_float(46, 48, imu660ra_temperature, 2, 2); + printf("%d,%d,%d\n",(int16_t)(imu_gyro_x*10),(int16_t)(imu_gyro_y*10),(int16_t)(imu_gyro_z*10)); + +} diff --git a/app/by_pt_button.h b/app/by_pt_button.h index 4e0ded1..0a7202f 100644 --- a/app/by_pt_button.h +++ b/app/by_pt_button.h @@ -8,8 +8,10 @@ #define POTATE_BUTTOM_BACKWARD 3 extern uint8_t potate_button; + extern void by_exit_init(void); extern void by_gpio_init(void); -extern uint8_t by_get_pb_statu(void); +extern uint8_t by_get_statu(void); extern void by_ips_show(void); + #endif \ No newline at end of file diff --git a/app/isr.c b/app/isr.c index 33f643e..e502d42 100644 --- a/app/isr.c +++ b/app/isr.c @@ -198,10 +198,11 @@ void EXTI9_5_IRQHandler(void) EXTI_ClearITPendingBit(EXTI_Line9); if (SET == gpio_get_level(E10)) { potate_button = POTATE_BUTTOM_BACKWARD; + } else { potate_button = POTATE_BUTTOM_FOREWARD; - + } } } @@ -222,6 +223,7 @@ void EXTI15_10_IRQHandler(void) system_delay_us(200); if (SET == !gpio_get_level(E11)) { potate_button = POTATE_BUTTOM_PRESS; + } } if (SET == EXTI_GetITStatus(EXTI_Line12)) { diff --git a/app/main.c b/app/main.c index e9e33be..58bca7f 100644 --- a/app/main.c +++ b/app/main.c @@ -37,9 +37,10 @@ #include "cw_servo.h" #include "by_pt_button.h" #include "by_fan_control.h" +#include "by_imu.h" uint8 (*Img_Gray)[MT9V03X_W]; // 定义指向包含 MT9V03X_W 列的 uint8 类型的二维数组的指针 -//uint8 *mt9v03x_image_copy[0]; // 定义指向 uint8 类型的一维数组的指针 +// uint8 *mt9v03x_image_copy[0]; // 定义指向 uint8 类型的一维数组的指针 sint32 pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; sint32 pts_left_count, pts_right_count; float32 pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2]; @@ -51,8 +52,7 @@ sint32 pts_resample_left_count, pts_resample_right_count; float32 mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2]; sint32 mid_left_count, mid_right_count; - -//左右边线局部角度变化率+非极大值抑制 +// 左右边线局部角度变化率+非极大值抑制 float angle_new_left[PT_MAXLEN]; float angle_new_right[PT_MAXLEN]; int angle_new_left_num, angle_new_right_num; @@ -66,7 +66,7 @@ int angle_left_num, angle_right_num; // L角点 int Lpt0_rpts0s_id, Lpt1_rpts1s_id; bool Lpt0_found, Lpt1_found; -int Lpt1[2],Lpt0[2]; +int Lpt1[2], Lpt0[2]; int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id; bool Lpt_in0_found, Lpt_in1_found; @@ -85,44 +85,43 @@ float aim_distance; enum track_type_e track_type = TRACK_RIGHT; - int frame_count = 0; void img_processing(); void get_corners(); - int main(void) { clock_init(SYSTEM_CLOCK_120M); // 初始化芯片时钟 工作频率为 120MHz debug_init(); // 务必保留，本函数用于初始化MPU 时钟 调试串口 - mt9v03x_init(); - ips114_init(); + // mt9v03x_init(); + ips200_init(IPS200_TYPE_SPI); by_gpio_init(); by_exit_init(); by_pwm_init(); cw_servo_init(); + while (imu660ra_init()) + ; while (1) { - //while (frame_count < 20) { - // if (mt9v03x_finish_flag) { - // memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); - // adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8); - // //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110); - // ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0); - // mt9v03x_finish_flag = 0; - // frame_count++; - // } + // while (frame_count < 20) { + // if (mt9v03x_finish_flag) { + // memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); + // adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8); + // //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110); + // ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0); + // mt9v03x_finish_flag = 0; + // frame_count++; + // } //} if (mt9v03x_finish_flag) { - //ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0); - memcpy(mt9v03x_image_copy[0], mt9v03x_image[0],(sizeof(mt9v03x_image_copy)/sizeof(uint8_t))); - //Img_Gray = mt9v03x_image; - //mt9v03x_image_copy[0] = Img_Gray[0]; + // ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0); + memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); + // Img_Gray = mt9v03x_image; + // mt9v03x_image_copy[0] = Img_Gray[0]; mt9v03x_finish_flag = 0; - state_type = COMMON_STATE; img_processing(); @@ -137,10 +136,9 @@ int main(void) ElementRun(); MidLineTrack(); - - } - - + by_imu_data_get(); + by_ips_show(); + system_delay_ms(200); } } \ No newline at end of file diff --git a/libraries/zf_device/zf_device_ips200.h b/libraries/zf_device/zf_device_ips200.h index 8a4c04d..090b71a 100644 --- a/libraries/zf_device/zf_device_ips200.h +++ b/libraries/zf_device/zf_device_ips200.h @@ -85,10 +85,10 @@ #endif // 濡傛灉浣跨敤鐨勬槸鍗曟帓鎺掗拡鐨勪袱瀵稿睆骞 SPI 椹卞姩鎺у埗寮曡剼 鍙互淇敼 -#define IPS200_RST_PIN_SPI (B7 ) // 娑叉櫠澶嶄綅寮曡剼瀹氫箟 -#define IPS200_DC_PIN_SPI (D7 ) // 娑叉櫠鍛戒护浣嶅紩鑴氬畾涔 -#define IPS200_CS_PIN_SPI (D4 ) -#define IPS200_BLk_PIN_SPI (D0 ) +#define IPS200_RST_PIN_SPI (D8 ) // 娑叉櫠澶嶄綅寮曡剼瀹氫箟 +#define IPS200_DC_PIN_SPI (D9 ) // 娑叉櫠鍛戒护浣嶅紩鑴氬畾涔 +#define IPS200_CS_PIN_SPI (D10 ) +#define IPS200_BLk_PIN_SPI (D11 ) // --------------------鍗曟帓涓ゅ灞忓箷SPI鎺ュ彛寮曡剼瀹氫箟--------------------// From 20666b0c756e774c4c2a03e671429d40386bd027 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Fri, 22 Dec 2023 22:39:59 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E9=99=80=E8=9E=BA=E4=BB=AA=E6=AC=A7?= =?UTF-8?q?=E6=8B=89=E8=A7=92=E7=9A=84=E8=AE=A1=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/by_imu.c | 173 +++++++++++++++++++++++++++++++++++++++++---- app/by_imu.h | 45 ++++++++++-- app/by_pt_button.c | 17 +++-- app/isr.c | 9 ++- app/main.c | 4 +- 5 files changed, 212 insertions(+), 36 deletions(-) diff --git a/app/by_imu.c b/app/by_imu.c index 11f945a..f64bd09 100644 --- a/app/by_imu.c +++ b/app/by_imu.c @@ -1,26 +1,171 @@ #include "by_imu.h" #include "zf_common_headfile.h" +#include -float imu_acc_x; -float imu_acc_y; -float imu_acc_z; -float imu_gyro_x; -float imu_gyro_y; -float imu_gyro_z; +#define delta_T 0.001f // 1ms璁＄畻涓娆 -void by_imu_data_get(void) +float I_ex, I_ey, I_ez; // 璇樊绉垎 + +quater_param_t Q_info = {1, 0, 0}; // 鍏ㄥ眬鍥涘厓鏁 +euler_param_t eulerAngle; // 娆ф媺瑙 + +icm_param_t icm_data; +gyro_param_t GyroOffset; + +bool GyroOffset_init = 0; + +float param_Kp = 0.17; // 鍔犻熷害璁＄殑鏀舵暃閫熺巼姣斾緥澧炵泭 0.17 +float param_Ki = 0.008; // 闄铻轰华鏀舵暃閫熺巼鐨勭Н鍒嗗鐩 0.004 + +float fast_sqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} + +void gyroOffset_init(void) /////////闄铻轰华闆堕鍒濆鍖 +{ + GyroOffset.Xdata = 0; + GyroOffset.Ydata = 0; + GyroOffset.Zdata = 0; + for (uint16_t i = 0; i < 500; ++i) { + imu660ra_get_acc(); + imu660ra_get_gyro(); + GyroOffset.Xdata += imu660ra_gyro_x; + GyroOffset.Ydata += imu660ra_gyro_y; + GyroOffset.Zdata += imu660ra_gyro_z; + system_delay_ms(1); + } + + GyroOffset.Xdata /= 500.0f; + GyroOffset.Ydata /= 500.0f; + GyroOffset.Zdata /= 500.0f; + + GyroOffset_init = 1; +} +// 杞寲涓哄疄闄呯墿鐞嗗 +void ICM_getValues() +{ +#define alpha 0.3f + icm_data.acc_x = imu660ra_acc_transition(imu660ra_acc_x) + icm_data.acc_x * (1 - alpha); + icm_data.acc_y = imu660ra_acc_transition(imu660ra_acc_y) + icm_data.acc_y * (1 - alpha); + icm_data.acc_z = imu660ra_acc_transition(imu660ra_acc_z) + icm_data.acc_z * (1 - alpha); + + icm_data.gyro_x = (imu660ra_gyro_transition(imu660ra_gyro_x)-GyroOffset.Xdata) / 57.3f; + icm_data.gyro_y = (imu660ra_gyro_transition(imu660ra_gyro_y)-GyroOffset.Ydata) / 57.3f; + icm_data.gyro_z = (imu660ra_gyro_transition(imu660ra_gyro_z)-GyroOffset.Zdata) / 57.3f; +} + +// 浜掕ˉ婊ゆ尝 +void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) +{ + float halfT = 0.5 * delta_T; + float vx, vy, vz; // 褰撳墠鐨勬満浣撳潗鏍囩郴涓婄殑閲嶅姏鍗曚綅鍚戦噺 + float ex, ey, ez; // 鍥涘厓鏁拌绠楀间笌鍔犻熷害璁℃祴閲忓肩殑璇樊 + float q0 = Q_info.q0; + float q1 = Q_info.q1; + float q2 = Q_info.q2; + float q3 = Q_info.q3; + float q0q0 = q0 * q0; + float q0q1 = q0 * q1; + float q0q2 = q0 * q2; + float q0q3 = q0 * q3; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q3q3 = q3 * q3; + // float delta_2 = 0; + + // 瀵瑰姞閫熷害鏁版嵁杩涜褰掍竴鍖 寰楀埌鍗曚綅鍔犻熷害 + float norm = fast_sqrt(ax * ax + ay * ay + az * az); + ax = ax * norm; + ay = ay * norm; + az = az * norm; + + // 鏍规嵁褰撳墠鍥涘厓鏁扮殑濮挎佸兼潵浼扮畻鍑哄悇閲嶅姏鍒嗛噺銆傜敤浜庡拰鍔犻熻瀹為檯娴嬮噺鍑烘潵鐨勫悇閲嶅姏鍒嗛噺杩涜瀵规瘮锛屼粠鑰屽疄鐜板鍥涜酱濮挎佺殑淇 + vx = 2.0f * (q1q3 - q0q2); + vy = 2.0f * (q0q1 + q2q3); + // vz = q0q0 - q1q1 - q2q2 + q3q3; + vz=1.0f-2.0f*q1q1-2.0f*q2q2; + // vz = (q0*q0-0.5f+q3 * q3) * 2; + + // 鍙夌Н鏉ヨ绠椾及绠楃殑閲嶅姏鍜屽疄闄呮祴閲忕殑閲嶅姏杩欎袱涓噸鍔涘悜閲忎箣闂寸殑璇樊銆 + ex = ay * vz - az * vy; + ey = az * vx - ax * vz; + ez = ax * vy - ay * vx; + + // 鐢ㄥ弶涔樿宸潵鍋歅I淇闄铻洪浂鍋忥紝 + // 閫氳繃璋冭妭 param_Kp锛宲aram_Ki 涓や釜鍙傛暟锛 + // 鍙互鎺у埗鍔犻熷害璁′慨姝ｉ檧铻轰华绉垎濮挎佺殑閫熷害銆 + I_ex += halfT * ex; // integral error scaled by Ki + I_ey += halfT * ey; + I_ez += halfT * ez; + + gx = gx + param_Kp * ex + param_Ki * I_ex; + gy = gy + param_Kp * ey + param_Ki * I_ey; + gz = gz + param_Kp * ez + param_Ki * I_ez; + + /*鏁版嵁淇瀹屾垚锛屼笅闈㈡槸鍥涘厓鏁板井鍒嗘柟绋*/ + + // 鍥涘厓鏁板井鍒嗘柟绋嬶紝鍏朵腑halfT涓烘祴閲忓懆鏈熺殑1/2锛実x gy gz涓洪檧铻轰华瑙掗熷害锛屼互涓嬮兘鏄凡鐭ラ噺锛岃繖閲屼娇鐢ㄤ簡涓闃堕緳鍝ュ簱濉旀眰瑙ｅ洓鍏冩暟寰垎鏂圭▼ + q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT; + q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT; + q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT; + q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT; + // delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz); + // 鏁村悎鍥涘厓鏁扮巼 鍥涘厓鏁板井鍒嗘柟绋 鍥涘厓鏁版洿鏂扮畻娉曪紝浜岄樁姣曞崱娉 + // q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT; + // q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT; + // q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT; + // q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT + + // normalise quaternion + norm = fast_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + Q_info.q0 = q0 * norm; + Q_info.q1 = q1 * norm; + Q_info.q2 = q2 * norm; + Q_info.q3 = q3 * norm; +} +void ICM_getEulerianAngles(void) +{ + + // 閲囬泦闄铻轰华鏁版嵁 imu660ra_get_acc(); imu660ra_get_gyro(); imu660ra_get_temperature(); - imu_acc_x = imu660ra_acc_transition(imu660ra_acc_x); - imu_acc_y = imu660ra_acc_transition(imu660ra_acc_y); - imu_acc_z = imu660ra_acc_transition(imu660ra_acc_z); + ICM_getValues(); + ICM_AHRSupdate(icm_data.gyro_x, icm_data.gyro_y, icm_data.gyro_z, icm_data.acc_x, icm_data.acc_y, icm_data.acc_z); + float q0 = Q_info.q0; + float q1 = Q_info.q1; + float q2 = Q_info.q2; + float q3 = Q_info.q3; - imu_gyro_x = imu660ra_gyro_transition(imu660ra_gyro_x); - imu_gyro_y = imu660ra_gyro_transition(imu660ra_gyro_y); - imu_gyro_z = imu660ra_gyro_transition(imu660ra_gyro_z); + // 鍥涘厓鏁拌绠楁鎷夎 + eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch + eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll + eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw - + /* 濮挎侀檺鍒*/ + if (eulerAngle.roll > 90 || eulerAngle.roll < -90) { + if (eulerAngle.pitch > 0) { + eulerAngle.pitch = 180 - eulerAngle.pitch; + } + if (eulerAngle.pitch < 0) { + eulerAngle.pitch = -(180 + eulerAngle.pitch); + } + } + + if (eulerAngle.yaw > 360) { + eulerAngle.yaw -= 360; + } else if (eulerAngle.yaw < 0) { + eulerAngle.yaw += 360; + } } diff --git a/app/by_imu.h b/app/by_imu.h index 0e81a20..7977aa2 100644 --- a/app/by_imu.h +++ b/app/by_imu.h @@ -3,13 +3,44 @@ #include "stdio.h" #include "ch32v30x.h" +typedef struct { + float gyro_x; + float gyro_y; + float gyro_z; + float acc_x; + float acc_y; + float acc_z; +} icm_param_t; -extern float imu_acc_x; -extern float imu_acc_y; -extern float imu_acc_z; -extern float imu_gyro_x; -extern float imu_gyro_y; -extern float imu_gyro_z; +typedef struct { + float q0; + float q1; + float q2; + float q3; +} quater_param_t; -extern void by_imu_data_get(void); +typedef struct { + float pitch; //淇话瑙 + float roll; //鍋忚埅瑙 + float yaw; //缈绘粴瑙 +} euler_param_t; + +typedef struct { + float Xdata; + float Ydata; + float Zdata; +} gyro_param_t; + +extern icm_param_t icm_data; +extern euler_param_t eulerAngle; + +void gyroOffset_init(void); + +float fast_sqrt(float x); + +void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); + +void ICM_getValues(); + +void ICM_getEulerianAngles(void); #endif \ No newline at end of file diff --git a/app/by_pt_button.c b/app/by_pt_button.c index 97f5ab1..eea70a9 100644 --- a/app/by_pt_button.c +++ b/app/by_pt_button.c @@ -42,13 +42,12 @@ void by_ips_show(void) } // 鎸夐挳 ips200_show_string(0, 16, "imu:"); - ips200_show_float(46, 16, imu_acc_x, 2, 2); - ips200_show_float(106, 16, imu_acc_y, 2, 2); - ips200_show_float(166, 16, imu_acc_z, 2, 2); - ips200_show_float(46, 32, imu_gyro_x, 2, 2); - ips200_show_float(106, 32, imu_gyro_y, 2, 2); - ips200_show_float(166, 32, imu_gyro_z, 2, 2); - ips200_show_float(46, 48, imu660ra_temperature, 2, 2); - printf("%d,%d,%d\n",(int16_t)(imu_gyro_x*10),(int16_t)(imu_gyro_y*10),(int16_t)(imu_gyro_z*10)); - + ips200_show_float(46, 32, eulerAngle.pitch, 3, 2); + ips200_show_float(46, 48, eulerAngle.roll, 3, 2); + ips200_show_float(46, 64, eulerAngle.yaw, 3, 2); + // ips200_show_float(46 , 32, icm_data.gyro_x, 2, 2); + // ips200_show_float(106, 32, icm_data.gyro_y, 2, 2); + // ips200_show_float(166, 32, icm_data.gyro_z, 2, 2); + ips200_show_float(46, 80, imu660ra_temperature, 2, 2); + // printf("%d,%d,%d\n", (int16_t)(icm_data.gyro_x * 10), (int16_t)(icm_data.gyro_y * 10), (int16_t)(icm_data.gyro_z * 10)); } diff --git a/app/isr.c b/app/isr.c index e502d42..ce1e22e 100644 --- a/app/isr.c +++ b/app/isr.c @@ -195,15 +195,14 @@ void EXTI9_5_IRQHandler(void) EXTI_ClearITPendingBit(EXTI_Line8); } if (SET == EXTI_GetITStatus(EXTI_Line9)) { - EXTI_ClearITPendingBit(EXTI_Line9); + if (SET == gpio_get_level(E10)) { potate_button = POTATE_BUTTOM_BACKWARD; - } else { potate_button = POTATE_BUTTOM_FOREWARD; - } + EXTI_ClearITPendingBit(EXTI_Line9); } } @@ -218,13 +217,12 @@ void EXTI15_10_IRQHandler(void) EXTI_ClearITPendingBit(EXTI_Line10); } if (SET == EXTI_GetITStatus(EXTI_Line11)) { - EXTI_ClearITPendingBit(EXTI_Line11); system_delay_us(200); if (SET == !gpio_get_level(E11)) { potate_button = POTATE_BUTTOM_PRESS; - } + EXTI_ClearITPendingBit(EXTI_Line11); } if (SET == EXTI_GetITStatus(EXTI_Line12)) { EXTI_ClearITPendingBit(EXTI_Line12); @@ -291,6 +289,7 @@ void TIM5_IRQHandler(void) void TIM6_IRQHandler(void) { if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) { + ICM_getEulerianAngles(); TIM_ClearITPendingBit(TIM6, TIM_IT_Update); } } diff --git a/app/main.c b/app/main.c index 58bca7f..fd646a1 100644 --- a/app/main.c +++ b/app/main.c @@ -102,6 +102,8 @@ int main(void) cw_servo_init(); while (imu660ra_init()) ; + gyroOffset_init(); + pit_ms_init(TIM6_PIT, 1); while (1) { // while (frame_count < 20) { @@ -137,7 +139,7 @@ int main(void) MidLineTrack(); } - by_imu_data_get(); + // by_imu_data_get(); by_ips_show(); system_delay_ms(200); }