基于STM32CubeIDE+PID调参做的动量轮平衡自行车（五）

[复制链接]

STMCU小助手发布时间：2023-2-9 17:53

文章
文章封面:
文章简介:	基于STM32CubeIDE+PID调参做的动量轮平衡自行车（五）

本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用采用STM32F103C8T6作为主控单元、MPU-6050作为位姿采集单元、0.96寸OLED显示位姿、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线通讯和PID调试、舵机用于控制行驶方向和支撑小车。

为了方便进行PID参数的调试，我写了一个微信小程序（蓝牙小车调试助手），操作参考视频里的介绍。PID我也不是很了解，只是照着葫芦画瓢，在定时器回调函数中定时进行PID计算，控制小车的平衡，详细到代码如下：

Ps：PID真是和孽畜一般的物件，以后不会再玩了，太难调试了！

/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2022 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mpu6050.h"
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
//定义变量接受微信小程序发送的数据
uint8_t startFlag = 0;
uint8_t displayFlag = 0;
uint8_t driveSpeed = 0;
int16_t driveDirection = -1;
float tempVerticalKp,tempVerticalKi,tempVerticalKd;
float tempVelocityKp,tempVelocityKi,tempVelocityKd;
//串口通讯接受缓存区
uint8_t bufferLen =100;
uint8_t buffer[100];
uint8_t bufferTr[20] = "";
extern float Pitch_Kalman;
extern float GyroY;
float GyroY_Lowout = 0;
int16_t counterA = 0;
int16_t brushless = 0;
//Vertical Parameter
float Vertical_Kp = 800,Vertical_Kd = 30;
float Angle = 0;////自平衡角度
float Angle_mid = 1;//机械中值
//Velocity Parameter
float Velocity_Kp = 0.16, Velocity_Ki=0.00016;
int16_t Encoder_Err=0, Encoder_Sum=0;
int16_t Encoder_lowout=0;
float a=0.7; //一阶互补滤波系数
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void PIDControl(){
/****************************************************************
速度环PI控制器：
输出：期望角度，实测角度，实测角速度
输出：PWM输出
****************************************************************/
//1.计算速度偏差 Encoder_Err
Encoder_Err = counterA;
counterA = 0;
//2.对速度偏差进行低通滤波 low_out = (1-a)*Ek+a*low_out_last;
Encoder_lowout = (1-a)*Encoder_Err+a*Encoder_lowout;
//3.对速度偏差,积分出位移
Encoder_Sum += Encoder_lowout;
if(Encoder_Sum>10000)
Encoder_Sum = 10000;
else if(Encoder_Sum<-10000)
Encoder_Sum = -10000;
//4.速度环控制输出——期望角度
Angle = Velocity_Kp*Encoder_lowout + Velocity_Ki*Encoder_Sum;
/****************************************************************
直立环PD控制器：
输入出：期望角度，实测角度，实测角速度
输出：PWM输出
****************************************************************/
GyroY_Lowout = 0.3*GyroY + 0.7*GyroY_Lowout;
brushless =Vertical_Kp*(Pitch_Kalman - Angle_mid + Angle) + Vertical_Kd * GyroY_Lowout;
//brushless = Vertical_Kp*(Pitch_Kalman - Angle_mid + Angle) + Vertical_Kd * GyroY;
}
/**
* @brief EXTI line detection callbacks.
* @param GPIO_Pin: Specifies the pins connected EXTI line
* @retval None
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
//Define external interrupt function
if(HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_4) == GPIO_PIN_RESET)
counterA +=1;
else
counterA -=1;
}
/**
* @brief Period elapsed callback in non-blocking mode
* @param htim TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance==TIM4)
{
if(startFlag||displayFlag)
{
//Timer 4 interrupt function
GetAngle();//get Pitch_Kalman and GyroY
PIDControl();
if(brushless > 3600){
brushless = 3600;
}
else if(brushless < -3600){
brushless = -3600;
}
if(brushless>=0){
HAL_GPIO_WritePin(GPIO_DIR_GPIO_Port, GPIO_DIR_Pin, SET);
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1,brushless);
}
else{
HAL_GPIO_WritePin(GPIO_DIR_GPIO_Port, GPIO_DIR_Pin, RESET);
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1,abs(brushless));
}
}
}
}
//get system time us
__STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
{
return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
}
static uint32_t getCurrentMicros(void)
{
/* Ensure COUNTFLAG is reset by reading SysTick control and status register */
GXT_SYSTICK_IsActiveCounterFlag();
uint32_t m = HAL_GetTick();
const uint32_t tms = SysTick->LOAD + 1;
__IO uint32_t u = tms - SysTick->VAL;
if (GXT_SYSTICK_IsActiveCounterFlag()) {
m = HAL_GetTick();
u = tms - SysTick->VAL;
}
return (m * 1000 + (u * 1000) / tms);
}
//
uint32_t micros(void)
{
return getCurrentMicros();
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
/* USER CODE BEGIN 2 */
//MPU-6050 initialization
while (MPU_Init()){
//printf("MPU_Init_Fail...");
}
//Starts the TIM2 PWM.
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); //direction servo
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); //support servo
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); //direct motor1
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); //direct motor1
//Set Servo duty cycle 500~2500
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,1500);
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,500);
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,0);
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,0);
//Starts the TIM3 PWM.
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1); //brushless motor
//Set Brushless Motor duty cycle 0~3600
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1,0);
HAL_GPIO_WritePin(GPIO_EN_GPIO_Port, GPIO_EN_Pin, SET);//brushless motor Enable
//Starts the TIM4 Base generation.
HAL_TIM_Base_Start_IT(&htim4); // PID/20ms
HAL_UART_Receive_DMA(&huart1, buffer, bufferLen);
__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);//开启空闲中断
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
if(startFlag||displayFlag){//遥控或调试
/* 收起支撑 */
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,1500);
/* 根据上位机命令，调整运用方向和速度 */
if (startFlag) {
if (driveDirection == -1)//停止
{
//__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,1500);
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,0);//N20停止
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,0);
HAL_Delay(5);
}
else{
if(driveDirection>=180){//前进
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,(uint16_t)(1500-500*(driveDirection-270)/90));
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,100*driveSpeed);//前进
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,0);
}
else{//后退
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,(uint16_t)(1500-500*(driveDirection-90)/90));
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,0);//N20后退
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,100*driveSpeed);
}
}
}
else{
// __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,1500);//direction servo
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,500);//放下支撑舵机
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,0);//N20停止
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,0);
HAL_Delay(5);
}
}
else {//停止
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_3,0);//N20电机停止
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_4,0);
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,500);//舵机放下支撑
//设置平衡角度，延时一段时间后关闭无刷电机
HAL_Delay(5);
}
/* 串口通讯，发送数据至上位机 */
if (displayFlag) {
switch ((int16_t)tempVerticalKi) {
case 1:
sprintf(bufferTr, "%.1f", (double)brushless);
HAL_UART_Transmit_DMA(&huart1, bufferTr, strlen(bufferTr));// send brushless
break;
case 2:
sprintf(bufferTr, "%.1f", (double)GyroY_Lowout);
HAL_UART_Transmit_DMA(&huart1, bufferTr, strlen(bufferTr));// send GyroY_Lowout
break;
case 3:
sprintf(bufferTr, "%.1f", (double)Encoder_Err);
HAL_UART_Transmit_DMA(&huart1, bufferTr, strlen(bufferTr));// send Encoder_Err
break;
case 4:
sprintf(bufferTr, "%.1f", (double)Encoder_Sum);
HAL_UART_Transmit_DMA(&huart1, bufferTr, strlen(bufferTr));// send Encoder_Sum
break;
default:
sprintf(bufferTr, "%.1f", Pitch_Kalman);
HAL_UART_Transmit_DMA(&huart1, bufferTr, strlen(bufferTr));// send Pitch
break;
}
}
HAL_Delay(20);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}