btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pref: 增加编码器输入滤波

Browse Source
This commit is contained in:
bmy
2024-03-05 19:38:31 +08:00
parent 2805caa10b
commit 6c0cfe1248
1 changed files with 21 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
app/jj_motion.c
View File

@@ -40,6 +40,22 @@ float set_speed = 0.0f;
int cnt1 = 0; int cnt1 = 0;
float sport_get_speed(void)
{
#define ALPHA (0.97f)
static float speed_now = 0;
static float speed_last = 0;
speed_now = ALPHA * (float)encoder_get_count(TIM5_ENCOEDER) + (1.0f - ALPHA) * speed_last;
speed_last = speed_now;
encoder_clear_count(TIM5_ENCOEDER);
return speed_now;
#undef ALPHA
}
void sport_motion(void) void sport_motion(void)
{ {
@@ -54,14 +70,14 @@ void sport_motion(void)
if (cnt1 >= 10) { if (cnt1 >= 10) {
PID_Compute(&far_angle_pid); PID_Compute(&far_angle_pid);
PID_Compute(&speed_pid); PID_Compute(&speed_pid);
in_speed = -encoder_get_count(TIM5_ENCOEDER); // 速度输入，m/s // in_speed = -encoder_get_count(TIM5_ENCOEDER); // 速度输入，m/s
// printf("rate:%d\r\n", (int16)in_speed); in_speed = sport_get_speed();
encoder_clear_count(TIM5_ENCOEDER); printf("rate:%d\r\n", (int16_t)in_speed);
PID_Compute(&near_pos_pid); PID_Compute(&near_pos_pid);
cnt1 = 0; cnt1 = 0;
} }
if (bt_run_flag == 1) { if (bt_run_flag == 1) {
by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro), by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro),
(int32_t)(500 - out_pos - out_gyro), (int32_t)(500 - out_pos - out_gyro),
(int32_t)(500 + out_speed + out_gyro), (int32_t)(500 + out_speed + out_gyro),
@@ -74,6 +90,7 @@ void sport_motion(void)
} }
by_pwm_update_duty(bt_fly + 500, bt_fly + 500); by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
} }
/** /**
* @brief 结构体初始化 * @brief 结构体初始化
* *