前言 --末尾有总体的.c和.h
本篇文章把麦克纳姆轮的代码封装到.c和.h，使用者只需要根据轮子正转的方向，在.h处修改定义方向引脚，把轮子都统一正向后，后面的轮子驱动函数就可以正常了，然后直接调用函数驱动即可。
设置满转预充装载值是1000
其中会有几种方案
第一种是直接开环，没有任何反馈
第二种是编码行走，开环走到指定位置，容易收到重力影响导致走歪
第三种是陀螺仪行走，有陀螺仪闭环防止走歪
第四种是编码陀螺仪，是第二种的升级版，走到指定距离且不歪
头文件处的定义内容讲解
我们先看.h，知道哪些函数会有哪些功能和宏定义参数
1.方向引脚，定时器引脚，编码中断引脚定义
首先 是在.h的宏定义处定义引脚编号，比如tb6612控制方向的引脚，以及设定了哪个定时器作为pwm输出，还有编码计次的中断引脚
/* 硬件相关宏定义 */
/*
1 前进方向往上 4
2 3
*/
// TB6612 方向引脚定义
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)
#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)
#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)
#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)
/* 定时器相关宏定义 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4
/* 编码器相关宏定义 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE
把自己的引脚根据原理图填对应，方向引脚也是，如果发现反转就把对应的引脚定义反过来，编码也是，编码我们用一个引脚就好，都在cubemx配置好。
2.参数定义
/* 控制参数相关宏定义 */
// 速度控制参数
#define MIN_ROTATION_DUTY_CYCLE 100
#define ROTATION_SPEED 300
// PD控制器参数
#define BACK_KP 10 // 后退PD参数
#define BACK_KD 0
#define STRAIGHT_KP 10 // 直行PD参数
#define STRAIGHT_KD 0
#define LEFT_KP 30 // 横向左转PD参数
#define LEFT_KD 0
#define RIGHT_KP 30 // 横向右转PD参数
#define RIGHT_KD 0
#define ROTATE_KP 5 // 旋转PD参数
#define ROTATE_KD 0.2
// 陀螺仪配置
#define GYRO_POLARITY 0
/* 类型定义 */
typedef struct {
float kp; // 比例系数
float kd; // 微分系数
float prev_error; // 上一次误差
} PDController;
这里的话包括，第一个是自旋转的时候，最小的旋转占空比，还有正常旋转占空比。
然后是配合上陀螺仪时候的PID各个功能参数
还有一个是陀螺仪极性，调用函数后发现是往相反方向移动，那么就修改这个值，因为陀螺仪正放和倒放的极性不同，0和1代表不同的极性。可以观察使用测试程序后，如果发现旋转后离目标点越来越大，那就修改极性。
一 功能函数的定义
后面会用到这么多函数，其中包括
1.基本开环函数
这里的函数就是不带反馈的，给占空比值就动，会容易收到重力分布不均影响，导致走歪。
2.距离开环函数
这里的函数根据给定要跑的距离，跑到后停止，距离由uint16_t wheel_get_count(void);函数返回，但是跑的过程中可能会因为重力分布不均影响，导致走歪。
3.带陀螺仪的闭环函数
这里的函数通过传入标准值，然后方向会以标准值作为参考调制轮子占空比，从而防止走偏。不过使用的时候要在循环里面使用，因为只调用一次肯定是行不通的。
4.带编码的陀螺仪闭环函数
这个根据陀螺仪的值防止走偏，然后由根据编码值确定要走的距离，最为精准。
/* 函数声明 */
// 初始化函数
void wheel_init(void);
void wheel_begin_go(void);
// 基础控制函数
void wheel_stop(void);
void set_all_pwm_channels(uint16_t speed);
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4);
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity);
// 闭环控制函数
void wheel_go_back(uint16_t now_z, uint16_t speed);
// 修改函数名
void wheel_go_forward(uint16_t now_z, uint16_t speed);
void wheel_go_right(uint16_t now_z, uint16_t speed);
void wheel_go_left(uint16_t now_z, uint16_t speed);
void wheel_rotate(int16_t angle);
// 开环控制函数
void wheel_go_forward_openloop(uint16_t speed);
void wheel_go_back_openloop(uint16_t speed);
void wheel_go_left_openloop(uint16_t speed);
void wheel_go_right_openloop(uint16_t speed);
void wheel_rotate_left_openloop(uint16_t speed);
void wheel_rotate_right_openloop(uint16_t speed);
// 带编码器的闭环控制函数
// 修改函数名
void wheel_go_forward_count_close(uint16_t count, uint16_t speed);
void wheel_go_back_count_close(uint16_t count, uint16_t speed);
void wheel_go_left_count_close(uint16_t count, uint16_t speed);
void wheel_go_right_count_close(uint16_t count, uint16_t speed);
// 带编码器的开环控制函数
void wheel_go_forward_count_open(uint16_t count, uint16_t speed);
void wheel_go_back_count_open(uint16_t count, uint16_t speed);
void wheel_go_left_count_open(uint16_t count, uint16_t speed);
void wheel_go_right_count_open(uint16_t count, uint16_t speed);
// 辅助函数
uint16_t clamp_speed(uint16_t speed, uint16_t error);
uint16_t get_angle_value(void);
uint16_t wheel_get_count(void);
int32_t count_save(uint8_t cmd);
void wheel_polarity_test(void);
#endif // WHEEL_H
二.辅助函数的介绍
有一些算法需要功能函数
1.过冲函数
我们使用陀螺仪配合的时候，万一根据差值相减后给的占空比小于0，因为是无符号整型，此时是无穷大的，这时候的占空比会拉满，导致过冲现象。
/**
* @brief 确保计算后的速度不小于0
* @param speed 当前速度值
* @param error 误差值
* @return 调整后的速度值
*/
uint16_t clamp_speed(uint16_t speed, uint16_t error) {
if (speed > error) {
return speed - error;
}
return 0;
}
/**
* @brief 限制速度在合理范围
* @param speed 当前速度值
* @param max_speed 最大速度限制
* @return 调整后的速度值
*/
uint16_t limit_speed(uint16_t speed, uint16_t max_speed) {
if (speed > max_speed) {
return max_speed;
}
return speed;
}2. 设置占空比函数
两个函数，一个设置全部占空比，一个分别设置占空比
/**
* @brief 设置四个通道PWM占空比(相同值)
* @param speed PWM占空比值
*/
void set_all_pwm_channels(uint16_t speed) {
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed);
}
/**
* @brief 设置四个通道PWM占空比(不同值)
* @param speed1 通道1PWM值
* @param speed2 通道2PWM值
* @param speed3 通道3PWM值
* @param speed4 通道4PWM值
*/
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4) {
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed1);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed2);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed3);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed4);
}
3.TB6612方向确认及初始化函数
这里主要是让引脚定义规范，基本上默认0就是正转
然后还有初始化和停车的函数
/**
* @brief 控制TB6612电机方向
* @param motor_num 电机编号(1-4)
* @param polarity 方向(0或1)
*/
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity) {
switch (motor_num) {
case 1:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 1);
}
break;
case 2:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 1);
}
break;
case 3:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 1);
}
break;
case 4:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 1);
}
break;
default:
break;
}
}
/**
* @brief 初始化轮子控制模块
*/
void wheel_init() {
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
set_all_pwm_channels(0);
}
/**
* @brief 停止所有轮子
*/
void wheel_stop() {
set_all_pwm_channels(0);
}4.编码计次及陀螺仪函数
每次启动前可以用wheel_begin_go()函数清零看，走的过程中可以用 wheel_get_count()返回数值，get_angle_value()函数里，修改成你返回z轴角度值的函数 ，修改成这样调用方便，要不然定义的函数不一样要修改的太多了。
/**
* @brief 开始运动，重置编码器计数
*/
void wheel_begin_go()
{
count_save(8);
}
/**
* @brief 获取编码器平均计数
* @return 四个编码器的平均计数值
*/
uint16_t wheel_get_count()
{
return (count_save(4)+count_save(5)+count_save(6)+count_save(7))/4;
}
/**
* @brief 编码器计数保存与读取函数
* @param cmd 命令码，0-3 为计数加 1，4-7 为读取计数，8 为重置计数
* @return 根据命令码返回相应计数值，无返回值命令返回 0
*/
int32_t count_save(uint8_t cmd)
{
static int32_t count[4]={0};
if(cmd<4)
{
count[cmd]++;
}
else if(cmd<8)
{
return count[cmd-4];
}
else if(cmd==8)
{
for(uint8_t i=0;i<4;i++)
count[i]=0;
}
return 0;
}
/**
* @brief GPIO 外部中断回调函数
* @param GPIO_Pin 触发中断的 GPIO 引脚
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
switch (GPIO_Pin) {
case ENCODER_CHA_1_PIN:
count_save(0);
break;
case ENCODER_CHA_2_PIN:
count_save(1);
break;
case ENCODER_CHA_3_PIN:
count_save(2);
break;
case ENCODER_CHA_4_PIN:
count_save(3);
break;
default:
break;
}
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
}
/**
* @brief 获取当前角度值
* @return 当前角度值，调用 get_xyz_z 函数获取
*/
uint16_t get_angle_value()
{
return (uint16_t)get_xyz_z();
}
5.防陀螺仪角度突变函数
我们的目标角度是355度，但是当前是5度，我们应该是往0处突变成359，而不是旋转一周，所以需要旋转突变辅助。
/**
* @brief 计算角度差值
* @param target 目标角度
* @param current 当前角度
* @return 角度差值
*/
int16_t calculate_angle_diff(uint16_t target, uint16_t current) {
int16_t diff = (int16_t)(target - current);
diff = (diff + 180) % 360;
if (diff < 0) {
diff += 360;
}
diff -= 180;
#if GYRO_POLARITY == 0
diff = -diff;
#endif
return diff;
}三.功能函数
1.基本开环函数
（1）前进
/**
* @brief 开环前进
* @param speed 目标速度
*/
void wheel_go_forward_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}（2）后退
/**
* @brief 开环后退
* @param speed 目标速度
*/
void wheel_go_back_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}
（3）左移
/**
* @brief 开环左移
* @param speed 目标速度
*/
void wheel_go_left_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}
（4）右移
/**
* @brief 开环右移
* @param speed 目标速度
*/
void wheel_go_right_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}（5）自旋转往左
/**
* @brief 开环左转
* @param speed 目标速度
*/
void wheel_rotate_left_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}（6）自旋转向右
/**
* @brief 开环右转
* @param speed 目标速度
*/
void wheel_rotate_right_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}2.带编码的开环函数
这个直接走指定距离
（1）前进
/**
* @brief 带编码器开环前进
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_forward_count_open(uint16_t count, uint16_t speed)
{
// 重置编码器计数
wheel_begin_go();
// 等待直到编码器计数达到目标值
while (wheel_get_count() < count) {
wheel_go_forward_openloop(speed);
}
// 停止电机
set_all_pwm_channels(0);
}（2）后退
/**
* @brief 带编码器开环后退
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_back_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_back_openloop(speed);
}
set_all_pwm_channels(0);
}（3）左移
/**
* @brief 带编码器开环左移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_left_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_left_openloop(speed);
}
set_all_pwm_channels(0);
}
（4）右移
/**
* @brief 带编码器开环右移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_right_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_right_openloop(speed);
}
set_all_pwm_channels(0);
}3.陀螺仪闭环函数
这种是根据陀螺仪变化的，需要在while()里执行
（1）前进
/**
* @brief 直行控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_forward(uint16_t now_z, uint16_t speed) {
PDController pd = {STRAIGHT_KP, STRAIGHT_KD, 0};
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}（2）后退
/**
* @brief 后退控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_back(uint16_t now_z, uint16_t speed) {
PDController pd = {BACK_KP, BACK_KD, 0};
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}（3）左移
/**
* @brief 左移控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_left(uint16_t now_z, uint16_t speed) {
PDController pd = {LEFT_KP, LEFT_KD, 0};
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 1);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}（4）右移
/**
* @brief 右移控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_right(uint16_t now_z, uint16_t speed) {
PDController pd = {RIGHT_KP, RIGHT_KD, 0};
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 0);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}（5）自旋转
其中左转为正
/**
* @brief 旋转控制(带陀螺仪闭环)
* @param angle 目标旋转角度(正值为左转，负值为右转)
*/
void wheel_rotate(int16_t angle) {
PDController pd = {ROTATE_KP, ROTATE_KD, 0};
uint16_t initial_z = get_angle_value();
uint16_t target_z = (uint16_t)((initial_z + angle + 360) % 360);
if (angle < 0) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
} else {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
}
set_all_pwm_channels(0);
while (1) {
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(target_z, z);
if (abs(diff) < 2) {
break;
}
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t speed = (uint16_t)fabs(output);
if (speed < MIN_ROTATION_DUTY_CYCLE) {
speed = MIN_ROTATION_DUTY_CYCLE;
} else if (speed > ROTATION_SPEED) {
speed = ROTATION_SPEED;
}
set_all_pwm_channels(speed);
}
set_all_pwm_channels(0);
}4.带编码的陀螺仪闭环函数
这种的话是带陀螺仪返款的运动，同时根据设定值运动到指定值停止
（1）前进
/**
* @brief 带编码器闭环直行
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_forward_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_forward(z, speed);
}
set_all_pwm_channels(0);
}
（2）后退
/**
* @brief 带编码器闭环后退
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_back_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_back(z, speed);
}
set_all_pwm_channels(0);
}
（3）左移
/**
* @brief 带编码器闭环左移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_left_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_left(z, speed);
}
set_all_pwm_channels(0);
}（4）右移
/**
* @brief 带编码器闭环右移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_right_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_right(z, speed);
}
set_all_pwm_channels(0);
}四.测试函数
通过调用测试函数，来判断轮子的方向极性是否正确
/**
* @brief 轮子极性测试函数，依次调用开环和闭环控制函数测试轮子运动极性
*/
void wheel_polarity_test(void) {
const uint16_t distance = 1000;
const uint16_t speed = 300;
wheel_go_forward_openloop(0);
// 开环测试
// 1234 编号轮子依次正转
set_all_pwm_channels_separate(300, 0, 0, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 300, 0, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 0, 300, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 0, 0, 300);
HAL_Delay(500);
wheel_stop();
HAL_Delay(500);
// 左转
wheel_rotate_left_openloop(speed);
HAL_Delay(500);
// 右转
wheel_rotate_right_openloop(speed);
HAL_Delay(500);
// 正左转
wheel_go_left_openloop(speed);
HAL_Delay(500);
// 正右转
wheel_go_right_openloop(speed);
HAL_Delay(500);
// 前进
wheel_go_forward_openloop(speed);
HAL_Delay(500);
// 后退
wheel_go_back_openloop(speed);
HAL_Delay(500);
// 闭环距离测试
// 前进
wheel_go_forward_count_close(distance, speed);
HAL_Delay(500);
// 后退
wheel_go_back_count_close(distance, speed);
HAL_Delay(500);
// 左移
wheel_go_left_count_close(distance, speed);
HAL_Delay(500);
// 右移
wheel_go_right_count_close(distance, speed);
HAL_Delay(500);
//左转90度
wheel_rotate(90);
wheel_stop();
HAL_Delay(500);
//右转90度
wheel_rotate(-90);
wheel_stop();
HAL_Delay(500);
}
五.使用方法
1.首先确定轮子的极性，把下列引脚对应好
/* 硬件相关宏定义
1 4
2 3
*/
// TB6612 方向引脚定义
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)
#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)
#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)
#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)
/* 定时器相关宏定义 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4
/* 编码器相关宏定义 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE然后这个函数的返回值修改成自己陀螺仪的返回值函数
/**
* @brief 获取当前角度值
* @return 当前角度值，调用 get_xyz_z 函数获取
*/
uint16_t get_angle_value()
{
return (uint16_t)get_xyz_z();
}2.极性正确测试
添加头文件wheel.h，在主函数添加初始化函数wheel_init();，然后在while（1）中只运行调用测试程序wheel_polarity_test() ，看看轮子是否按着1234编号运动，不是请修改
3.陀螺仪极性
随后看看左转和右转是不是以最近角度旋转，如果不是那么就是极性反了
基本上没有问题后就可以正常使用了
六.完整代码
wheel.c
#include "wheel.h"
#include "gpio.h"
#include "tim.h"
#include <stdio.h>
#include <math.h>
/* ==================== 辅助函数 ==================== */
/**
* @brief 确保计算后的速度不小于0
* @param speed 当前速度值
* @param error 误差值
* @return 调整后的速度值
*/
uint16_t clamp_speed(uint16_t speed, uint16_t error) {
if (speed > error) {
return speed - error;
}
return 0;
}
/**
* @brief 限制速度在合理范围
* @param speed 当前速度值
* @param max_speed 最大速度限制
* @return 调整后的速度值
*/
uint16_t limit_speed(uint16_t speed, uint16_t max_speed) {
if (speed > max_speed) {
return max_speed;
}
return speed;
}
/* ==================== 基础控制函数 ==================== */
/**
* @brief 设置四个通道PWM占空比(相同值)
* @param speed PWM占空比值
*/
void set_all_pwm_channels(uint16_t speed) {
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed);
}
/**
* @brief 设置四个通道PWM占空比(不同值)
* @param speed1 通道1PWM值
* @param speed2 通道2PWM值
* @param speed3 通道3PWM值
* @param speed4 通道4PWM值
*/
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4) {
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed1);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed2);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed3);
__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed4);
}
/**
* @brief 控制TB6612电机方向
* @param motor_num 电机编号(1-4)
* @param polarity 方向(0或1)
*/
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity) {
switch (motor_num) {
case 1:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 1);
}
break;
case 2:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 1);
}
break;
case 3:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 1);
}
break;
case 4:
if (polarity == 0) {
HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 1);
HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 0);
} else {
HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 0);
HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 1);
}
break;
default:
break;
}
}
/**
* @brief 初始化轮子控制模块
*/
void wheel_init() {
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
set_all_pwm_channels(0);
}
/**
* @brief 停止所有轮子
*/
void wheel_stop() {
set_all_pwm_channels(0);
}
/**
* @brief 开始运动，重置编码器计数
*/
void wheel_begin_go()
{
count_save(8);
}
/**
* @brief 获取编码器平均计数
* @return 四个编码器的平均计数值
*/
uint16_t wheel_get_count()
{
return (count_save(4)+count_save(5)+count_save(6)+count_save(7))/4;
}
/**
* @brief 编码器计数保存与读取函数
* @param cmd 命令码，0-3 为计数加 1，4-7 为读取计数，8 为重置计数
* @return 根据命令码返回相应计数值，无返回值命令返回 0
*/
int32_t count_save(uint8_t cmd)
{
static int32_t count[4]={0};
if(cmd<4)
{
count[cmd]++;
}
else if(cmd<8)
{
return count[cmd-4];
}
else if(cmd==8)
{
for(uint8_t i=0;i<4;i++)
count[i]=0;
}
return 0;
}
/**
* @brief GPIO 外部中断回调函数
* @param GPIO_Pin 触发中断的 GPIO 引脚
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
switch (GPIO_Pin) {
case ENCODER_CHA_1_PIN:
count_save(0);
break;
case ENCODER_CHA_2_PIN:
count_save(1);
break;
case ENCODER_CHA_3_PIN:
count_save(2);
break;
case ENCODER_CHA_4_PIN:
count_save(3);
break;
default:
break;
}
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
}
/**
* @brief 获取当前角度值
* @return 当前角度值，调用 get_xyz_z 函数获取
*/
uint16_t get_angle_value()
{
return (uint16_t)get_xyz_z();
}
/* ==================== 闭环控制函数 ==================== */
/**
* @brief 计算角度差值
* @param target 目标角度
* @param current 当前角度
* @return 角度差值
*/
int16_t calculate_angle_diff(uint16_t target, uint16_t current) {
int16_t diff = (int16_t)(target - current);
diff = (diff + 180) % 360;
if (diff < 0) {
diff += 360;
}
diff -= 180;
#if GYRO_POLARITY == 0
diff = -diff;
#endif
return diff;
}
/**
* @brief 后退控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_back(uint16_t now_z, uint16_t speed) {
PDController pd = {BACK_KP, BACK_KD, 0};
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}
/**
* @brief 直行控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_forward(uint16_t now_z, uint16_t speed) {
PDController pd = {STRAIGHT_KP, STRAIGHT_KD, 0};
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}
/**
* @brief 左移控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_left(uint16_t now_z, uint16_t speed) {
PDController pd = {LEFT_KP, LEFT_KD, 0};
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 1);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}
/**
* @brief 右移控制(带陀螺仪闭环)
* @param now_z 当前角度
* @param speed 目标速度
*/
void wheel_go_right(uint16_t now_z, uint16_t speed) {
PDController pd = {RIGHT_KP, RIGHT_KD, 0};
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 0);
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(now_z, z);
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t adjustment = (uint16_t)fabs(output);
adjustment = (adjustment > speed) ? speed : adjustment;
if (diff == 0) {
set_all_pwm_channels(speed);
} else if (diff > 0) {
uint16_t left_speed = speed - adjustment;
uint16_t right_speed = speed;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
} else {
uint16_t left_speed = speed;
uint16_t right_speed = speed - adjustment;
set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);
}
}
/**
* @brief 旋转控制(带陀螺仪闭环)
* @param angle 目标旋转角度(正值为左转，负值为右转)
*/
void wheel_rotate(int16_t angle) {
PDController pd = {ROTATE_KP, ROTATE_KD, 0};
uint16_t initial_z = get_angle_value();
uint16_t target_z = (uint16_t)((initial_z + angle + 360) % 360);
if (angle < 0) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
} else {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
}
set_all_pwm_channels(0);
while (1) {
uint16_t z = get_angle_value();
int16_t diff = calculate_angle_diff(target_z, z);
if (abs(diff) < 2) {
break;
}
float derivative = diff - pd.prev_error;
float output = pd.kp * diff + pd.kd * derivative;
pd.prev_error = diff;
uint16_t speed = (uint16_t)fabs(output);
if (speed < MIN_ROTATION_DUTY_CYCLE) {
speed = MIN_ROTATION_DUTY_CYCLE;
} else if (speed > ROTATION_SPEED) {
speed = ROTATION_SPEED;
}
set_all_pwm_channels(speed);
}
set_all_pwm_channels(0);
}
/* ==================== 开环控制函数 ==================== */
/**
* @brief 开环前进
* @param speed 目标速度
*/
void wheel_go_forward_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}
/**
* @brief 开环后退
* @param speed 目标速度
*/
void wheel_go_back_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}
/**
* @brief 开环左移
* @param speed 目标速度
*/
void wheel_go_left_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}
/**
* @brief 开环右移
* @param speed 目标速度
*/
void wheel_go_right_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}
/**
* @brief 开环左转
* @param speed 目标速度
*/
void wheel_rotate_left_openloop(uint16_t speed) {
tb6612_set_direction(1, 1);
tb6612_set_direction(2, 1);
tb6612_set_direction(3, 0);
tb6612_set_direction(4, 0);
set_all_pwm_channels(speed);
}
/**
* @brief 开环右转
* @param speed 目标速度
*/
void wheel_rotate_right_openloop(uint16_t speed) {
tb6612_set_direction(1, 0);
tb6612_set_direction(2, 0);
tb6612_set_direction(3, 1);
tb6612_set_direction(4, 1);
set_all_pwm_channels(speed);
}
/* ==================== 带编码器的闭环控制函数 ==================== */
/**
* @brief 带编码器闭环直行
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_forward_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_forward(z, speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器闭环后退
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_back_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_back(z, speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器闭环左移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_left_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_left(z, speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器闭环右移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_right_count_close(uint16_t count, uint16_t speed) {
wheel_begin_go();
uint16_t z = get_angle_value();
while (wheel_get_count() < count) {
wheel_go_right(z, speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器开环前进
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_forward_count_open(uint16_t count, uint16_t speed)
{
// 重置编码器计数
wheel_begin_go();
// 等待直到编码器计数达到目标值
while (wheel_get_count() < count) {
wheel_go_forward_openloop(speed);
}
// 停止电机
set_all_pwm_channels(0);
}
/**
* @brief 带编码器开环后退
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_back_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_back_openloop(speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器开环左移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_left_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_left_openloop(speed);
}
set_all_pwm_channels(0);
}
/**
* @brief 带编码器开环右移
* @param count 目标编码器计数
* @param speed 目标速度
*/
void wheel_go_right_count_open(uint16_t count, uint16_t speed) {
wheel_begin_go();
while (wheel_get_count() < count) {
wheel_go_right_openloop(speed);
}
set_all_pwm_channels(0);
}
/* ==================== 轮子极性测试函数 ==================== */
/**
* @brief 轮子极性测试函数，依次调用开环和闭环控制函数测试轮子运动极性
*/
void wheel_polarity_test(void) {
const uint16_t distance = 1000;
const uint16_t speed = 300;
wheel_go_forward_openloop(0);
// 开环测试
// 1234 编号轮子依次正转
set_all_pwm_channels_separate(300, 0, 0, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 300, 0, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 0, 300, 0);
HAL_Delay(500);
set_all_pwm_channels_separate(0, 0, 0, 300);
HAL_Delay(500);
wheel_stop();
HAL_Delay(500);
// 左转
wheel_rotate_left_openloop(speed);
HAL_Delay(500);
// 右转
wheel_rotate_right_openloop(speed);
HAL_Delay(500);
// 正左转
wheel_go_left_openloop(speed);
HAL_Delay(500);
// 正右转
wheel_go_right_openloop(speed);
HAL_Delay(500);
// 前进
wheel_go_forward_openloop(speed);
HAL_Delay(500);
// 后退
wheel_go_back_openloop(speed);
HAL_Delay(500);
// 闭环距离测试
// 前进
wheel_go_forward_count_close(distance, speed);
HAL_Delay(500);
// 后退
wheel_go_back_count_close(distance, speed);
HAL_Delay(500);
// 左移
wheel_go_left_count_close(distance, speed);
HAL_Delay(500);
// 右移
wheel_go_right_count_close(distance, speed);
HAL_Delay(500);
wheel_rotate(90);
wheel_stop();
HAL_Delay(500);
wheel_rotate(-90);
wheel_stop();
HAL_Delay(500);
}
wheel.h
#ifndef WHEEL_H
#define WHEEL_H
/**
* @file wheel.h
* @brief 轮子控制模块头文件
*/
/* 系统头文件包含 */
#include "main.h"
/* 硬件相关宏定义 */
// TB6612 方向引脚定义
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)
#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)
#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)
#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)
/* 定时器相关宏定义 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4
/* 编码器相关宏定义 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE
/* 控制参数相关宏定义 */
// 速度控制参数
#define MIN_ROTATION_DUTY_CYCLE 100
#define ROTATION_SPEED 300
// PD控制器参数
#define BACK_KP 10 // 后退PD参数
#define BACK_KD 0
#define STRAIGHT_KP 10 // 直行PD参数
#define STRAIGHT_KD 0
#define LEFT_KP 30 // 横向左转PD参数
#define LEFT_KD 0
#define RIGHT_KP 30 // 横向右转PD参数
#define RIGHT_KD 0
#define ROTATE_KP 5 // 旋转PD参数
#define ROTATE_KD 0.2
// 陀螺仪配置
#define GYRO_POLARITY 0 // 陀螺仪角度极性，0表示角度减，1表示角度加
/* 类型定义 */
typedef struct {
float kp; // 比例系数
float kd; // 微分系数
float prev_error; // 上一次误差
} PDController;
/* 函数声明 */
// 初始化函数
void wheel_init(void);
void wheel_begin_go(void);
// 基础控制函数
void wheel_stop(void);
void set_all_pwm_channels(uint16_t speed);
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4);
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity);
// 闭环控制函数
void wheel_go_back(uint16_t now_z, uint16_t speed);
// 修改函数名
void wheel_go_forward(uint16_t now_z, uint16_t speed);
void wheel_go_right(uint16_t now_z, uint16_t speed);
void wheel_go_left(uint16_t now_z, uint16_t speed);
void wheel_rotate(int16_t angle);
// 开环控制函数
void wheel_go_forward_openloop(uint16_t speed);
void wheel_go_back_openloop(uint16_t speed);
void wheel_go_left_openloop(uint16_t speed);
void wheel_go_right_openloop(uint16_t speed);
void wheel_rotate_left_openloop(uint16_t speed);
void wheel_rotate_right_openloop(uint16_t speed);
// 带编码器的闭环控制函数
// 修改函数名
void wheel_go_forward_count_close(uint16_t count, uint16_t speed);
void wheel_go_back_count_close(uint16_t count, uint16_t speed);
void wheel_go_left_count_close(uint16_t count, uint16_t speed);
void wheel_go_right_count_close(uint16_t count, uint16_t speed);
// 带编码器的开环控制函数
void wheel_go_forward_count_open(uint16_t count, uint16_t speed);
void wheel_go_back_count_open(uint16_t count, uint16_t speed);
void wheel_go_left_count_open(uint16_t count, uint16_t speed);
void wheel_go_right_count_open(uint16_t count, uint16_t speed);
// 辅助函数
uint16_t clamp_speed(uint16_t speed, uint16_t error);
uint16_t get_angle_value(void);
uint16_t wheel_get_count(void);
int32_t count_save(uint8_t cmd);
void wheel_polarity_test(void);
#endif // WHEEL_H
/*
1 4
2 3
设置好对应编号后使用
wheel_polarity_test(void);
这个函数确定极性是否正确，是否按照
1234编号轮子依次正转
然后
左转
右转
正右转
正左转
后退
前进
的方式运动，如果错误请重新矫正
*/
