diff --git a/app/jj_motion.c b/app/jj_motion.c index c890957..9127b20 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -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 结构体初始化 *