在之前TB6612驱动中，主要实现了固定速度下小车的循迹，在车身偏转使用默认的速度进行纠偏，使车身恢复正常状态。接线图和之前TB6612一样STM32Cubemx TB6612直流电机驱动-CSDN博客。
今天要做的是，两路循迹判断车身偏的情况下，使用PID进行差速调节最后将误差分别给到左右电机，实现自动调速。
实现两路循迹PID控制差速的关键在于根据循迹模块的读数来计算出控制差速的误差，然后再利用PID控制算法来调节左右两侧驱动轮的转速。
一、CubeMX配置
SYS
RCC 选则外部高速晶振
GPIO 配置
T6612PWM配置
时钟树配置
freertos配置
二、Keil代码配置
在freertos.c添加代码
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "tim.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
uint16_t base_speed=120;
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define Kp 15 // 比例参数
#define Ki 0.1 // 积分参数
#define Kd 0.05 // 微分参数
/* USER CODE END PD *//* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void motorControlL(uint8_t direction, uint16_t speed) {//左电机驱动
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
/* 根据方向设置 IN1 和 IN2 引脚 */
HAL_GPIO_WritePin(GPIOA, BIN1_Pin, (direction & 0x01));
HAL_GPIO_WritePin(GPIOA, BIN2_Pin, ((direction & 0x01) ^ 0x01));
/* 设置 PWM 引脚以控制速度 */
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, speed);
}
void motorControlR(uint8_t direction, uint16_t speed) {//右电机驱动
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
/* 根据方向设置 AIN1 和 IN2 引脚 */
HAL_GPIO_WritePin(GPIOA, AIN2_Pin, (direction & 0x01));
HAL_GPIO_WritePin(GPIOA, AIN1_Pin, ((direction & 0x01) ^ 0x01));
/* 设置 PWM 引脚以控制速度 */
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed);
}
// 读取左右循迹传感器数据
int read_left_trace_sensor() {
// 返回左侧循迹传感器的读数
uint16_t a=0;
if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_8)==1){a=10;}
else if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_8)==0){a=6;}
return a;
}
int read_right_trace_sensor() {
// 返回右侧循迹传感器的读数
uint16_t a=0;
if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_9)==1){a=10;}
else if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_9)==0){a=6;}
return a;
}
// 设置左右电机转速
void set_motor_speed(float left_speed, float right_speed) {
motorControlL(1, left_speed); //第一个参数 0反向转 1正向转 第二个参数控制小车速度
motorControlR(1, right_speed); // 控制左右电机的转速
}
// PID控制差速
void pid_control_diff() {
int left_sensor_reading = read_left_trace_sensor();
int right_sensor_reading = read_right_trace_sensor();
// 计算误差
float error = left_sensor_reading - right_sensor_reading;
// 计算PID输出
float output = Kp * error;
// 根据输出值调整左右电机的转速
float left_speed = base_speed + output;
float right_speed = base_speed - output;
// 将PID输出应用到电机控制
set_motor_speed(right_speed, left_speed);
}
void BEEP(){
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET);
osDelay(500);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
osDelay(500);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET);
osDelay(500);
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
}
/* USER CODE END FunctionPrototypes */默认任务
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
/* USER CODE BEGIN StartDefaultTask */
/* Infinite loop */
uint8_t start=0;
for(;;)
{
HAL_GPIO_TogglePin(GPIOC,GPIO_PIN_13);
osDelay(300);
}
/* USER CODE END StartDefaultTask */
}主任务
/* USER CODE END Header_MainTask */
void MainTask(void const * argument)
{
/* USER CODE BEGIN MainTask */
/* Infinite loop */
for(;;)
{
// 执行PID控制差速
pid_control_diff();
if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_8)==1 && HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_9)==1){//stop
motorControlL(0, 0);
motorControlR(0, 0);
BEEP();
}
}
/* USER CODE END MainTask */
}链接: https://pan.baidu.com/s/1Pz_WwB-WyBlk_gm2Gab3xg?pwd=9yjh 提取码: 9yjh
