296 lines
8.7 KiB
C
296 lines
8.7 KiB
C
#include "motor.h"
|
||
|
||
#include <stdbool.h>
|
||
#include <stdint.h>
|
||
|
||
#include "tim.h"
|
||
|
||
static uint16_t s_last_count[4] = {0U, 0U, 0U, 0U};
|
||
static volatile float s_current_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||
static volatile int32_t s_odom_acc_ticks[4] = {0, 0, 0, 0};
|
||
|
||
/* 速度估计滚动窗口：降低 5ms 单拍增量测速的量化噪声 */
|
||
static int32_t s_speed_acc_ticks[4] = {0, 0, 0, 0};
|
||
static uint8_t s_speed_window_count = 0U;
|
||
static float s_speed_window_dt = 0.0f;
|
||
|
||
static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
|
||
uint32_t channel_in1,
|
||
uint32_t channel_in2,
|
||
uint16_t pwm_in1,
|
||
uint16_t pwm_in2);
|
||
static void Motor_PrimeEncoderCounters(void);
|
||
static uint16_t Motor_ClampAbsToPwm(int32_t value);
|
||
static int16_t Motor_SaturateToI16(int32_t value);
|
||
|
||
static inline void Motor_WriteBridge(TIM_HandleTypeDef *htim,
|
||
uint32_t channel_in1,
|
||
uint32_t channel_in2,
|
||
uint16_t pwm_in1,
|
||
uint16_t pwm_in2)
|
||
{
|
||
__HAL_TIM_SET_COMPARE(htim, channel_in1, pwm_in1);
|
||
__HAL_TIM_SET_COMPARE(htim, channel_in2, pwm_in2);
|
||
}
|
||
|
||
static uint16_t Motor_ClampAbsToPwm(int32_t value)
|
||
{
|
||
if (value > PWM_LIMIT)
|
||
{
|
||
value = PWM_LIMIT;
|
||
}
|
||
else if (value < -PWM_LIMIT)
|
||
{
|
||
value = -PWM_LIMIT;
|
||
}
|
||
|
||
if (value < 0)
|
||
{
|
||
value = -value;
|
||
}
|
||
|
||
return (uint16_t)value;
|
||
}
|
||
|
||
static int16_t Motor_SaturateToI16(int32_t value)
|
||
{
|
||
if (value > 32767)
|
||
{
|
||
return 32767;
|
||
}
|
||
if (value < -32768)
|
||
{
|
||
return -32768;
|
||
}
|
||
return (int16_t)value;
|
||
}
|
||
|
||
static void Motor_PrimeEncoderCounters(void)
|
||
{
|
||
s_last_count[MOTOR_FL] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5);
|
||
s_last_count[MOTOR_RL] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim3);
|
||
s_last_count[MOTOR_FR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4);
|
||
s_last_count[MOTOR_RR] = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
|
||
}
|
||
|
||
void Motor_Init(void)
|
||
{
|
||
uint32_t primask = __get_PRIMASK();
|
||
|
||
if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim9, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4) != HAL_OK) { Error_Handler(); }
|
||
|
||
if (HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
|
||
if (HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL) != HAL_OK) { Error_Handler(); }
|
||
|
||
Motor_Brake_All();
|
||
Motor_PrimeEncoderCounters();
|
||
|
||
__disable_irq();
|
||
s_current_rpm[MOTOR_FL] = 0.0f;
|
||
s_current_rpm[MOTOR_RL] = 0.0f;
|
||
s_current_rpm[MOTOR_FR] = 0.0f;
|
||
s_current_rpm[MOTOR_RR] = 0.0f;
|
||
|
||
s_odom_acc_ticks[MOTOR_FL] = 0;
|
||
s_odom_acc_ticks[MOTOR_RL] = 0;
|
||
s_odom_acc_ticks[MOTOR_FR] = 0;
|
||
s_odom_acc_ticks[MOTOR_RR] = 0;
|
||
|
||
s_speed_acc_ticks[MOTOR_FL] = 0;
|
||
s_speed_acc_ticks[MOTOR_RL] = 0;
|
||
s_speed_acc_ticks[MOTOR_FR] = 0;
|
||
s_speed_acc_ticks[MOTOR_RR] = 0;
|
||
s_speed_window_count = 0U;
|
||
s_speed_window_dt = 0.0f;
|
||
__set_PRIMASK(primask);
|
||
}
|
||
|
||
void Set_Motor_Output(Motor_ID_t id, int16_t control_out)
|
||
{
|
||
int32_t hw_command = (int32_t)control_out;
|
||
bool forward;
|
||
uint16_t pwm_val;
|
||
uint16_t in1_pwm, in2_pwm;
|
||
|
||
if ((id == MOTOR_FL) || (id == MOTOR_RL))
|
||
{
|
||
hw_command = -hw_command;
|
||
}
|
||
|
||
forward = (hw_command < 0);
|
||
pwm_val = Motor_ClampAbsToPwm(hw_command);
|
||
|
||
/*
|
||
* --- 慢衰减 (Slow Decay) 实现 ---
|
||
* 正转：IN1 = 100% (常高), IN2 = 100% - PWM
|
||
* 反转：IN1 = 100% - PWM, IN2 = 100% (常高)
|
||
* 此时 PWM 关断期间的状态均为 IN1=1, IN2=1 (短接刹车)
|
||
*/
|
||
if (forward)
|
||
{
|
||
in1_pwm = MAX_PWM_DUTY;
|
||
in2_pwm = MAX_PWM_DUTY - pwm_val;
|
||
}
|
||
else
|
||
{
|
||
in1_pwm = MAX_PWM_DUTY - pwm_val;
|
||
in2_pwm = MAX_PWM_DUTY;
|
||
}
|
||
|
||
/*
|
||
* 选配：如果希望目标速度为 0 时芯片进入低功耗待机(Standby)让电机自由滑行，
|
||
* 可以取消下面这段注释。如果不取消，速度为 0 时电机将被死死刹住。
|
||
*/
|
||
/*
|
||
if (pwm_val == 0) {
|
||
in1_pwm = 0U;
|
||
in2_pwm = 0U;
|
||
}
|
||
*/
|
||
|
||
switch (id)
|
||
{
|
||
case MOTOR_FL:
|
||
Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, in1_pwm, in2_pwm);
|
||
break;
|
||
|
||
case MOTOR_RL:
|
||
Motor_WriteBridge(&htim9, TIM_CHANNEL_1, TIM_CHANNEL_2, in1_pwm, in2_pwm);
|
||
break;
|
||
|
||
case MOTOR_FR:
|
||
Motor_WriteBridge(&htim8, TIM_CHANNEL_1, TIM_CHANNEL_2, in1_pwm, in2_pwm);
|
||
break;
|
||
|
||
case MOTOR_RR:
|
||
Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, in1_pwm, in2_pwm);
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void Motor_Brake_All(void)
|
||
{
|
||
/* AT8236 的主动刹车 (慢衰减) 逻辑为 IN1=1, IN2=1，因此传入最大占空比 */
|
||
Motor_WriteBridge(&htim2, TIM_CHANNEL_3, TIM_CHANNEL_4, MAX_PWM_DUTY, MAX_PWM_DUTY);
|
||
Motor_WriteBridge(&htim9, TIM_CHANNEL_1, TIM_CHANNEL_2, MAX_PWM_DUTY, MAX_PWM_DUTY);
|
||
Motor_WriteBridge(&htim8, TIM_CHANNEL_1, TIM_CHANNEL_2, MAX_PWM_DUTY, MAX_PWM_DUTY);
|
||
Motor_WriteBridge(&htim8, TIM_CHANNEL_3, TIM_CHANNEL_4, MAX_PWM_DUTY, MAX_PWM_DUTY);
|
||
}
|
||
|
||
void Motor_Update_RPM(float dt_s)
|
||
{
|
||
uint16_t curr_count_fl;
|
||
uint16_t curr_count_rl;
|
||
uint16_t curr_count_fr;
|
||
uint16_t curr_count_rr;
|
||
int16_t delta[4];
|
||
float rpm_scale;
|
||
int i;
|
||
|
||
if (dt_s <= 0.0f)
|
||
{
|
||
return;
|
||
}
|
||
|
||
curr_count_fl = (uint16_t)__HAL_TIM_GET_COUNTER(&htim5);
|
||
curr_count_rl = (uint16_t)__HAL_TIM_GET_COUNTER(&htim3);
|
||
curr_count_fr = (uint16_t)__HAL_TIM_GET_COUNTER(&htim4);
|
||
curr_count_rr = (uint16_t)__HAL_TIM_GET_COUNTER(&htim1);
|
||
|
||
delta[MOTOR_FL] = (int16_t)(curr_count_fl - s_last_count[MOTOR_FL]);
|
||
delta[MOTOR_RL] = (int16_t)(curr_count_rl - s_last_count[MOTOR_RL]);
|
||
delta[MOTOR_FR] = (int16_t)(curr_count_fr - s_last_count[MOTOR_FR]);
|
||
delta[MOTOR_RR] = (int16_t)(curr_count_rr - s_last_count[MOTOR_RR]);
|
||
|
||
s_last_count[MOTOR_FL] = curr_count_fl;
|
||
s_last_count[MOTOR_RL] = curr_count_rl;
|
||
s_last_count[MOTOR_FR] = curr_count_fr;
|
||
s_last_count[MOTOR_RR] = curr_count_rr;
|
||
|
||
delta[MOTOR_FL] = (int16_t)(-delta[MOTOR_FL]);
|
||
delta[MOTOR_RL] = (int16_t)(-delta[MOTOR_RL]);
|
||
|
||
s_odom_acc_ticks[MOTOR_FL] += delta[MOTOR_FL];
|
||
s_odom_acc_ticks[MOTOR_RL] += delta[MOTOR_RL];
|
||
s_odom_acc_ticks[MOTOR_FR] += delta[MOTOR_FR];
|
||
s_odom_acc_ticks[MOTOR_RR] += delta[MOTOR_RR];
|
||
|
||
/* 速度估计窗口累加 */
|
||
s_speed_acc_ticks[MOTOR_FL] += delta[MOTOR_FL];
|
||
s_speed_acc_ticks[MOTOR_RL] += delta[MOTOR_RL];
|
||
s_speed_acc_ticks[MOTOR_FR] += delta[MOTOR_FR];
|
||
s_speed_acc_ticks[MOTOR_RR] += delta[MOTOR_RR];
|
||
s_speed_window_dt += dt_s;
|
||
s_speed_window_count++;
|
||
|
||
if (s_speed_window_count >= MOTOR_SPEED_WINDOW_SAMPLES)
|
||
{
|
||
rpm_scale = 60.0f / ((float)PULSES_PER_REV * s_speed_window_dt);
|
||
|
||
for (i = 0; i < 4; ++i)
|
||
{
|
||
s_current_rpm[i] = (float)s_speed_acc_ticks[i] * rpm_scale;
|
||
s_speed_acc_ticks[i] = 0;
|
||
}
|
||
|
||
s_speed_window_count = 0U;
|
||
s_speed_window_dt = 0.0f;
|
||
}
|
||
}
|
||
|
||
float Get_Motor_RPM(Motor_ID_t id)
|
||
{
|
||
if ((id < MOTOR_FL) || (id > MOTOR_RR))
|
||
{
|
||
return 0.0f;
|
||
}
|
||
|
||
return s_current_rpm[id];
|
||
}
|
||
|
||
void Motor_Get_And_Clear_Delta_Ticks(int16_t *d_fl, int16_t *d_rl, int16_t *d_fr, int16_t *d_rr)
|
||
{
|
||
int32_t fl;
|
||
int32_t rl;
|
||
int32_t fr;
|
||
int32_t rr;
|
||
uint32_t primask;
|
||
|
||
if ((d_fl == NULL) || (d_rl == NULL) || (d_fr == NULL) || (d_rr == NULL))
|
||
{
|
||
return;
|
||
}
|
||
|
||
primask = __get_PRIMASK();
|
||
__disable_irq();
|
||
|
||
fl = s_odom_acc_ticks[MOTOR_FL];
|
||
rl = s_odom_acc_ticks[MOTOR_RL];
|
||
fr = s_odom_acc_ticks[MOTOR_FR];
|
||
rr = s_odom_acc_ticks[MOTOR_RR];
|
||
|
||
s_odom_acc_ticks[MOTOR_FL] = 0;
|
||
s_odom_acc_ticks[MOTOR_RL] = 0;
|
||
s_odom_acc_ticks[MOTOR_FR] = 0;
|
||
s_odom_acc_ticks[MOTOR_RR] = 0;
|
||
|
||
__set_PRIMASK(primask);
|
||
|
||
*d_fl = Motor_SaturateToI16(fl);
|
||
*d_rl = Motor_SaturateToI16(rl);
|
||
*d_fr = Motor_SaturateToI16(fr);
|
||
*d_rr = Motor_SaturateToI16(rr);
|
||
}
|