在之前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
