一天时间终于做完了这颗倒立摆,能够实现持久站立,但是抗干扰能力很弱。
先上两张未经排版的图(好吧,不能上传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
*
* <h2><center>© Copyright (c) 2019 STMicroelectronics.
* All rights reserved.</center></h2>
*
* 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
个人见解,如果不当,恳请各位朋友批评指正。
版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/37135.html