#include "bsp_at8236.h"
static int16_t Motor_Ignore_Dead_Zone(int16_t pulse)
{
if (pulse > 0) return pulse + MOTOR_IGNORE_PULSE;
if (pulse < 0) return pulse - MOTOR_IGNORE_PULSE;
return 0;
}
void init_motor(void)
{
DL_TimerA_startCounter(PWM_L_INST);
DL_TimerA_startCounter(PWM_R_INST);
}
void L_control(uint16_t motor_speed,uint8_t dir)
{
if(dir)
{
DL_TimerA_setCaptureCompareValue(PWM_L_INST, motor_speed, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_1_INDEX);
}
else
{
DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_L_INST, motor_speed, DL_TIMER_CC_1_INDEX);
}
}
void R_control(uint16_t motor_speed,uint8_t dir)
{
if(dir)
{
DL_TimerA_setCaptureCompareValue(PWM_R_INST, motor_speed, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_1_INDEX);
}
else
{
DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_R_INST, motor_speed, DL_TIMER_CC_1_INDEX);
}
}
// 控制小车运动,Motor_X=[-3600, 3600],超过范围则无效。
void Motion_Set_Pwm(int16_t Motor_1, int16_t Motor_2)
{
if (Motor_1 >= -MOTOR_MAX_PULSE && Motor_1 <= MOTOR_MAX_PULSE)
{
Motor_Set_Pwm(Motor_M1, Motor_1);
}
if (Motor_2 >= -MOTOR_MAX_PULSE && Motor_2 <= MOTOR_MAX_PULSE)
{
Motor_Set_Pwm(Motor_M2, Motor_2);
}
}
// 设置电机速度,speed:±3600, 0为停止
void Motor_Set_Pwm(uint8_t id, int16_t speed)
{
int16_t pulse = Motor_Ignore_Dead_Zone(speed);
// 限制输入
if (pulse >= MOTOR_MAX_PULSE)
pulse = MOTOR_MAX_PULSE;
if (pulse <= -MOTOR_MAX_PULSE)
pulse = -MOTOR_MAX_PULSE;
switch (id)
{
case Motor_M1:
{
if (pulse >= 0)
{
DL_TimerA_setCaptureCompareValue(PWM_L_INST, pulse, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_1_INDEX);
}
else
{
pulse = -pulse; //把负数转成正数
DL_TimerA_setCaptureCompareValue(PWM_L_INST, 0, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_L_INST, pulse, DL_TIMER_CC_1_INDEX);
}
break;
}
case Motor_M2:
{
if (pulse >= 0)
{
DL_TimerA_setCaptureCompareValue(PWM_R_INST, pulse, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_1_INDEX);
}
else
{
pulse = -pulse; //把负数转成正数
DL_TimerA_setCaptureCompareValue(PWM_R_INST, 0, DL_TIMER_CC_0_INDEX);
DL_TimerA_setCaptureCompareValue(PWM_R_INST, pulse, DL_TIMER_CC_1_INDEX);
}
break;
}
default:
break;
}
}
上述代码为at8236驱动模块驱动电机的代码,帮我改成tb6612驱动模块的代码,并配合以下代码实现控制速度
#include "app_motor.h"
extern volatile int32_t gEncoderCount_L,gEncoderCount_R;
pid_t pid_motor[2];
int Encoder_speed_offset[2] = {0};
int Encoder_speed_last[2] = {0};
int Encoder_speed_now[2] = {0};
uint8_t g_start_ctrl = 0;
car_data_t car_data;
motor_data_t motor_data;
// 获取开机到现在总共的两路编码器计数。
void Encoder_Get_ALL(int* Encoder_all)
{
Encoder_all[0] -= gEncoderCount_L;
Encoder_all[1] += gEncoderCount_R;
gEncoderCount_L = 0;//清0
gEncoderCount_R = 0;//清0
}
// 获取编码器数据,并计算偏差脉冲数
void Motion_Get_Encoder(void)
{
Encoder_Get_ALL(Encoder_speed_now);
for(uint8_t i = 0; i < Motor_MAX; i++)
{
// 记录两次测试时间差的脉冲数
Encoder_speed_offset[i] = Encoder_speed_now[i] - Encoder_speed_last[i];
// 记录上次编码器数据
Encoder_speed_last[i] = Encoder_speed_now[i];
}
}
//返回编码器脉冲
static float Motion_Get_Circle_Pulse(void)
{
return ENCODER_CIRCLE_450;
}
// 返回当前小车轮子轴间距和的一半
static float Motion_Get_APB(void)
{
return STM32Car_APB;
}
// 返回当前小车轮子转一圈的多少毫米
static float Motion_Get_Circle_MM(void)
{
return WHEEL_CIRCLE_MM;
}
void Motor_PID_Init(void)
{
for (int i = 0; i < Motor_MAX; i++)
{
pid_motor[i].target_val = 0.0;
pid_motor[i].pwm_output = 0.0;
pid_motor[i].err = 0.0;
pid_motor[i].err_last = 0.0;
pid_motor[i].err_next = 0.0;
pid_motor[i].integral = 0.0;
pid_motor[i].Kp = MOTOR_P;
pid_motor[i].Ki = MOTOR_I;
pid_motor[i].Kd = MOTOR_D;
}
}
// 设置PID目标速度,单位为:mm/s
void PID_Set_Motor_Target(uint8_t motor_id, float target)
{
if (motor_id > Motor_MAX) return;
if (motor_id == Motor_MAX)
{
for (int i = 0; i < Motor_MAX; i++)
{
pid_motor[i].target_val = target;
}
}
else
{
pid_motor[motor_id].target_val = target;
}
}
// 清除PID数据
void PID_Clear_Motor(uint8_t motor_id)
{
if (motor_id > Motor_MAX) return;
if (motor_id == Motor_MAX)
{
for (int i = 0; i < Motor_MAX; i++)
{
pid_motor[i].pwm_output = 0.0;
pid_motor[i].err = 0.0;
pid_motor[i].err_last = 0.0;
pid_motor[i].err_next = 0.0;
pid_motor[i].integral = 0.0;
}
}
else
{
pid_motor[motor_id].pwm_output = 0.0;
pid_motor[motor_id].err = 0.0;
pid_motor[motor_id].err_last = 0.0;
pid_motor[motor_id].err_next = 0.0;
pid_motor[motor_id].integral = 0.0;
}
}
// 增量式PID计算公式
float PID_Incre_Calc(pid_t *pid, float actual_val)
{
/*计算目标值与实际值的误差*/
pid->err = pid->target_val - actual_val;
/*PID算法实现*/
pid->pwm_output += pid->Kp *(pid->err - pid->err_next)
+ pid->Ki * pid->err
+ pid->Kd *(pid->err - 2 * pid->err_next + pid->err_last);
/*传递误差*/
pid->err_last = pid->err_next;
pid->err_next = pid->err;
/*返回PWM输出值*/
if (pid->pwm_output > (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE))
pid->pwm_output = (MOTOR_MAX_PULSE-MOTOR_IGNORE_PULSE);
if (pid->pwm_output < (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE))
pid->pwm_output = (MOTOR_IGNORE_PULSE-MOTOR_MAX_PULSE);
return pid->pwm_output;
}
// PID计算输出值
void PID_Calc_Motor(motor_data_t* motor)
{
int i;
for (i = 0; i < Motor_MAX; i++)
{
motor->speed_pwm[i] = PID_Incre_Calc(&pid_motor[i], motor->speed_mm_s[i]);
}
}
void Motion_Set_Speed(int16_t speed_m1, int16_t speed_m2)
{
if(speed_m1 == 0 && speed_m2 == 0)
{
Motion_Stop();
}
else
{
//因为有毛刺,编码器不一定为0,根据pid会进行控制,会导致停止不了。
//所以产生if
g_start_ctrl = 1;
}
motor_data.speed_set[0] = speed_m1;
motor_data.speed_set[1] = speed_m2;
for (uint8_t i = 0; i < Motor_MAX; i++)
{
PID_Set_Motor_Target(i, motor_data.speed_set[i]*1.0);
}
}
// 从编码器读取当前各轮子速度,单位mm/s
void Motion_Get_Speed(car_data_t* car)
{
int i = 0;
float speed_mm[Motor_MAX] = {0};
float circle_mm = Motion_Get_Circle_MM();
float circle_pulse = Motion_Get_Circle_Pulse();
float robot_APB = Motion_Get_APB();
Motion_Get_Encoder();
// 计算轮子速度,单位mm/s。
for (i = 0; i < Motor_MAX; i++)
{
speed_mm[i] = (Encoder_speed_offset[i]) * 100 * circle_mm / circle_pulse;
}
car->Vx = (speed_mm[0] + speed_mm[1]) / 2.0f;
car->Vy = 0;
car->Vz = -(speed_mm[0] - speed_mm[1] ) / 2.0f / robot_APB * 1000;
if (g_start_ctrl)
{
PID_Calc_Motor(&motor_data);
}
}
// 小车停止
void Motion_Stop(void)
{
Motion_Set_Speed(0,0);
PID_Clear_Motor(Motor_MAX);
g_start_ctrl = 0;
Motion_Set_Pwm(0,0);
}
static float speed_lr = 0;
static float speed_fb = 0;
static float speed_spin = 0;
static int speed_L_setup = 0;
static int speed_R_setup = 0;
void wheel_Ctrl(int16_t V_x, int16_t V_y, int16_t V_z) //巡线PID直接输出此值
{
float robot_APB = Motion_Get_APB();
// speed_lr = -V_y;
speed_lr = 0;
speed_fb = V_x;
speed_spin = (V_z / 1000.0f) * robot_APB;
if (V_x == 0 && V_y == 0 && V_z == 0)
{
Motion_Stop();
return;
}
speed_L_setup = speed_fb + speed_lr + speed_spin;
speed_R_setup = speed_fb - speed_lr - speed_spin;
if (speed_L_setup > 1000) speed_L_setup = 1000;
if (speed_L_setup < -1000) speed_L_setup = -1000;
if (speed_R_setup > 1000) speed_R_setup = 1000;
if (speed_R_setup < -1000) speed_R_setup = -1000;
//printf("%d\t,%d\t,%d\t,%d\r\n",speed_L1_setup,speed_L2_setup,speed_R1_setup,speed_R2_setup);
Motion_Set_Speed(speed_L_setup, speed_R_setup);
}
// 运动控制句柄,每10ms调用一次,主要处理速度相关的数据
void Motion_Handle(void)
{
Motion_Get_Speed(&car_data);
if (g_start_ctrl)
{
Motion_Set_Pwm(motor_data.speed_pwm[0], motor_data.speed_pwm[1]);
}
}
//直接控制pwm
void motion_car_control(int16_t V_x, int16_t V_y, int16_t V_z)
{
float robot_APB = Motion_Get_APB();
speed_lr = 0;
speed_fb = V_x;
speed_spin = (V_z / 1000.0f) * robot_APB;
if (V_x == 0 && V_y == 0 && V_z == 0)
{
Motion_Set_Pwm(0,0);
return;
}
speed_L_setup = speed_fb + speed_spin;
speed_R_setup = speed_fb - speed_spin;
if (speed_L_setup > 1000) speed_L_setup = 1000;
if (speed_L_setup < -1000) speed_L_setup = -1000;
if (speed_R_setup > 1000) speed_R_setup = 1000;
if (speed_R_setup < -1000) speed_R_setup = -1000;
//printf("%d\t,%d\t,%d\t,%d\r\n",speed_L1_setup,speed_L2_setup,speed_R1_setup,speed_R2_setup);
Motion_Set_Pwm(speed_L_setup, speed_R_setup);
}