当前位置： 首页 > article >正文

STM32Cubemx-H7-17-麦克纳姆轮驱动

article 2025/6/3 17:23:47

前言   --末尾有总体的.c和.h

本篇文章把麦克纳姆轮的代码封装到.c和.h，使用者只需要根据轮子正转的方向，在.h处修改定义方向引脚，把轮子都统一正向后，后面的轮子驱动函数就可以正常了，然后直接调用函数驱动即可。

设置满转预充装载值是1000

其中会有几种方案

第一种是直接开环，没有任何反馈

第二种是编码行走，开环走到指定位置，容易收到重力影响导致走歪

第三种是陀螺仪行走，有陀螺仪闭环防止走歪

第四种是编码陀螺仪，是第二种的升级版，走到指定距离且不歪

头文件处的定义内容讲解

我们先看.h，知道哪些函数会有哪些功能和宏定义参数

1.方向引脚，定时器引脚，编码中断引脚定义

首先 是在.h的宏定义处定义引脚编号，比如tb6612控制方向的引脚，以及设定了哪个定时器作为pwm输出，还有编码计次的中断引脚

/* 硬件相关宏定义 *//*
1      前进方向往上       42                        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 == 0diff = -diff;
#endifreturn 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 == 0diff = -diff;
#endifreturn 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                42                3设置好对应编号后使用
wheel_polarity_test(void);
这个函数确定极性是否正确，是否按照
1234编号轮子依次正转
然后
左转
右转
正右转
正左转
后退
前进
的方式运动，如果错误请重新矫正
*/

相关文章：

STM32Cubemx-H7-17-麦克纳姆轮驱动

前言 --末尾有总体的.c和.h 本篇文章把麦克纳姆轮的代码封装到.c和.h&#xff0c;使用者只需要根据轮子正转的方向&#xff0c;在.h处修改定义方向引脚&#xff0c;把轮子都统一正向后&#xff0c;后面的轮子驱动函数就可以正常了&#xff0c;然后直接调用函数驱动即可。 设…...

编程日记 2025/6/3 17:23:47

机器学习算法-逻辑回归

今天我们用 「预测考试是否及格」 的例子来讲解逻辑回归&#xff0c;从原理到实现一步步拆解&#xff0c;保证零基础也能懂&#xff01; &#x1f3af; 例子背景 假设你是班主任&#xff0c;要根据学生的「学习时间」预测「是否及格」&#xff0c;手上有以下数据&#xff1a;…...

编程日记 2025/6/4 3:56:02

Office 2024免费下载 安装包

各位办公小能手们&#xff0c;你们知道吗&#xff1f;咱们日常办公经常会用到一个超厉害的软件套件&#xff0c;那就是Office&#xff0c;它全称Microsoft Office&#xff0c;是微软公司开发的。这玩意儿能大大提升个人和团队的办公效率&#xff0c;像文档处理、数据分析、演示…...

编程日记 2025/6/3 21:22:54

Linux云计算训练营笔记day18（Python）

# 猜数字游戏: 程序生产一个 1-100的随机数 # 让用户重复去猜测, 直到猜对为止 # 如果用户输入的数字 大于 随机生成的数字 提示 大了 # 如果用户输入的数字 小于 随机生产的数字 提示 小了 # 否则 猜对了 break # 增加需求 最多猜6次,如果没有猜对&#xff0c;提示 你失…...

编程日记 2025/6/3 22:07:53

Git深入解析功能逻辑与核心业务场景流程

一、Git核心功能逻辑架构 #mermaid-svg-9tj1iCr99u6QenJM {font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-9tj1iCr99u6QenJM .error-icon{fill:#552222;}#mermaid-svg-9tj1iCr99u6QenJM .error-text{fill:#552222;st…...

编程日记 2025/6/3 17:59:58

Opencv4 c++ 自用笔记 03 滑动条、相机与视频操作

1. 相机与视频操作 1.1 打开视频&#xff0f;相机 OpenCV 中 imread() 只能读取静态图像&#xff0c;若要读取视频文件或摄像头流&#xff0c;需要使用 VideoCapture 类&#xff1a; // 构造函数 cv::VideoCapture::VideoCapture(); cv::VideoCapture…...

编程日记 2025/6/3 15:05:14

LINUX528 重定向

2>&1 我的理解&#xff1a; 2>&1&#xff0c;2stderr错误输出&#xff0c;1stdout输出&#xff0c;stderr一般和stdout是分别输出&#xff08;管道符只传递stdout&#xff0c;据元宝&#xff0c;stderr默认输出到终端&#xff1b;如果重定向符不进行2显示重定向&…...

编程日记 2025/6/3 20:06:06

研华工控机安装Windows10系统,适用UEFI（GPT）格式安装

主要硬件 主板&#xff1a;AIMB-787 、CPU&#xff1a;i5-6500 U盘启动工具&#xff1a;通过网盘分享的文件&#xff1a;rufus-3.20.zip 链接: https://pan.baidu.com/s/1YlFfd-_EhFHCG4sEHBQ8dQ?pwdQT12 提取码: QT12 Win10 22H2 Pro 纯净版系统&#xff1a;通过网盘分享…...

编程日记 2025/6/3 16:29:11

1、树莓派更换软件下载源

树莓派官方系统raspbian自带的是国外的软件源&#xff0c;在国内使用经常会遇到无法下载软件的问题。 以下是把raspbian系统&#xff08;buster版本&#xff09;的下载源改为阿里云软件源的方法。 1、修改sources.list文件 sudo nano /etc/apt/sources.list 将初始化中的代…...

编程日记 2025/6/3 20:56:31

历年中山大学计算机保研上机真题

历年中山大学计算机保研上机真题 2025中山大学计算机保研上机真题 2024中山大学计算机保研上机真题 2023中山大学计算机保研上机真题 在线测评链接&#xff1a;https://pgcode.cn/school 不连续1的子串 题目描述 给定一个数字 n n n&#xff0c;输出长度为 n n n 的 01…...

编程日记 2025/6/3 15:33:35

Python----目标检测（《SSD: Single Shot MultiBox Detector》论文和SSD的原理与网络结构）

一、SSD&#xff1a;单次多框检测器 1.1、基本信息 标题&#xff1a;SSD: Single Shot MultiBox Detector 作者&#xff1a;Wei Liu (UNC Chapel Hill), Dragomir Anguelov (Zoox Inc.), Dumitru Erhan, Christian Szegedy (Google Inc.), Scott Reed (University of Michiga…...

编程日记 2025/6/3 16:10:06

springboot集成websocket给前端推送消息

一般通常情况下&#xff0c;我们都是前端主动朝后端发送请求&#xff0c;那么有没有可能&#xff0c;后端主动给前端推送消息呢&#xff1f;这时候就可以借助websocket来实现。下面给出一个简单的实现样例。 首先创建一个websocketDemo工程&#xff0c;该工程的整体结构如下&a…...

编程日记 2025/6/3 16:20:00

DrissionPage SessionPage模式：轻量级HTTP请求的利器

引言 在Python自动化领域&#xff0c;DrissionPage以其创新的三模式设计脱颖而出。作为专为HTTP请求优化的SessionPage模式&#xff0c;凭借其轻量级架构和高效性能&#xff0c;成为API调用、数据采集等场景的首选方案。本文将深入解析SessionPage的技术特性、核心优势及典型应…...

编程日记 2025/6/4 3:44:32

0527漏洞原理：XSS笔记

理论知识 01 前端基础知识 1.1 HTML基础 定义&#xff1a;HTML&#xff08;超文本标记语言&#xff09;用于描述网页结构。标准结构&#xff1a; 内嵌脚本&#xff1a; <script>JavaScript代码</script>1.4 JavaScript弹窗函数 函数描述alert("文本&quo…...

编程日记 2025/6/3 17:27:23

智能制造之精读——RPA制造行业常见场景【附全文阅读】

RPA 在制造行业应用广泛&#xff0c;为企业带来显著价值&#xff0c;是极具潜力的智能化解决方案。它能节省成本&#xff0c;降低人力与管理成本&#xff1b;提升运营效率&#xff0c;减少人机交互损耗&#xff1b;提高质量&#xff0c;保障流程准确性&#xff1b;还能增强合规…...

编程日记 2025/6/4 2:49:49

spark shuffle的分区支持动态调整，而hive不支持

根据Spark官方文档&#xff0c;Spark Shuffle分区支持动态调整的核心原因在于其架构设计和执行模型的先进性&#xff1a; 1. 自适应查询执行&#xff08;AQE&#xff09;机制 Spark 3.0引入的AQE特性允许在运行时动态优化执行计划&#xff0c;包括Shuffle分区调整&#xff1a…...

编程日记 2025/6/4 2:36:56

网络安全十大漏洞

1️⃣ 失效的访问控制&#xff08;Broken Access Control&#xff09; 核心问题&#xff1a;用户能访问本应被禁止的资源或操作 攻击案例&#xff1a; 修改URL参数&#xff1a;https://shop.com/order?user_id100 → 改为 user_id101 查看他人订单 直接访问管理员页面&#…...

编程日记 2025/6/4 3:28:58

关于uv 工具的使用总结（uv，conda，pip什么关系）

最近要开发MCP 项目&#xff0c;uv工具使用是官方推荐的方式&#xff0c;逐要了解这个uv工具。整体理解如下&#xff1a; 一.uv工具的基本情况 UV 是一个由 Rust 编写的现代化 Python 包管理工具&#xff0c;旨在通过极速性能和一体化功能替代传统工具&#xff08;如 pip、vi…...

编程日记 2025/6/3 16:38:31

深入剖析 Docker 容器化原理与实战应用，开启技术新征程！

文章目录 前言一、为什么 是Docker &#xff1f;二、Docker 容器化原理分析2.1 镜像&#xff08;Image&#xff09;2.2 容器&#xff08;Container&#xff09;2.3 仓库&#xff08;Registry&#xff09; 三、Docker 容器化实践3.1 Docker安装3.2 创建一个 Docker 镜像3.3 运行…...

编程日记 2025/6/4 2:57:51

Xamarin劝退之踩坑笔记

初级代码游戏的专栏介绍与文章目录-CSDN博客 我的github&#xff1a;codetoys&#xff0c;所有代码都将会位于ctfc库中。已经放入库中我会指出在库中的位置。 这些代码大部分以Linux为目标但部分代码是纯C的&#xff0c;可以在任何平台上使用。 源码指引&#xff1a;github源…...

编程日记 2025/6/3 22:23:46

计算机网络(4)——网络层

1.概述 1.1 网络层服务 (1) 网络层为不同主机(Host)之间提供了一种逻辑通信机制 (2)每个主机和路由器都运行网络层协议 发送方&#xff1a;将来自传输层的消息封装到数据报(datagram)中接收方&#xff1a;向传输层交付数据段(segment) 1.2 网络层核心功能 路由选择(routing…...

编程日记 2025/6/3 20:28:01

java 多线程中的volatile关键字作用

文章目录 前置作用一&#xff1a;多线程下&#xff0c;保证可见性作用二&#xff1a;多线程下&#xff0c;禁止指令重排序 前置 保证可见性和保证没有指令重排导致的问题 但是不保证原子性 volatile 常常见到和 static 一起使用&#xff0c;因为 volatile 用在多线程中共享变…...

编程日记 2025/6/3 18:33:03

ESP32基础知识1：项目工程建立和烧录

ESP32基础知识1&#xff1a;项目工程建立和烧录 一、本文内容与前置知识点1. 本文内容2. 前置知识点 二、新建工程1. 工程配置2. 依照模板建立项目 三、硬件烧录1. 硬件准备2. 烧录器和ESP32连接3. 电脑端设置4. 烧录成功演示 四、参考文献 一、本文内容与前置知识点 1. 本文内…...

编程日记 2025/6/3 23:48:24

allWebPlugin中间件VLC专用版之录像功能介绍

背景 VLC控件原有接口是不支持录像的&#xff0c;且libVLC提供的接口库&#xff0c;不能获取录像文件完整名称&#xff08;VLC-3.0.11 录制直播时有的无法保存视频的解决方法 - 1CM - 博客园&#xff09;&#xff1b;因此&#xff0c;非常的不友好。为了能够彻底解决这个问题&a…...

编程日记 2025/6/3 16:36:43

Vim 支持多种编程语言编辑器

软件简介 Vim是Vi编辑器的增强版&#xff0c;它提供了更多的功能和快捷键。Vim是一款自由软件&#xff0c;它是由Bram Moolenaar在1991年创建的。Vim支持多种编程语言&#xff0c;包括C、C、Java、Python、Perl等等。它是一款轻量级的编辑器&#xff0c;可以快速打开和编辑大型…...

编程日记 2025/6/3 21:57:35

解决 IDEA 在运行时中文乱码问题

直接说解决办法 编译 IDEA 所在目录的启动的 .vmoptions 文件&#xff0c;添加以下JVM 参数即可 -Dfile.encodingUTF-8如下图所示&#xff0c;Help > Edit Custom VM Options&#xff0c;随后在编辑框中添加-Dfile.encodingUTF-8 的 JVM 参数...

编程日记 2025/6/3 20:14:49

Diffusion Planner:扩散模型重塑自动驾驶路径规划（ICLR‘25）

1. 概述 2025年2月14日&#xff0c;清华大学AIR智能产业研究院联合毫末智行、中科院自动化所和香港中文大学团队&#xff0c;在ICLR 2025会议上发布了Diffusion Planner——一种创新性的基于Diffusion Transformer的自动驾驶规划模型架构。该系统联合建模周车运动预测与自车行…...

编程日记 2025/6/4 0:51:00

华为OD机试真题——阿里巴巴找黄金宝箱 IV（2025A卷：200分）Java/python/JavaScript/C++/C语言/GO六种最佳实现

2025 A卷 200分 题型 本文涵盖详细的问题分析、解题思路、代码实现、代码详解、测试用例以及综合分析； 并提供Java、python、JavaScript、C++、C语言、GO六种语言的最佳实现方式！ 2025华为OD真题目录+全流程解析/备考攻略/经验分享 华为OD机试真题《阿里巴巴找黄金宝箱 IV》：…...

编程日记 2025/6/4 3:24:02

数据结构：时间复杂度（Time Complexity）和空间复杂度（Space Complexity）

目录 什么是时间复杂度&#xff1f; 如何表示时间复杂度&#xff1f; 为什么需要时间复杂度&#xff1f; 用几个例子理解 怎么分析代码的时间复杂度&#xff1f; 什么是空间复杂度&#xff1f; 举例理解 什么是时间复杂度&#xff1f; 时间复杂度是用来衡量一个算法“…...

编程日记 2025/6/3 22:15:24

CentOS7.9环境离线部署docker和docker-compose的两种方式

目 录 一、yum安装&#xff0c;使用rpm安装包和相关依赖 1.1 准备rpm安装包 1.2 将docker-23.0.4.tar.gz上传至/opt目录下 二、二进制文件方式安装 三、安装docker-compose 一、yum安装&#xff0c;使用rpm安装包和相关依赖 1.1 准备rpm安装包 1&#xff09;在一台与…...

编程日记 2025/6/3 21:44:06