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

前言   --末尾右总体的.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方向确认及初始化函数

这里主要是让引脚定义规范，基本上默认1就是正转

然后还有初始化和停车的函数

/*** @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 "hand.h"
#include "tim.h"
#include "wtkj.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编号轮子依次正转
然后
左转
右转
正右转
正左转
后退
前进
的方式运动，如果错误请重新矫正
*/

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.pswp.cn/diannao/85324.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

文档核心结构优化（程序C++...）

文档核心结构优化（程序C++...）

文档核心结构优化 一、文档核心结构优化二、C关键特性详解框架2.1 从C到C的范式迁移 三、深度代码解析模板3.1 现代C特性分层解析 四、C vs C 关键差异矩阵五、交互式文档设计策略5.1 三维学习路径5.2 代码缺陷互动区 六、现代C特性演进图七、性能优化可视化呈现&#xff08;深…
阅读更多...
PyTorch ——torchvision数据集使用

PyTorch ——torchvision数据集使用

如果下载的很慢&#xff0c;可以试试下面这个
阅读更多...
纯前端实现图片伪3D视差效果

纯前端实现图片伪3D视差效果

作者&#xff1a;vivo 互联网前端团队- Su Ning 本文通过depth-anything获取图片的深度图&#xff0c;同时基于pixi.js&#xff0c;通过着色器编程&#xff0c;实现了通过深度图驱动的伪3D效果。该方案支持鼠标/手势与手机陀螺仪双模式交互&#xff0c;在保证性能的同时&#x…
阅读更多...
英语写作中“专注于”focus on、concentrate的用法

英语写作中“专注于”focus on、concentrate的用法

Focus on在论文写作中常用&#xff0c;指出研究点&#xff0c;例如&#xff1a; There are three approaches to achieving ID authentication. Our study will focus on ……&#xff08;有三种途径实现身份认证&#xff0c;我们的研究专注于……&#xff09; concentrate &…
阅读更多...
go环境配置

go环境配置

下载对应版本的 go 版本 https://go.dev/dl/ 配置 vim ~/.zshrc export GOROOT/usr/local/go export PATH$PATH:$GOROOT/binsource ~/.zshrc >>>>>> go versiongoland 配置&#xff1a; &#x1f50d; 一、什么是GOPATH&#xff1f; GOPATH 是旧的项目结…
阅读更多...
AI Agent智能体：底层逻辑、原理与大模型关系深度解析·优雅草卓伊凡

AI Agent智能体：底层逻辑、原理与大模型关系深度解析·优雅草卓伊凡

AI Agent智能体&#xff1a;底层逻辑、原理与大模型关系深度解析优雅草卓伊凡 一、AI Agent的底层架构与核心原理 1.1 AI Agent的基本构成要素 AI Agent&#xff08;人工智能代理&#xff09;是一种能够感知环境、自主决策并执行行动的智能系统。其核心架构包含以下关键组件…
阅读更多...
【手搓一个原生全局loading组件解决页面闪烁问题】

【手搓一个原生全局loading组件解决页面闪烁问题】

页面闪烁效果1 页面闪烁效果2 封装一个全局loading组件 class GlobalLoading extends HTMLElement {constructor() {super();this.attachShadow({ mode: open });}connectedCallback() {this.render();this.init();}render() {this.shadowRoot.innerHTML <style>.load…
阅读更多...
unix/linux source 命令，其高级使用

unix/linux source 命令，其高级使用

就像在物理学中，掌握了基本定律后，我们可以开始研究更复杂的系统和现象，source 的高级用法也是建立在对其基本行为深刻理解之上的。 让我们一起探索 source 的高级应用领域： 1. 条件化加载 (Conditional Sourcing) 根据某些条件来决定是否 source 一个文件，或者 source…
阅读更多...
DexGarmentLab 论文翻译

DexGarmentLab 论文翻译

单个 专家 演示 装扮 15 任务 场景 2500+ 服装 手套 棒球帽 裤子 围巾 碗 帽子 上衣 外套 服装-手部交互 捕捉 摇篮 夹紧 平滑 任务 ...... 投掷 悬挂 折叠 ... 多样化位置 ... 多样化 变形 ... 多样化服装形状 类别级 一般化 类别级（有或没有变形） 服装具有相同结构 变形 生…
阅读更多...
WPF-Prism学习笔记之 “导航功能和依赖注入“

WPF-Prism学习笔记之 “导航功能和依赖注入“

新建空白模板(Prism) 新建好后会有自动创建ViewModels和Views 在"MainWindow.xaml"文件里面标题去绑定了一个属性"Title"&#xff0c;而"MainWindowViewModel.cs"里面继承一个非常重要的"BindbleBase"(prism框架里面非常重要的)。所以…
阅读更多...
《C++初阶之入门基础》【C++的前世今生】

《C++初阶之入门基础》【C++的前世今生】

【C的前世今生】目录 前言&#xff1a;---------------起源---------------一、历史背景二、横空出世---------------发展---------------三、标准立世C98&#xff1a;首个国际标准版本C03&#xff1a;小修订版本 四、现代进化C11&#xff1a;现代C的开端C14&#xff1a;对C11的…
阅读更多...
YOLOv5-入门篇笔记

YOLOv5-入门篇笔记

1.创建环境 conda create -n yolvo5 python3.8 去pytorch.org下载1.8.2的版本。 pip --default-timeout1688 install torch1.8.2 torchvision0.9.2 torchaudio0.8.2 --extra-index-url https://download.pytorch.org/whl/lts/1.8/cu111 github上下载yolov5的zip pip --def…
阅读更多...
【PostgreSQL 03】PostGIS空间数据深度实战：从地图服务到智慧城市

【PostgreSQL 03】PostGIS空间数据深度实战：从地图服务到智慧城市

PostGIS空间数据深度实战&#xff1a;从地图服务到智慧城市 关键词 PostGIS, 空间数据库, 地理信息系统, GIS, 空间查询, 地理分析, 位置服务, 智慧城市, 空间索引, 坐标系统 摘要 PostGIS是PostgreSQL的空间数据扩展&#xff0c;它将普通的关系数据库转变为强大的地理信息系统…
阅读更多...
科技修真的解决方案

科技修真的解决方案

“科技修真”是一个结合现代科技与修真&#xff08;玄幻&#xff09;元素的创新概念&#xff0c;通常出现在科幻或玄幻文学作品中&#xff0c;但也可能指代现实中的科技与传统文化、超自然理念的融合探索。以下是几种可能的“科技修真”方案&#xff0c;涵盖技术实现、文化融合…
阅读更多...
STM32的HAL编码流程总结（上部）

STM32的HAL编码流程总结（上部）

目录 一、GPIO二、中断系统三、USART串口通信四、I2C通信五、定时器 一、GPIO 1.选择调试类型 在SYS中Debug选择Serial Wire模式 2.选择时钟源 在RCC中将HSE和LSH都选择为内部晶振 3.时钟树配置 4.GPIO配置 在芯片图上选择开启的引脚和其功能 配置引脚的各自属性 5.工…
阅读更多...
java直接获取MyBatis将要执行的动态sql命令（不是拦截器方式）

java直接获取MyBatis将要执行的动态sql命令（不是拦截器方式）

目录 前言 一. 准备数据 1. 传输过来的json条件数据 2. mybatis 配置的动态sql 3. 想要的最终会执行的sql并返回给页面展示 二. 实现方式 三. 最终代码 前言 1.在平常开发过程中，MyBatis使用时非常多的，一般情况下我们只需要在控制台看看MyBatis输出的日志，要不就是实…
阅读更多...
机器学习算法-决策树

机器学习算法-决策树

今天我们用一个 「相亲决策」 的例子来讲解决策树算法&#xff0c;保证你轻松理解原理和实现&#xff01; &#x1f333; 决策树是什么&#xff1f; 决策树就像玩 「20个问题」猜谜游戏&#xff1a; 你心里想一个东西&#xff08;比如「苹果」&#xff09; 朋友通过一系列问题…
阅读更多...
2025——》VSCode Windows 最新安装指南/VSCode安装完成后如何验证是否成功？2025最新VSCode安装配置全攻略

2025——》VSCode Windows 最新安装指南/VSCode安装完成后如何验证是否成功？2025最新VSCode安装配置全攻略

1.VSCode Windows 最新安装指南： 以下是 2025 年 Windows 系统下安装 Visual Studio Code（VSCode）的最新指南，结合官方文档与实际操作经验整理而成： 一、下载官方安装包： 1.访问官网： 打开浏览器，进入 VSCode 官方下载页面https://code.visualstudio.com/Download 2…
阅读更多...
【Elasticsearch】suggest

【Elasticsearch】suggest

在Elasticsearch中&#xff0c;suggest 是一个非常强大的功能&#xff0c;用于实现自动补全、拼写纠错和模糊搜索等功能。它可以帮助用户更快地找到他们想要的内容&#xff0c;同时提升搜索体验。以下是关于 suggest 的详细使用方法和常见场景。 1\. Suggest 的基本概念 sugges…
阅读更多...
[SAP] 如何查询当前屏幕的Tcode?

[SAP] 如何查询当前屏幕的Tcode?

事务代码Tcode是SAP中到达特定屏幕的快捷路径 如何查询以下屏幕的事务码Tcode&#xff1f; 要浏览当前所使用的屏幕的事务码&#xff0c;可以选择System | Status 这里的事务代码是[VA22]&#xff0c;它是Change Quotation的事务代码
阅读更多...
最新文章

Copyright @ 2022~2023