From 82e5f25e3aff34135a769c0a246da03910826a40 Mon Sep 17 00:00:00 2001 From: bmy <2583236812@qq.com> Date: Fri, 10 May 2024 12:17:54 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=A2=9E=E5=8A=A0=E8=B7=9D=E7=A6=BB?= =?UTF-8?q?=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/by_motion.c | 170 ++++++++++++++++++++++++++++--------- app/by_motion.h | 1 + project/src/at32f413_int.c | 1 + project/src/main.c | 2 +- 4 files changed, 132 insertions(+), 42 deletions(-) diff --git a/app/by_motion.c b/app/by_motion.c index f6e01a5..7e945e0 100644 --- a/app/by_motion.c +++ b/app/by_motion.c @@ -8,11 +8,16 @@ #define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15) #define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15) +#define MU (2.854f) // speed = \mu * speed_in_pulse + by_motor_param param_m1; by_motor_param param_m2; PID_TypeDef pid_m1; PID_TypeDef pid_m2; uint8_t motion_enable_flag; +uint8_t motion_busy_flag; +int16_t time_via; +float position_abs; void by_motion_set_pwm_m1(int32_t pwm_duty) { @@ -23,61 +28,116 @@ void by_motion_set_pwm_m1(int32_t pwm_duty) tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty); } -void by_motion_set_pwm_m2(int32_t pwm_duty) -{ - pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦 - pwm_duty += 499; +// void by_motion_set_pwm_m2(int32_t pwm_duty) +// { +// pwm_duty = clip_s32(pwm_duty, -440, 440); // 不可以拉满哦 +// pwm_duty += 499; - // 互补 pwm 输出，499 为中值 - tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty); -} +// // 互补 pwm 输出，499 为中值 +// tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty); +// } int16_t by_motion_get_speed_m1(void) { #define alpha (0.1f) static float last_speed = 0.0f; param_m1.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(+1 * (int16_t)tmr_counter_value_get(TMR2)); - last_speed = param_m1.real_speed; + last_speed = param_m1.real_speed; tmr_counter_value_set(TMR2, 0); return (int16_t)param_m1.real_speed; #undef alpha } -int16_t by_motion_get_speed_m2(void) -{ -#define alpha (0.1f) - static float last_speed = 0.0f; - param_m2.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3)); - last_speed = param_m2.real_speed; - tmr_counter_value_set(TMR3, 0); - return (int16_t)param_m2.real_speed; -#undef alpha -} +// int16_t by_motion_get_speed_m2(void) +// { +// #define alpha (0.1f) +// static float last_speed = 0.0f; +// param_m2.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3)); +// last_speed = param_m2.real_speed; +// tmr_counter_value_set(TMR3, 0); +// return (int16_t)param_m2.real_speed; +// #undef alpha +// } void by_motion_set_speed_m1(int16_t speed) { param_m1.target_speed = speed; } -void by_motion_set_speed_m2(int16_t speed) +// void by_motion_set_speed_m2(int16_t speed) +// { +// param_m2.target_speed = speed; +// } + +void by_motion_set_distance(float distance, int16_t speed) { - param_m2.target_speed = speed; + if (motion_busy_flag) { + return; + } + + if (speed < 0) { + speed = -speed; + } + + if (distance < 0.0f) { + by_motion_set_speed_m1(-1 * speed); + distance = -distance; + } else { + by_motion_set_speed_m1(speed); + } + + motion_busy_flag = 1; + position_abs += distance; + time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s + // lwprintf("time_via = %d\r\n", time_via); +} + +void by_motion_set_distance2(float distance, int16_t speed) +{ + if (motion_busy_flag) { + return; + } + + if (speed < 0) { + speed = -speed; + } + + if (distance < 0.0f) { + distance = -distance; + } + + distance = position_abs - distance; + + if (distance < 0.0f) { + distance = -distance; + by_motion_set_speed_m1(-1 * speed); + } else { + by_motion_set_speed_m1(speed); + } + + motion_busy_flag = 1; + position_abs += distance; + time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s + // lwprintf("time_via = %d\r\n", time_via); } void by_motion_init(void) { motion_enable_flag = 0; + motion_busy_flag = 0; + time_via = 0; + position_abs = 0.0f; // TODO 待添加复位操作 by_motion_set_pwm_m1(0); - by_motion_set_pwm_m2(0); + // by_motion_set_pwm_m2(0); /* set default parameters */ param_m1.Kp = BY_MOTION_DEFAULT_KP_M1; param_m1.Ki = BY_MOTION_DEFAULT_KI_M1; param_m1.Kd = BY_MOTION_DEFAULT_KD_M1; - param_m2.Kp = BY_MOTION_DEFAULT_KP_M2; - param_m2.Ki = BY_MOTION_DEFAULT_KI_M2; - param_m2.Kd = BY_MOTION_DEFAULT_KD_M2; + // param_m2.Kp = BY_MOTION_DEFAULT_KP_M2; + // param_m2.Ki = BY_MOTION_DEFAULT_KI_M2; + // param_m2.Kd = BY_MOTION_DEFAULT_KD_M2; /* load parameters form ee */ // TODO @@ -87,10 +147,10 @@ void by_motion_init(void) PID_SetSampleTime(&pid_m1, 10); PID_SetOutputLimits(&pid_m1, -400, 400); - PID(&pid_m2, ¶m_m2.real_speed, ¶m_m2.out_pwm, ¶m_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT); - PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC); - PID_SetSampleTime(&pid_m2, 10); - PID_SetOutputLimits(&pid_m2, -400, 400); + // PID(&pid_m2, ¶m_m2.real_speed, ¶m_m2.out_pwm, ¶m_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT); + // PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC); + // PID_SetSampleTime(&pid_m2, 10); + // PID_SetOutputLimits(&pid_m2, -400, 400); motion_enable_flag = 1; DRV_ENABLE(); @@ -112,22 +172,50 @@ void by_motion_run(void) void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len) { -#define BC2D_MODEL1 - if (0x01 == stdd_id) { -#if defined(BC2D_MODEL1) - int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8)); - int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8)); - by_motion_set_speed_m1(speed_m1_temp); - by_motion_set_speed_m2(speed_m2_temp); -#elif defined(BC2D_MODEL2) - int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8)); - int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8)); - by_motion_set_speed_m1(speed_m1_temp); - by_motion_set_speed_m2(speed_m2_temp); -#endif + if (0x08 == stdd_id) { + uint8_t mode = data[0]; + int16_t speed = (int16_t)data[1]; + // float distance = (float)(data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24)); + float distance; + memcpy(&distance, &data[2], 4); + if (0 == mode) { + by_motion_set_distance2(distance, speed); + } else if (1 == mode) { + by_motion_set_distance(distance, speed); + } } + // #define BC2D_MODEL1 + + // if (0x01 == stdd_id) { + // #if defined(BC2D_MODEL1) + // int16_t speed_m1_temp = (int16_t)(data[0] | (data[1] << 8)); + // int16_t speed_m2_temp = (int16_t)(data[2] | (data[3] << 8)); + // by_motion_set_speed_m1(speed_m1_temp); + // by_motion_set_speed_m2(speed_m2_temp); + // #elif defined(BC2D_MODEL2) + // int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8)); + // int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8)); + // by_motion_set_speed_m1(speed_m1_temp); + // by_motion_set_speed_m2(speed_m2_temp); + // #endif + // } + LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm); LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm); +} + +void by_motion_tmr_handle(void) +{ + if (!motion_busy_flag) { + return; + } + + if (time_via > 0) { + time_via--; + } else { + by_motion_set_speed_m1(0); + motion_busy_flag = 0; + } } \ No newline at end of file diff --git a/app/by_motion.h b/app/by_motion.h index c8e43ef..058c87b 100644 --- a/app/by_motion.h +++ b/app/by_motion.h @@ -28,6 +28,7 @@ extern int16_t by_motion_get_speed_m2(void); extern void by_motion_set_speed_m1(int16_t speed); extern void by_motion_set_speed_m2(int16_t speed); extern void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len); +extern void by_motion_tmr_handle(void); extern by_motor_param param_m1; extern by_motor_param param_m2; diff --git a/project/src/at32f413_int.c b/project/src/at32f413_int.c index 2f57a73..b0a573a 100644 --- a/project/src/at32f413_int.c +++ b/project/src/at32f413_int.c @@ -237,6 +237,7 @@ void TMR4_GLOBAL_IRQHandler(void) /* add user code begin TMR4_GLOBAL_IRQ 0 */ if (SET == tmr_interrupt_flag_get(TMR4, TMR_OVF_FLAG)) { by_motion_run(); + by_motion_tmr_handle(); tmr_flag_clear(TMR4, TMR_OVF_FLAG); } /* add user code end TMR4_GLOBAL_IRQ 0 */ diff --git a/project/src/main.c b/project/src/main.c index 6feb0b4..cac91df 100644 --- a/project/src/main.c +++ b/project/src/main.c @@ -129,7 +129,7 @@ int main(void) /* add user code begin 3 */ // lwprintf("pwm value: %f\r\n", param_m1.out_pwm); // lwprintf("mot speed: %f\r\n", param_m1.real_speed); - lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm); + // lwprintf("%f,%f,%f\r\n", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm); /* add user code end 3 */ } }