STM32CubeMX PID差速循迹小车

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