【STM32 + HAL库】倒立摆

一天时间终于做完了这颗倒立摆，能够实现持久站立，但是抗干扰能力很弱。

先上两张未经排版的图（好吧，不能上传GIF）

截止本文写完，该摆最长站立了17分钟。

【硬件部分】

1、倒立摆机械部分由老师提供，由同步轮+同步带带动滑台在光杆上运动；角度反馈部分为安装在倒立摆下端的高精度电位器

2、电机为带编码器的减速直流电机，由TB6612芯片驱动

3、主控板为stm32f103vet6开发板

4、电机使用学生电源提供10V电压驱动，单片机由电脑USB供电。

【使用HAL库构建硬件层代码】

 

1、RCC时钟树配置

在CubeMX中，使用外部晶振提供时钟源，通过锁相环倍频到72MHz提供给外设，ADC模块的时钟不能太高，否则会影响转换精度。详细配置如图

 

2、TIM8编码器模式配置

使用stm32f103高级定时器TIM8的编码器模式，测量电机的转速。

在模式选择窗口，选择通道串联模式，由两个TIM输入通道接入正交编码器的A线和B线。

详细配置窗口中，选择向上计数，不允许自动重装载。编码器模式下，选择在通道1和通道2的上升沿计数。理论上而言，这样会导致计数值是实际值的4倍（但是实际程序中不是）

编码器数值由软件直接读取和重置，不需要重装载，也不需要中断。

注意将GPIO脚设置为上拉输入，部分编码器没有接上拉电阻，需要单片机内部提供。

 

3、ADC1测量电位器阻值

配置ADC1的一通道对电位器阻值进行测量，从而获取倒立摆的倾角。

如图是选择Channel1独立测量、连续测量模式，因为只有一个通道测量，所以扫描测量模式关闭。AD转换同样不需要中断，在软件中直接触发一次转换，之后读取转换值即可。

4、TIM1输出PWM

在模式配置中，选择时钟源为内部时钟源72MHz，选择通道一为PWM输出模式

在配置选项中，我们需要产生频率为1000Hz，占空比可变的PWM。可以选择预分频系数为72-1，预分频之后得到1MHz时钟信号；重装载寄存器为1000-1，得到1000Hz的PWM。选择PWM Mode 1，当计数器值小于某个值时，输出高电平，否则输出低电平（Mode 2）相反。同时配置输出GPIO为推挽输出模式。

【串级PID算法】

部分内容来自个人理解，如果不当，恳请批评指正。

显然这是一个串级PID算法。

第一级PID输入为倒立摆的实际角度和目标角度，输出电机的目标转速。

实际上输入为ADC转化的电位器两端的电压值，目标值为电位器在平衡位置时ADC转换的电压值，输出为目标转速。

第一级PID主要由比例和微分控制，由于下面还有一层PID，所以积分环节要么选择误差过零时将积分项清零，要么选择较小的积分限幅。考虑到倒立摆对平衡性要求较高，误差过零时将积分项清零的方法将使第一级PID的输出在平衡位置发生跳变，所以选择积分限幅，限制幅值较小，但积分参数较大。

第二级PID输入为电机的目标转速和实际转速，输出电机的PWM。

第二级PID主要由PID三项控制构成，原本决定不用I控制，但是在实际调试过程中，由于电机存在一定的死区，在平衡位置附近PWM输出很小，电机不转，严重影响控制品质；可以按固定的PWM值进行死区补偿，但是这样较为死板，考虑到倒立摆的摆动是一个相对慢的过程，所以使用积分项。同样需要积分限幅。

 

【调试过程】

一开始PID调了好久都没有效果，冷静下来才发现方法不对。后来的调试过程如下：

1、先给第一级PID赋较大的P，I和D参数为0

2、利用Arduino的串口绘图器，将目标值与实际值绘图，观察变化趋势，调节第二级参数

3、利用Arduino的串口绘图器，将目标值与实际值绘图，观察变化趋势，调节第一级参数

 

调试过程中的小技巧：

设置当倒立摆角度大于一定角度之后，电机目标速度置0，避免了频繁开关电源。

把倒立摆固定好，如果桌面不平且倒立摆在桌面上移动，那么每次调试时平衡位置对应的电位器的值不同，需要频繁尝试。

 

【主要代码】

pid.h

#ifndef __pid_H_
#define __pid_H_
#include "main.h"
#include "pid.h"

#define TAR_ANGLE 1425    

typedef struct{
		float k_pro;	//±ÈÀý²ÎÊý	
		float k_int;	//»ý·Ö²ÎÊý
		float k_dif;	//΢·Ö²ÎÊý
		long max_int;	//»ý·ÖÏÞ·ù
		long i;	//»ý·Ö
		int tar_value;	//Ä¿±êÖµ
		int true_value; //Õæʵֵ
		int pre_error;	//ÉÏ´ÎÎó²î
		int output_limit_min;
		int output_limit_max;
		int output;
} pid;

void pid_speed_init();
void pid_set_tarspeed(uint32_t tar);
void pid_speed_cal();
void pid_angle_cal();

void pid_angle_init();



#endif /* __pid_H_ */

pid.c

#include "main.h"
#include "pid.h"

#define PID_SPEED_K_PRO 85.0
#define PID_SPEED_K_INT 2.0
#define PID_SPEED_K_DIF -30.0

#define PID_ANGLE_K_PRO 0.60
#define PID_ANGLE_K_INT 0.70
#define PID_ANGLE_K_DIF -0.105



pid pid_speed;
pid pid_angle;

void pid_speed_init(){
	pid_speed.i = 0;
	pid_speed.k_dif = PID_SPEED_K_DIF;
	pid_speed.k_int = PID_SPEED_K_INT;
	pid_speed.k_pro = PID_SPEED_K_PRO;
	pid_speed.max_int = 180;
	pid_speed.tar_value = 0;
	pid_speed.true_value = 0;
	pid_speed.pre_error = 0;
	
	pid_speed.output_limit_min = -999;
	pid_speed.output_limit_max = 999;
	pid_speed.output = 0;
	
}
void pid_angle_init(){
	pid_angle.i = 0;
	pid_angle.k_dif = PID_ANGLE_K_DIF;
	pid_angle.k_int = PID_ANGLE_K_INT;
	pid_angle.k_pro = PID_ANGLE_K_PRO;
	pid_angle.max_int = 60;
	
	pid_angle.tar_value = TAR_ANGLE;
	
	pid_angle.true_value = 0;
	pid_angle.pre_error = 0;
	
	pid_angle.output_limit_min = -65535;
	pid_angle.output_limit_max = 65535;
	pid_angle.output = 0;
	
}

void pid_set_tarspeed(uint32_t tar){
	pid_speed.tar_value = tar;
	
}
extern uint32_t adc_raw;
void pid_angle_cal(){
	int error = pid_angle.tar_value - pid_angle.true_value;
	
	if(pid_angle.true_value < TAR_ANGLE - 250 || pid_angle.true_value > TAR_ANGLE + 250);
	else pid_angle.i += error;
	
	if (pid_angle.i > pid_angle.max_int) pid_angle.i = pid_angle.max_int;
	if (pid_angle.i < -pid_angle.max_int) pid_angle.i = -pid_angle.max_int;
		
	//printf("T:%d\n", pid_speed.tar_value);
	//printf("E:%d\n", error);
	int delta_error = error - pid_angle.pre_error;
	pid_angle.pre_error = error;
	
	pid_angle.output = pid_angle.k_pro * error + pid_angle.k_int * pid_angle.i + pid_angle.k_dif * delta_error;
	printf("P:%d I:%d D:%d\n", (int)(pid_angle.k_pro * error), (int)(pid_angle.k_int * pid_angle.i), (int)(pid_angle.k_dif * delta_error));
	if (pid_angle.output > pid_angle.output_limit_max) pid_angle.output = pid_angle.output_limit_max;
	else if (pid_angle.output < pid_angle.output_limit_min) pid_angle.output = pid_angle.output_limit_min;
	//printf("AO:%d\n", pid_angle.output);
}



void pid_speed_cal(){
	int error = pid_speed.tar_value - pid_speed.true_value;
	
	if(pid_angle.true_value < TAR_ANGLE - 250 || pid_angle.true_value > TAR_ANGLE + 250);
	else pid_speed.i += error;
	
	if (pid_speed.i > pid_speed.max_int) pid_speed.i = pid_speed.max_int;
	else if (pid_speed.i < -pid_speed.max_int) pid_speed.i = -pid_speed.max_int;
	
	

	int delta_error = error - pid_speed.pre_error;
	pid_speed.pre_error = error;
	
	pid_speed.output = pid_speed.k_pro * error + pid_speed.k_int * pid_speed.i + pid_speed.k_dif * delta_error;
	

	
	//printf("P:%d ", (int)(pid_speed.k_pro * error));
	//printf("P:%d\n",(int)(pid_speed.k_int * pid_speed.i));
	if (pid_speed.output > pid_speed.output_limit_max) pid_speed.output = pid_speed.output_limit_max;
	else if (pid_speed.output < pid_speed.output_limit_min) pid_speed.output = pid_speed.output_limit_min;
	
	
}

主代码main.c

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * 
© Copyright (c) 2019 STMicroelectronics.
  * All rights reserved.
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

#include "pid.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 */
int a = 0;
uint8_t aTxStartMessages[] = "\r\n******UART commucition using IT******\r\nPlease enter 10 characters:\r\n";
uint8_t aRxBuffer[1];
extern pid pid_speed;
extern pid pid_angle;
	
/* 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 */
int fputc(int c, FILE *stream)    //??fputc??
{

    HAL_UART_Transmit(&huart1, (unsigned char *)&c, 1, 1000);   
    return 1;
}

uint32_t adc_raw = 0;
uint16_t timer2_counter = 0;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
		timer2_counter ++;
		if (timer2_counter == 50){
			
			timer2_counter = 0;
			
			pid_angle.true_value = adc_raw;
			//printf("%d %d\n", pid_angle.true_value, pid_angle.tar_value);
			pid_angle_cal();
			
			pid_set_tarspeed(pid_angle.output);
		}
		if (timer2_counter % 10 == 0){
			pid_speed.true_value = 30000 - __HAL_TIM_GET_COUNTER(&htim8);
			__HAL_TIM_SET_COUNTER(&htim8, 30000);
			pid_speed_cal();
			
			if(pid_angle.true_value < TAR_ANGLE - 250 || pid_angle.true_value > TAR_ANGLE + 250)
				pid_speed.output = 0;
			//printf("Speed:%d\n", pid_speed.true_value);
			if (pid_speed.output < 0){
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -pid_speed.output);
				
			}
			
			else if (pid_speed.output > 0){ 
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pid_speed.output);
			}
			else if (pid_speed.output == 0){ 
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);
				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
			}
		}
	
}
uint8_t msg[1];

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){
		printf("%d\n", msg[0]);
		if(msg[0] == '1'){
			pid_angle.k_pro += 0.05;
			printf("Kp->:%.2f\n", pid_angle.k_pro);
		}
		else if (msg[0] == '2'){
			pid_angle.k_pro -= 0.05;
			printf("Kp->:%.2f\n", pid_angle.k_pro);
		}
		else if(msg[0] == '3'){
			pid_angle.k_int += 0.05;
			printf("Kd->:%.2f\n", pid_angle.k_int);
		}
		else if (msg[0] == '4'){
			pid_angle.k_int -= 0.05;
			printf("Kd->:%.2f\n", pid_angle.k_int);
		}
		
		else if(msg[0] == '5'){
			pid_angle.k_dif += 0.005;
			printf("Kp->:%.3f\n", pid_angle.k_dif);
		}
		else if (msg[0] == '6'){
			pid_angle.k_dif -= 0.005;
			printf("Kp->:%.3f\n", pid_angle.k_dif);
		}
		
		else if(msg[0] == '7'){
			pid_angle.k_pro += 0.05;
			printf("Kd->:%.2f\n", pid_angle.k_pro);
		}
		else if(msg[0] == '8'){
			pid_angle.k_pro -= 0.05;
			printf("Kd->:%.2f\n", pid_angle.k_pro);
		}
		else if(msg[0] == '9'){
			pid_angle.k_dif += 0.005;
			printf("Kd->:%.3f\n", pid_angle.k_dif);
		}
		else if(msg[0] == '0'){
			pid_angle.k_dif -= 0.005;
			printf("Kd->:%.3f\n", pid_angle.k_dif);
		}
		
		HAL_UART_Receive_IT(&huart1, (uint8_t *)&msg, 1);
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
	

	uint8_t adc = 0;
	
  /* 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_TIM1_Init();
  MX_USART1_UART_Init();
  MX_ADC1_Init();
  MX_TIM2_Init();
  MX_TIM8_Init();
  /* USER CODE BEGIN 2 */
	__HAL_TIM_SET_COUNTER(&htim8, 30000);
	HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
	pid_speed_init();
	pid_angle_init();
	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
	
	HAL_UART_Receive_IT(&huart1, (uint8_t *)&msg, 1);
	HAL_TIM_Base_Start_IT(&htim1); 
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	
	
	
	HAL_UART_Receive_IT(&huart1,(uint8_t*)aRxBuffer,1);
	HAL_UART_Transmit(&huart1,aTxStartMessages,sizeof(aTxStartMessages),100);
	HAL_TIM_Base_Start_IT(&htim2);
	
	
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
//		HAL_UART_Transmit(&huart1, (uint8_t *)&msg, 3, 1000);
//		HAL_Delay(500);
//		HAL_GPIO_WritePin(LED_Pin1_GPIO_Port, LED_Pin1_Pin, GPIO_PIN_SET);
//		HAL_Delay(500);
			HAL_ADC_Start(&hadc1);
      HAL_ADC_PollForConversion(&hadc1, 500);
			adc_raw = HAL_ADC_GetValue(&hadc1);
			//adc = adc_raw / 0xFFFFFF;
			//HAL_UART_Transmit(&huart1, &adc, 1, 100);
//		printf("Data: %d\n", adc_raw);
//		printf("End of while");
		
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB busses clocks 
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
  PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV8;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

由于一遍一遍尝试PID参数较为麻烦，程序中使用串口接收电脑端的字符信息，进行PID参数的加减，较为方便的完成了调试。

 

【进一步优化】

倒立摆能够实现较为持久的直立，但是抗干扰能力较弱，在弱干扰情况下会震荡然后摔倒。下一步考虑能否在由倾角较大返回平衡状态的过程中，对PID控制器进行优化。

【下载链接】

链接：https://pan.baidu.com/s/1qwym3qKKoyAnxOy4WM-fpQ 
提取码：drc5 

 

个人见解，如果不当，恳请各位朋友批评指正。