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;
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)
{
@@ -54,14 +70,14 @@ void sport_motion(void)
if (cnt1 >= 10) {
PID_Compute(&far_angle_pid);
PID_Compute(&speed_pid);
in_speed = -encoder_get_count(TIM5_ENCOEDER); // 速度输入，m/s
// printf("rate:%d\r\n", (int16)in_speed);
encoder_clear_count(TIM5_ENCOEDER);
// in_speed = -encoder_get_count(TIM5_ENCOEDER); // 速度输入，m/s
in_speed = sport_get_speed();
printf("rate:%d\r\n", (int16_t)in_speed);
PID_Compute(&near_pos_pid);
cnt1 = 0;
}
if (bt_run_flag == 1) {
if (bt_run_flag == 1) {
by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro),
(int32_t)(500 - out_pos - 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);
}
/**
* @brief 结构体初始化
*