devbox
在线问答5
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

超详细！一文搞定PID！嵌入式STM32-PID位置环和速度环_速度环pid

作者：在线问答5 | 2024-08-03 19:31:52

赞

踩

速度环pid

本文目录


  

一、知识点

1. PID是什么？

  在PID控制中，P、I、D分别代表比例（Proportional）、积分（Integral）、微分（Derivative）三个部分。它们是PID控制器中的三个调节参数，用于调节控制系统的输出，以使系统的反馈与期望值更加接近。

  P（比例）部分：根据当前偏差的大小来调节输出。当偏差较大时，P部分的作用就越强烈，输出的变化也就越大。P控制项对应于系统的当前状态，它的作用是减小系统对设定值的超调和稳定时间。

  I（积分）部分：对偏差的积累进行调节。它的作用是消除稳态误差，使系统更快地达到稳定状态。I控制项对应于系统过去的行为，它的作用是减小系统对外部干扰的影响。

  D（微分）部分：根据偏差变化的速度来调节输出。它的作用是预测系统未来的行为，以减小系统的振荡和过冲现象，提高系统的响应速度和稳定性。

  综合来说，PID控制器通过比例、积分、微分三个部分的组合来调节系统的输出，以实现对系统的精确控制。

2. 积分限幅–用于限制无限累加的积分项

  因为积分系数的Ki是与累计误差相乘的，所以效果是累加，随着时间的推移，积分项的值会升到很高，积分本来的作用是用来减小静态误差，但积分项过大会引起过大的震荡，所以我们可以加一个判断函数if，当积分项的值达到一定值后，就让积分项保持这个值，避免引起更大的震荡。
积分限幅的最大值，要根据经验实际多调试调试。

//为了防止积分项过度累积，引入积分项的限幅是一种常见的做法。
//限制积分项的幅值可以防止积分项过度增加，从而限制了系统的累积误差。这样可以避免系统过度响应或者不稳定。
float abs_limit(float value, float ABS_MAX)   //积分限幅，设置最大值。
{
	if(value > ABS_MAX)
		value = ABS_MAX;

	if(value< -ABS_MAX)
		value = -ABS_MAX;
	return value;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

3. 输出值限幅–用于任何pid的输出

这个需要查看产生pwm的定时器的计数周期初值设定。如Motor_PWM_Init(7200-1,0);，则outputmax就不能大于7200。

  //限制输出最大值，防止出现突发意外。输出outputmax的最大值
	if(pid->output > pid->outputmax )  pid->output = pid->outputmax; 
	if(pid->output < - pid->outputmax )  pid->output = -pid->outputmax

  • 1
  • 2
  • 3

4. PID工程

在这里插入图片描述

（1）定时器1（产生pwm）
tim1.c

#include "tim1.h"

void Motor_PWM_Init(u16 arr,u16 psc)
{		 		
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);// 
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);  //使能GPIO外设时钟使能
 	
   //设置该引脚为复用输出功能,输出TIM1 CH1 CH4的PWM脉冲波形
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11; //TIM_CH1 //TIM_CH4
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
	
	TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值	 
	TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
 
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式1
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStructure.TIM_Pulse = 0;                            //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;     //输出极性:TIM输出比较极性高
	TIM_OC4Init(TIM1, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx

    TIM_CtrlPWMOutputs(TIM1,ENABLE);	//MOE 主输出使能	

	TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);  //CH4预装载使能	 
	
	TIM_ARRPreloadConfig(TIM1, ENABLE); //使能TIMx在ARR上的预装载寄存器
	
	TIM_Cmd(TIM1, ENABLE);  //使能TIM1
} 



  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39

tim1.h

#ifndef __TIM1_H
#define __TIM1_H

#include <sys.h>	 
#define PWMB   TIM1->CCR4  //PA11
void Motor_PWM_Init(u16 arr,u16 psc);

#endif


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

（2）定时器2（定时）

#include "tim2.h"
#include "led.h"
#include "usart.h"
#include "sys.h"

void MotorControl(void)
{
 Encoder_Posion = Read_Position();//1.获取定时器3的编码器数值
 Speed=PosionPID_realize(&PosionPID,Encoder_Posion);//2.输入位置式PID计算
 Set_Pwm(Speed);  //3.PWM输出给电机
//指令/通道/发送数据/个数
 set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Posion, 1);   /*4.给上位机通道2发送实际的电机速度值,详情看下面内容*/
}


void Time2_Init(u16 arr,u16 psc)
{
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	TIM_InternalClockConfig(TIM2);
	
	
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = arr; //电机PWM频率要和定时器采样频率一致
	TIM_TimeBaseInitStructure.TIM_Prescaler = psc;
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
	
	TIM_ClearFlag(TIM2, TIM_FLAG_Update);
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	NVIC_Init(&NVIC_InitStructure);
	
	TIM_Cmd(TIM2, ENABLE);
}

void TIM2_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
	    MotorControl();
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54

（3）定时器4（编码器）

#include "stm32f10x.h"                  // Device header

void Encoder_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_ICInitTypeDef TIM_ICInitStructure;
	
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
		

	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 65536 - 1;		//ARR
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1 - 1;		//PSC
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);
	
	TIM_ICStructInit(&TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
	TIM_ICInitStructure.TIM_ICFilter = 0xF;
	
	TIM_ICInit(TIM4, &TIM_ICInitStructure);
	TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
	TIM_ICInitStructure.TIM_ICFilter = 0xF;
	
	TIM_ICInit(TIM4, &TIM_ICInitStructure);
	/*TI1和TI2都计数，上升沿计数*/
	TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	
	TIM_Cmd(TIM4, ENABLE);
}

int16_t Read_Position(void)
{
	int16_t Temp;
	Temp = TIM_GetCounter(TIM4);  //获取定时器计数值
	TIM_SetCounter(TIM4, 0);  
	return Temp;
}



  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48

（4）串口1
usart.c

#include "sys.h"
#include "usart.h"	  

#if SYSTEM_SUPPORT_OS
#include "includes.h"					//ucos 使用	  
#endif
  
#if 1
#pragma import(__use_no_semihosting)             
//标准库需要的支持函数                 
struct __FILE 
{ 
	int handle; 

}; 

FILE __stdout;       
//定义_sys_exit()以避免使用半主机模式    
void _sys_exit(int x) 
{ 
	x = x; 
} 
//重定义fputc函数 
int fputc(int ch, FILE *f)
{      
	while((USART1->SR&0X40)==0);//循环发送,直到发送完毕   
    USART1->DR = (u8) ch;      
	return ch;
}
#endif 

 
//串口1中断服务程序
//注意,读取USARTx->SR能避免莫名其妙的错误   	
u8 USART_RX_BUF[USART_REC_LEN];     //接收缓冲,最大USART_REC_LEN个字节.
//接收状态
//bit15，	接收完成标志
//bit14，	接收到0x0d
//bit13~0，	接收到的有效字节数目
u16 USART_RX_STA=0;       //接收状态标记	  
  
void uart_init(u32 bound){
  //GPIO端口设置
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA, ENABLE);	//使能USART1，GPIOA时钟
	
	//USART1_TX   GPIOA.9
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //PA.9
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	//复用推挽输出
	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.9
	
	//USART1_RX	  GPIOA.10初始化
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA10
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10  
	
	//Usart1 NVIC 配置
	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3 ;//抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;		//子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	//根据指定的参数初始化VIC寄存器
	
	//USART 初始化设置
	
	USART_InitStructure.USART_BaudRate = bound;//串口波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
	
	USART_Init(USART1, &USART_InitStructure); //初始化串口1
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启串口接受中断
	USART_Cmd(USART1, ENABLE);                    //使能串口1 

}


void USART1_IRQHandler(void)//串口中断服务函数
{
	 u8 Res;
	if(USART_GetITStatus(USART1, USART_IT_RXNE)== SET ) //产生了接收中断
	{
		USART_ClearITPendingBit(USART1,USART_IT_RXNE);  	//清除接收中断标志位
		Res=USART_ReceiveData(USART1);
		protocol_data_recv(&Res,1);
  }	
}


void usart1_send(u8*data, u8 len)  //发送数据函数
{
	u8 i;
	for(i=0;i<len;i++)
	{
		while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); 
		USART_SendData(USART1,data[i]);   
	}
}


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105

usart.h

#ifndef __USART_H
#define __USART_H
#include "stdio.h"	
#include "sys.h" 

#define USART_REC_LEN  			200  	//定义最大接收字节数 200
#define EN_USART1_RX 			1		//使能（1）/禁止（0）串口1接收
	  	
extern u8  USART_RX_BUF[USART_REC_LEN]; //接收缓冲,最大USART_REC_LEN个字节.末字节为换行符 
extern u16 USART_RX_STA;         		//接收状态标记	

void uart_init(u32 bound);
void usart1_send(u8*data, u8 len);
#endif




  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

二、各类PID

1. 位置式PID（用于位置环）

  测量位置就是通过stm32去采集编码器的脉冲数据，通过脉冲计算出位置（角度）。目标位置和测量位置之间做差这个就是目前系统的偏差。送入 PID 控制器进行计算输出，然后再经过电机驱动的功率放大控制电机的转动去减小偏差， 最终达到目标位置的过程。

（1）公式

在这里插入图片描述

（2）代码

pid.c

typedef struct PID {
		float  Kp;         //  Proportional Const  P系数
		float  Ki;           //  Integral Const      I系数
		float  Kd;         //  Derivative Const    D系数
		
		float  PrevError ;          //  Error[-2]  
		float  LastError;          //  Error[-1]  
		float  Error;              //  Error[0 ]  
		float  DError;            //pid->Error - pid->LastError	
		float  SumError;           //  Sums of Errors  
		
		float  output;
		
		float  Integralmax;      //积分项的最大值
		float  outputmax;        //输出项的最大值
} PID;

//为了防止积分项过度累积，引入积分项的限幅是一种常见的做法。
//限制积分项的幅值可以防止积分项过度增加，从而限制了系统的累积误差。这样可以避免系统过度响应或者不稳定。
float abs_limit(float value, float ABS_MAX)   //积分限幅，设置最大值。
{
	if(value > ABS_MAX)
		value = ABS_MAX;

	if(value< -ABS_MAX)
		value = -ABS_MAX;
	return value;
}


//函数里传入指针，修改时会修改指针里的值。
float PID_Position_Calc(PID *pid, float Target_val, float Actual_val)  //位置式PID
{   

	pid->Error = Target_val - Actual_val;      //与pid P系数相乘。比例误差值 当前差值=目标值-实际值
	pid->SumError += pid->Error;                 //与pid I系数相乘。稳态误差值 误差相加作为误差总和，给积分项
	pid->DError = pid->Error - pid->LastError;   //与pid D系数相乘。 微分项-消除震荡

	pid->output =   pid->Kp* pid->Error +        
					abs_limit( pid->Ki* pid->SumError, pid->Integralmax ) +   
					pid->Kd* pid->DError ;  
								
   pid->LastError = pid->Error; //更新误差
   
  //限制输出最大值，防止出现突发意外。输出outputmax的最大值
	if(pid->output > pid->outputmax )  pid->output = pid->outputmax; 
	if(pid->output < - pid->outputmax )  pid->output = -pid->outputmax;
	
	return pid->output ;   //输出为pwm值
}

//PID初始化
void PID_Init(PID *pid, float Kp , float Ki , float Kd , float Limit_value)  
{  
	pid->Kp= Kp;
	pid->Ki= Ki;
	pid->Kd= Kd;
	
	pid->PrevError =pid->LastError = pid->Error =pid->SumError= pid->output =  0; 
	pid->Integralmax = pid->outputmax  = Limit_value;
}  

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61

使用代码

#include "sys.h"

PID postion_pid;
float Encoder_Speed =0;
float Position =0;
float Speed=0;
float Target_val =500;

int main()
{
	Time2_Init(10000-1,7200-1); //定时器2用于定时 10000*7200/72 = 1s
	Encoder_Init();  //定时器4的编码器
	Motor_PWM_Init(7200-1,0); //定时器1，初始化pwm输出
	PID_Init(&postion_pid, 1.0, 0, 1.0, 7000);
	
	while(1)
	{
	
	}
}

//---- 获得电机的脉冲
int16_t Encoder_Get(void)
{
	int16_t Temp;
	Temp = TIM_GetCounter(TIM4); //获取编码器当前值
	TIM_SetCounter(TIM4, 0);  //将编码器计数器清0
	return Temp;
}

//设置pwm
void Set_Pwm(int motor_pwm)
{
	TIM_SetCompare4(TIM1, motor_pwm);
}

void MotorControl(void)
{
	Encoder_Speed = Encoder_Get();//1.获取电机1s的脉冲数。即1s获取的脉冲数。即速度
	Position +=Encoder_Speed ; //累计实际脉冲数。与时间无关。即总路程
	
	Speed=PID_Position_Calc(&postion_pid, Target_val , Position);//2.输入增量式PID计算
	Set_Pwm(Speed);  //3.PWM输出给电机
	//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1);   /*4.给上位机通道2发送实际的电机速度值*/
}



void TIM2_IRQHandler(void)  //定时器中断函数，1s进一次中断
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
		MotorControl();
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57

2. 增量式PID（用于速度环）

  增量式PID也称速度环PID，速度闭环控制就是根据单位时间获取的脉冲数测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。

（1）公式

在这里插入图片描述

（2）代码

typedef struct PID {
		float  Kp;         //  Proportional Const  P系数
		float  Ki;           //  Integral Const      I系数
		float  Kd;         //  Derivative Const    D系数
		
		float  PrevError ;          //  Error[-2]  
		float  LastError;          //  Error[-1]  
		float  Error;              //  Error[0 ]  
		float  DError;            //pid->Error - pid->LastError	
		float  SumError;           //  Sums of Errors  
		
		float  output;
		
		float  Integralmax;      //积分项的最大值
		float  outputmax;        //输出项的最大值
} PID;


float PID_Incremental_Calc(PID *pid, float Target_val, float Actual_val)  
{  
	pid->Error = Target_val- Actual_val;                        

	pid->output  +=  pid->Kp* ( pid->Error - pid->LastError )+   
					 pid->Ki* pid->Error +   
					 pid->Kd* ( pid->Error +  pid->PrevError - 2*pid->LastError);  
								 
	pid->PrevError = pid->LastError;  
	pid->LastError = pid->Error;

	if(pid->output > pid->outputmax )    pid->output = pid->outputmax;
	if(pid->output < - pid->outputmax )  pid->output = -pid->outputmax;
	
	return pid->output ;   //输出为pwm值
}

//PID初始化
void PID_Init(PID *pid, float Kp , float Ki , float Kd , float Limit_value)  
{  
	pid->Kp= Kp;
	pid->Ki= Ki;
	pid->Kd= Kd;
	
	pid->PrevError =pid->LastError = pid->Error =pid->SumError= pid->output =  0; 
	pid->Integralmax = pid->outputmax  = Limit_value;
}  

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45

（3）使用代码

#include "sys.h"

PID speedpid;
float Encoder_Speed =0;
float Target_val =500;  //目标1s的脉冲数
float Speed=0;//实际速度


int main()
{
	Time2_Init(10000-1,7200-1); //定时器2用于定时 10000*7200/72 = 1s
	Encoder_Init();  //定时器4的编码器
	Motor_PWM_Init(7200-1,0); //定时器1，初始化pwm输出
	PID_Init(&speedpid, 1.0, 0, 1.0, 7000);
	
	while(1)
	{
	
	}
}

//获得电机的脉冲
int16_t Encoder_Get(void)
{
	int16_t Temp;
	Temp = TIM_GetCounter(TIM4); //获取编码器当前值
	TIM_SetCounter(TIM4, 0);  //将编码器计数器清0
	return Temp;
}

//设置pwm
void Set_Pwm(int motor_pwm)
{
	TIM_SetCompare4(TIM1, motor_pwm);
}

void MotorControl(void)
{
	Encoder_Speed = Encoder_Get();//1.获取电机1s的脉冲数。即1s获取的脉冲数。即速度。
	Speed=PID_Incremental_Calc(&speedpid,Target_val ,Encoder_Speed);//2.输入增量式PID计算
	Set_Pwm(Speed);  //3.PWM输出给电机
	//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1);   /*4.给上位机通道2发送实际的电机速度值*/
}




void TIM2_IRQHandler(void)  //定时器中断函数，1s进一次中断
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
		MotorControl();
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56

3. 串级PID

（1）位置环–速度环（用于控制电机）

  利用位置式pid的方法将位置环和速度环组合在一起使用。位置环的输出作为速度环的输入。位置环的输出作为速度环的目标期望值。这意味着位置环的输出被视为速度环应该追踪的目标位置。速度环的任务是根据当前位置和目标位置之间的偏差来生成控制输出，使系统尽可能快地接近目标位置。速度环将根据当前速度和目标速度之间的差异来调整电机的输出，以便使实际速度接近目标速度。
在这里插入图片描述

简易代码

  将目标位置和实际位置传入位置环PID中，计算出期望转速。然后通过期望转速与实际转速传入速度环PID中计算出对应的pwm，然后通过pwm去控制电机。

#include "stdio.h"

PID  postion_pid;
PID  speed_pid;

float Encoder_Speed =0;
float Target_val =500;  //目标总的脉冲数
float Speed=0;//实际速度
float Position =0;

int main(void)
{

	  Time2_Init(10000-1,7200-1); //定时器2用于定时 10000*7200/72 = 1s，如果觉得时间太长可以缩短一些
	  Encoder_Init();  //定时器4的编码器
	  Motor_PWM_Init(7200-1,0); //定时器1，初始化pwm输出
	  // 初始化PID控制器
	  PID_Init(&postion_pid, 1.0, 0.1, 0.01, 300); // PID参数根据实际情况调整
	  PID_Init(&speed_pid, 1.0, 0.1, 0.01, 300);  // PID参数根据实际情况调整
  
	  while (1)
	  {

	  }
}

//获得电机的脉冲
int16_t Encoder_Get(void)
{
	int16_t Temp;
	Temp = TIM_GetCounter(TIM4); //获取编码器当前值
	TIM_SetCounter(TIM4, 0);  //将编码器计数器清0
	return Temp;
}


//设置pwm
void Set_Pwm(int motor_pwm)
{
	TIM_SetCompare4(TIM1, motor_pwm);
}


void MotorControl(void)
{
	Encoder_Speed = Encoder_Get(); //1.获取电机1s的脉冲数。即1s获取的脉冲数。即速度
	Position +=Encoder_Speed ;  //累计实际脉冲数。与时间无关。即总路程
	
	Speed=PID_Position_Calc(&postion_pid, Target_val , Position);//2.输入位置式PID计算
	Speed=PID_Incremental_Calc(&speedpid,Speed, Encoder_Speed);//2.输入增量式PID计算
	Set_Pwm(Speed);  //3.PWM输出给电机
	//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1);   /*4.给上位机通道2发送实际的电机速度值*/
}


void TIM2_IRQHandler(void)  //定时器中断函数，1s进一次中断
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
		MotorControl();
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}



  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65

（2）位置环–位置环（用于控制舵机）

  因为舵机没有编码器，无法获取实际速度，所以我们可以使用两个位置环来进行串级pid的使用，这样更加精准。两个位置环的实际值输入都为距离值。第一个位置环的输出作为第二个位置环的目标值输入。
  实际举例：假设我们使用舵机来进行目标追踪。则第一个位置环的实际值输入：当前坐标-上次坐标的差值，目标值为0。将这两个值传入位置环计算的输出作为第二个位置环的目标值，第二个位置环的实际值可以传入：当前位置和摄像头中心点位置的差值。计算第二个位置环的输出。将其作为pwm值输入定时器通道去控制舵机。

三、调参

讲述Kp、Ki、Kd的作用。

P：增加快速性，过大会引起震荡和超调，P单独作用会一直有静态误差。
I：减少静态误差，过大会引起震荡。
D：减小超调，过大会使响应速度变慢。

1. 知识点

（1）纯Kp调节（比例）

假设有一个高为10m的水桶需要灌满水，这里我们假设Kp=0.2（每次灌水量为剩余灌水量的0.2倍）。
第一次灌水：10×0.2， 剩余8（10-10×0.2）。
第二次灌水： 8×0.2， 剩余6.4（8-8×0.2）。
第三次灌水：6.4×0.2 ，剩余5.12。
…
  这里我们发现当我们设置Kp后，一直会慢慢接近目标值，但是永远不会到达目标值，这也就是会一直有静态误差。当Kp设置过小时，消耗的时间也就会更多。这里我们可以适当的调大Kp，使得更快的接近目标值。但是当Kp大于某个定值时，就会出现抖动，如下，假设Kp=1.5。
则第一次灌水：10×1.5，剩余 -5。
第二次灌水：-5×1.5，剩余2.5（-5 - (-5×1.5)）。
第三次灌水：2.5×1.5，剩余 -1.25。
…
所以，要根据实际适当调整p值，不要使得Kp过大，而出现抖动。

（2）Ki调节（积分）

  作用：积分时间用于解决系统的稳态误差问题，即系统无法完全到达期望值的情况。当存在稳态误差时，积分项会不断积累偏差，并且在一段时间内持续作用于控制器的输出，直到系统到达期望状态为止。
  水桶例子：假设你在使用一个PID控制系统来控制一个水桶的水位。如果水桶的出水口略微大于水龙头的流量，那么水位就会慢慢下降，形成一个稳态偏差。积分时间就像是一个将稳态偏差中的水慢慢积累起来，直到水桶完全满了。如果积分时间设置得太大，可能会导致水桶溢出，而设置得太小则可能导致水桶永远无法完全填满。

（3）Kd调节（微分）

  作用：微分时间用于减小系统的超调和提高系统的稳定性。它通过监测偏差的变化速率来预测系统未来的行为，并相应地调整控制器的输出，以减少振荡和过冲现象。
  水桶例子：继续以水桶控制系统为例，微分时间就像是观察水流速度的变化。如果你突然关闭水龙头，但是水桶的水位仍然在上升，那么微分项会告诉你要逐渐减小输出，以避免水位超过期望值。如果微分时间设置得太大，可能会导致系统对外部干扰过于敏感，反而引起不稳定性；而设置得太小，则可能无法有效地抑制超调和振荡。

2. 调参软件–野火多功能调试助手

  注意： 在串级PID控制中，上位机下传的PID参数通常应该是位置式的PID参数。因为在串级控制中，位置PID控制器的输出作为速度PID控制器的输入。因此，上位机通常会调节位置PID控制器的参数，以影响整个串级PID系统的行为。
  当上位机调节位置PID参数时，它会直接影响到位置PID控制器的输出，从而间接地影响到速度PID控制器的输入，进而影响到整个系统的运行状态。因此，在串级PID控制中，上位机通常下传的是位置式的PID参数。

这个软件需要使用串口进行通信调参，下面是通信代码。
在这里插入图片描述

Ⅰ. 传输格式

在这里插入图片描述

在这里插入图片描述

Ⅱ. 协议解析代码

  只需要先将protocol.c和protocol.h添加到工程中，然后使用相应的函数即可。切记：该代码需要和串口1代码搭配使用，因为使用了串口1的发送函数（见上面PID工程）。

protocol.c

/**
  ******************************************************************************
  * @file    protocol.c
  * @brief   野火PID调试助手通讯协议解析
  ******************************************************************************
  */ 
#include "protocol.h"
#include <string.h>
#include "pid.h"
#include "timer.h"

/*协议帧解析结构体*/
struct prot_frame_parser_t
{
    uint8_t *recv_ptr;         /*数据接收数组*/
    uint16_t r_oft;            /*读偏移*/
    uint16_t w_oft;            /*写偏移*/
    uint16_t frame_len;        /*帧长度*/
    uint16_t found_frame_head;
};

/*定义一个协议帧解析结构体*/
static struct prot_frame_parser_t parser;
/*定义一个接收缓冲区*/
static uint8_t recv_buf[PROT_FRAME_LEN_RECV];

/**
 * @brief   初始化接收协议
 * @param   void
 * @return  初始化结果.
 */
int32_t protocol_init(void)
{
	/*全局变量parser清空*/
    memset(&parser, 0, sizeof(struct prot_frame_parser_t));
    
    /* 初始化分配数据接收与解析缓冲区*/
    parser.recv_ptr = recv_buf;
  
    return 0;
}


/**
  * @brief 计算校验和
  * @param ptr：需要计算的数据
  * @param len：需要计算的长度
  * @retval 校验和
  */
uint8_t check_sum(uint8_t init, uint8_t *ptr, uint8_t len )
{
	/*校验和的计算结果*/
	uint8_t sum = init;

	while(len--)
	{
		sum += *ptr;/*依次累加各个数据的值*/
		ptr++;
	}

	return sum;
}

/**
 * @brief   获取帧类型（帧命令）
 * @param   *buf: 数据缓冲区
 * @param   head_oft: 帧头的偏移位置
 * @return  帧类型（帧命令）
 */
static uint8_t get_frame_type(uint8_t *buf, uint16_t head_oft)
{
	/*计算“帧命令”在帧数据中的位置*/
	uint16_t cmdIndex = head_oft + CMD_INDEX_VAL;
	
    return (buf[cmdIndex % PROT_FRAME_LEN_RECV] & 0xFF);
}

/**
 * @brief   获取帧长度
 * @param   *buf: 数据缓冲区
 * @param   head_oft: 帧头的偏移位置
 * @return  帧长度.
 */
static uint16_t get_frame_len(uint8_t *buf, uint16_t head_oft)
{
	/*计算“帧长度”在帧数据中的位置*/
	uint16_t lenIndex = head_oft + LEN_INDEX_VAL;
	
    return ((buf[(lenIndex + 0) % PROT_FRAME_LEN_RECV] <<  0) |
            (buf[(lenIndex + 1) % PROT_FRAME_LEN_RECV] <<  8) |
            (buf[(lenIndex + 2) % PROT_FRAME_LEN_RECV] << 16) |
            (buf[(lenIndex + 3) % PROT_FRAME_LEN_RECV] << 24));    // 合成帧长度
}

/**
 * @brief   获取crc-16校验值
 * @param   *buf:  数据缓冲区.
 * @param   head_oft: 帧头的偏移位置
 * @param   frame_len: 帧长
 * @return  校验值
 */
static uint8_t get_frame_checksum(uint8_t *buf, uint16_t head_oft, uint16_t frame_len)
{
	/*计算“校验和”在帧数据中的位置*/
	uint16_t crcIndex = head_oft + frame_len - 1;
	
    return (buf[crcIndex % PROT_FRAME_LEN_RECV]);
}

/**
 * @brief   查找帧头
 * @param   *buf:  数据缓冲区.
 * @param   ring_buf_len: 缓冲区大小（常量，如128）
 * @param   start: 起始位置（读偏移）
 * @param   len: 需要查找的长度
 * @return  -1：没有找到帧头，其他值：帧头的位置.
 */
static int32_t recvbuf_find_header(uint8_t *buf, const uint16_t ring_buf_len, uint16_t start, uint16_t len)
{
    uint16_t i = 0;

	/*帧头是4字节，从0查找到len-4，逐个比对*/
    for (i = 0; i < (len - 3); i++)
    {
        if (((buf[(start + i + 0) % ring_buf_len] <<  0) |
             (buf[(start + i + 1) % ring_buf_len] <<  8) |
             (buf[(start + i + 2) % ring_buf_len] << 16) |
             (buf[(start + i + 3) % ring_buf_len] << 24)) == FRAME_HEADER) /*0x59485A53*/
        {
            return ((start + i) % ring_buf_len);
        }
    } 
    return -1;
}

/**
 * @brief   计算未解析的数据的长度
 * @param   frame_len: 帧长度（数据中记录的帧长度）
 * @param   ring_buf_len: 缓冲区大小（常量，如128）
 * @param   start: 起始位置（读偏移）
 * @param   end: 结束位置（写偏移）
 * @return  未解析的数据长度
 */
static int32_t recvbuf_get_len_to_parse(uint16_t frame_len, const uint16_t ring_buf_len,uint16_t start, uint16_t end)
{
    uint16_t unparsed_data_len = 0; /*未解析的数据长度*/

	/*读偏移<=写偏移，说明数据在环形缓存区中是连续存储的*/
    if (start <= end)
	{
        unparsed_data_len = end - start;
	}
	/*否则，数据被分成了两部分，一部分在缓冲区结尾，一部分在缓冲区开头*/
    else
	{
		/*缓冲区结尾处的长度 + 缓冲区开头处处的长度*/
        unparsed_data_len = (ring_buf_len - start) + end;
	}

    if (frame_len > unparsed_data_len)
	{
		/*数据中记录的帧长度 > 未解析的数据长度*/
        return 0;
	}
    else
	{
        return unparsed_data_len;
	}
}

/**
 * @brief   接收数据写入缓冲区
 * @param   *buf:  数据缓冲区.
 * @param   ring_buf_len: 缓冲区大小（常量，如128）
 * @param   w_oft: 写偏移
 * @param   *data: 需要写入的数据
 * @param   data_len: 需要写入数据的长度
 * @return  void.
 */
 void recvbuf_put_data(uint8_t *buf, const uint16_t ring_buf_len, uint16_t w_oft, uint8_t *data, uint16_t data_len)
{
	/*要写入的数据超过了缓冲区尾*/
    if ((w_oft + data_len) > ring_buf_len)               
    {
		/*计算缓冲区剩余长度*/
        uint16_t data_len_part = ring_buf_len - w_oft;     

        /*数据分两段写入缓冲区*/
        memcpy((buf + w_oft), data, data_len_part); /*先将一部分写入缓冲区尾*/
        memcpy(buf, (data + data_len_part), (data_len - data_len_part));/*再将剩下的覆盖写入缓冲区头*/
    }
    else
	{
        memcpy(buf + w_oft, data, data_len);/*直接将整个数据写入缓冲区*/
	}
}

/**
 * @brief   协议帧解析
 * @param   *data: 返回解析出的帧数据
 * @param   *data_len: 返回帧数据的大小
 * @return  帧类型（命令）
 */
 uint8_t protocol_frame_parse(uint8_t *data, uint16_t *data_len)
{
    uint8_t frame_type = CMD_NONE;  /*帧类型*/
    uint16_t need_to_parse_len = 0; /*需要解析的原始数据的长度*/
    uint8_t checksum = 0;           /*校验和*/
    
	/*计算未解析的数据的长度*/
    need_to_parse_len = recvbuf_get_len_to_parse(parser.frame_len, PROT_FRAME_LEN_RECV, parser.r_oft, parser.w_oft);    
    if (need_to_parse_len < 9)     
	{
		/*数据太少，肯定还不能同时找到帧头和帧长度*/
        return frame_type;
	}

    /*还未找到帧头，需要进行查找*/
    if (0 == parser.found_frame_head)
    {
		int16_t header_oft = -1; /*帧头偏移*/
		
        /* 同步头为四字节，可能存在未解析的数据中最后一个字节刚好为同步头第一个字节的情况，
           因此查找同步头时，最后一个字节将不解析，也不会被丢弃*/
        header_oft = recvbuf_find_header(parser.recv_ptr, PROT_FRAME_LEN_RECV, parser.r_oft, need_to_parse_len);
        if (0 <= header_oft)
        {
            /* 已找到帧头*/
            parser.found_frame_head = 1;
            parser.r_oft = header_oft;
          
            /* 确认是否可以计算帧长*/
            if (recvbuf_get_len_to_parse(parser.frame_len, PROT_FRAME_LEN_RECV, parser.r_oft, parser.w_oft) < 9)
			{
                return frame_type;
			}
        }
        else 
        {
            /* 未解析的数据中依然未找到帧头，丢掉此次解析过的所有数据*/
            parser.r_oft = ((parser.r_oft + need_to_parse_len - 3) % PROT_FRAME_LEN_RECV);
            return frame_type;
        }
    }
    
    /* 计算帧长，并确定是否可以进行数据解析*/
    if (0 == parser.frame_len) 
    {
        parser.frame_len = get_frame_len(parser.recv_ptr, parser.r_oft);
        if(need_to_parse_len < parser.frame_len)
		{
            return frame_type;
		}
    }

    /* 帧头位置确认，且未解析的数据超过帧长，可以计算校验和*/
    if ((parser.frame_len + parser.r_oft - PROT_FRAME_LEN_CHECKSUM) > PROT_FRAME_LEN_RECV)
    {
        /* 数据帧被分为两部分，一部分在缓冲区尾，一部分在缓冲区头 */
        checksum = check_sum(checksum, parser.recv_ptr + parser.r_oft, PROT_FRAME_LEN_RECV - parser.r_oft);
        checksum = check_sum(checksum, parser.recv_ptr, parser.frame_len - PROT_FRAME_LEN_CHECKSUM + parser.r_oft - PROT_FRAME_LEN_RECV);
    }
    else 
    {
        /* 数据帧可以一次性取完*/
        checksum = check_sum(checksum, parser.recv_ptr + parser.r_oft, parser.frame_len - PROT_FRAME_LEN_CHECKSUM);
    }

    if (checksum == get_frame_checksum(parser.recv_ptr, parser.r_oft, parser.frame_len))
    {
        /* 校验成功，拷贝整帧数据 */
        if ((parser.r_oft + parser.frame_len) > PROT_FRAME_LEN_RECV) 
        {
            /* 数据帧被分为两部分，一部分在缓冲区尾，一部分在缓冲区头*/
            uint16_t data_len_part = PROT_FRAME_LEN_RECV - parser.r_oft;
            memcpy(data, parser.recv_ptr + parser.r_oft, data_len_part);
            memcpy(data + data_len_part, parser.recv_ptr, parser.frame_len - data_len_part);
        }
        else 
        {
            /* 数据帧可以一次性取完*/
            memcpy(data, parser.recv_ptr + parser.r_oft, parser.frame_len);
        }
        *data_len = parser.frame_len;
        frame_type = get_frame_type(parser.recv_ptr, parser.r_oft);

        /* 丢弃缓冲区中的命令帧*/
        parser.r_oft = (parser.r_oft + parser.frame_len) % PROT_FRAME_LEN_RECV;
    }
    else
    {
        /* 校验错误，说明之前找到的帧头只是偶然出现的废数据*/
        parser.r_oft = (parser.r_oft + 1) % PROT_FRAME_LEN_RECV;
    }
    parser.frame_len = 0;
    parser.found_frame_head = 0;

    return frame_type;
}

/**
 * @brief   接收到的数据写入缓冲区
 * @param   *data:  接收到的数据的数组.
 * @param   data_len: 接收到的数据的大小
 * @return  void.
 */
void protocol_data_recv(uint8_t *data, uint16_t data_len)
{
	/*数据写入缓冲区*/
    recvbuf_put_data(parser.recv_ptr, PROT_FRAME_LEN_RECV, parser.w_oft, data, data_len);    
	
	/*计算写偏移*/
    parser.w_oft = (parser.w_oft + data_len) % PROT_FRAME_LEN_RECV;                          
}


/**
  * @brief 设置上位机的值
  * @param cmd：命令
  * @param ch: 曲线通道
  * @param data：参数指针
  * @param num：参数个数
  * @retval 无
  */
void set_computer_value(uint8_t cmd, uint8_t ch, void *data, uint8_t num)
{
	static packet_head_t set_packet;

	uint8_t sum = 0;    // 校验和
	num *= 4;           // 一个参数 4 个字节

	set_packet.head = FRAME_HEADER;     // 包头 0x59485A53
	set_packet.ch   = ch;              // 设置通道
	set_packet.len  = 0x0B + num;      // 包长
	set_packet.cmd  = cmd;             // 设置命令

	sum = check_sum(0, (uint8_t *)&set_packet, sizeof(set_packet));       // 计算包头校验和
	sum = check_sum(sum, (uint8_t *)data, num);                           // 计算参数校验和

	usart1_send((uint8_t *)&set_packet, sizeof(set_packet));    // 发送数据头
	usart1_send((uint8_t *)data, num);                          // 发送参数
	usart1_send((uint8_t *)&sum, sizeof(sum));                  // 发送校验和
}

/**********************************************************************************************/


  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346

protocol.h


#ifndef __PROTOCOL_H__
#define __PROTOCOL_H__

/*****************************************************************************/
/* Includes                                                                  */
/*****************************************************************************/
#include "sys.h"
#include "usart.h"

#ifdef _cplusplus
extern "C" {
#endif   

/* 数据接收缓冲区大小 */
#define PROT_FRAME_LEN_RECV  128

/* 校验数据的长度 */
#define PROT_FRAME_LEN_CHECKSUM    1

/* 数据头结构体 */
typedef __packed struct
{
  uint32_t head;    // 包头
  uint8_t ch;       // 通道
  uint32_t len;     // 包长度
  uint8_t cmd;      // 命令
}packet_head_t;

#define FRAME_HEADER     0x59485A53    // 帧头

/* 通道宏定义 */
#define CURVES_CH1      0x01
#define CURVES_CH2      0x02
#define CURVES_CH3      0x03
#define CURVES_CH4      0x04
#define CURVES_CH5      0x05

/* 指令(下位机 -> 上位机) */
#define SEND_TARGET_CMD      0x01     // 发送上位机通道的目标值
#define SEND_FACT_CMD        0x02     // 发送通道实际值
#define SEND_P_I_D_CMD       0x03     // 发送 PID 值（同步上位机显示的值）
#define SEND_START_CMD       0x04     // 发送启动指令（同步上位机按钮状态）
#define SEND_STOP_CMD        0x05     // 发送停止指令（同步上位机按钮状态）
#define SEND_PERIOD_CMD      0x06     // 发送周期（同步上位机显示的值）

/* 指令(上位机 -> 下位机) */
#define SET_P_I_D_CMD        0x10     // 设置 PID 值
#define SET_TARGET_CMD       0x11     // 设置目标值
#define START_CMD            0x12     // 启动指令
#define STOP_CMD             0x13     // 停止指令
#define RESET_CMD            0x14     // 复位指令
#define SET_PERIOD_CMD       0x15     // 设置周期

/* 空指令 */
#define CMD_NONE             0xFF     // 空指令

/*********************************************************************************************
协议数据示例

1.下发目标值55：
	|----包头----|通道|---包长度---|命令|----参数---|校验|
	| 0  1  2  3 |  4 |  5  6  7  8|  9 |10 11 12 13| 14 | <-索引
	|53 5A 48 59 | 01 | 0F 00 00 00| 11 |37 00 00 00| A6 | <-协议帧数
	
2.下发PID（P=1 I=2 D=3）：
	|----包头----|通道|---包长度---|命令|---参数P---|---参数I---|---参数D---|校验|
	| 0  1  2  3 |  4 |  5  6  7  8|  9 |10 11 12 13|14 15 15 17|18 19 20 21| 22 | <-索引
	|53 5A 48 59 | 01 | 17 00 00 00| 10 |00 00 80 3F|00 00 00 40|00 00 40 40| F5 | <-协议帧数
	
**********************************************************************************************/

/* 索引值宏定义 */
#define HEAD_INDEX_VAL       0x3u     // 包头索引值（4字节）
#define CHX_INDEX_VAL        0x4u     // 通道索引值（1字节）
#define LEN_INDEX_VAL        0x5u     // 包长索引值（4字节）
#define CMD_INDEX_VAL        0x9u     // 命令索引值（1字节）

/* 交换高低字节（未用到） */
#define EXCHANGE_H_L_BIT(data)      ((((data) << 24) & 0xFF000000) |\
                                     (((data) <<  8) & 0x00FF0000) |\
                                     (((data) >>  8) & 0x0000FF00) |\
                                     (((data) >> 24) & 0x000000FF))     
/* 合成为一个字 */
#define COMPOUND_32BIT(data)        (((*(data-0) << 24) & 0xFF000000) |\
                                     ((*(data-1) << 16) & 0x00FF0000) |\
                                     ((*(data-2) <<  8) & 0x0000FF00) |\
                                     ((*(data-3) <<  0) & 0x000000FF))      
                                     
/**
 * @brief   接收数据处理
 * @param   *data:  要计算的数据的数组.
 * @param   data_len: 数据的大小
 * @return  void.
 */
void protocol_data_recv(uint8_t *data, uint16_t data_len);

/**
 * @brief   初始化接收协议
 * @param   void
 * @return  初始化结果.
 */
int32_t protocol_init(void);

/**
  * @brief 设置上位机的值
  * @param cmd：命令
  * @param ch: 曲线通道
  * @param data：参数指针
  * @param num：参数个数
  * @retval 无
  */
void set_computer_value(uint8_t cmd, uint8_t ch, void *data, uint8_t num);
 uint8_t protocol_frame_parse(uint8_t *data, uint16_t *data_len);
#ifdef _cplusplus
}
#endif   

#endif

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119

（1）上位机将pid参数发送给下位机

  上位机通过串口发送设置的pid参数信息，我们通过串口接收，并解析出这些信息，然后设置到我们的pid上。
  我们在对pid调参时，如果我们使用的串级pid，我们只需要调外层的pid参数即可，因为内层的目标值是外层的输出。所以调外层的pid就可以影响整个系统。假如我们有x的内外层pid和y的内外层pid时，我们应该先调一个，如先调x。当把x层的参数调好后，y的pid直接使用x一样的参数即可。如下所示:
   注意：为了全局代码的一致性，我们不使用上位机调整目标值，如果需要修改目标值，我们直接在代码中修改即可。此文我们只使用上位机调整pid参数（外层–位置层）！


/*
#define SET_P_I_D_CMD        0x10     // 设置 PID 值
#define SET_TARGET_CMD       0x11     // 设置目标值
#define START_CMD            0x12     // 启动指令
#define STOP_CMD             0x13     // 停止指令
#define RESET_CMD            0x14     // 复位指令
#define SET_PERIOD_CMD       0x15     // 设置周期
*/
PID PosionPID;
PID SpeedPID;

//该代码为串口接收上位机pid信息解析代码，直接复制使用即可。
void receiving_process(void)
{
	uint8_t frame_data[128];         // 要能放下最长的帧
	uint16_t frame_len = 0;          // 帧长度
	uint8_t cmd_type = CMD_NONE;     // 命令类型

		/*解析指令类型*/
		cmd_type = protocol_frame_parse(frame_data, &frame_len);
		
		switch (cmd_type)
		{
			/*空指令*/
			case CMD_NONE:
			{
				break;
			}

			/***************设置PID***************/
			case SET_P_I_D_CMD:
			{
				/* 接收的4bytes的float型的PID数据合成为一个字 */
				uint32_t temp0 = COMPOUND_32BIT(&frame_data[13]);
				uint32_t temp1 = COMPOUND_32BIT(&frame_data[17]);
				uint32_t temp2 = COMPOUND_32BIT(&frame_data[21]);

				/*uint32_t强制转换为float*/
				float p_temp, i_temp, d_temp;
				p_temp = *(float *)&temp0;
				i_temp = *(float *)&temp1;
				d_temp = *(float *)&temp2;
				
                /*设置PID*/
				set_PID(p_temp, i_temp, d_temp);   
			}
			break;

			/**************设置目标值***************/
			case SET_TARGET_CMD:
			{
				/* 接收的4bytes的int型的数据合成为一个字 */
				int actual_temp = COMPOUND_32BIT(&frame_data[13]);  
				
				/*设置目标值*/
				set_PID_target((float)actual_temp);    
			}
			break;
			
			/******************启动*****************/
			case START_CMD:
			{
				/*开启pid运算*/
				TIM_Cmd(TIM2,ENABLE); //使能定时器2
			}
			break;

			/******************停止*****************/
			case STOP_CMD:
			{
				/*停止pid运算*/
				Set_Pwm(0);
				TIM_Cmd(TIM2,DISABLE); //关闭定时器2
			}
			break;

			case RESET_CMD:
			{
				NVIC_SystemReset();          // 复位系统
			}
			break;
		}
}

//设置外层（位置层）的pid参数
void set_PID(float p, float i, float d)
{
	PosionPID.Kp = p;    // 设置比例系数 P
	PosionPID.Ki = i;    // 设置积分系数 I
	PosionPID.Kd = d;    // 设置微分系数 D
}

//设置目标值
void set_PID_target(float temp_val)
{  
	postion_outerx.Target_val = temp_val;    // 设置当前的目标值
}

//获取目标值
float get_pid_target(PID *pid)
{
  return pid->Target_val;    // 获取当前的目标值
}


void USART1_IRQHandler(void)//串口中断服务函数
{
	 u8 Res;
	if(USART_GetITStatus(USART1, USART_IT_RXNE)== SET ) //产生了接收中断
	{
		USART_ClearITPendingBit(USART1,USART_IT_RXNE);  	//清除接收中断标志位
		Res=USART_ReceiveData(USART1);
		protocol_data_recv(&Res,1);   //该函数的定义在protocol.c里面。
  }	
}

//-------------------------放到主函数的while里。


int main()
{

   protocol_init();   //该函数的定义在protocol.c里面。
   while(1)
   {
 		receiving_process(); //一直解析处理接收到的数据。
   }

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130

（2）发送实际值、目标值给上位机

发送目标值与实际值。这里的目标值和实际值是外层pid（位置层）的目标值和实际值。

/*
#define SEND_TARGET_CMD      0x01     // 发送上位机通道的目标值
#define SEND_FACT_CMD        0x02     // 发送通道实际值
#define SEND_P_I_D_CMD       0x03     // 发送 PID 值（同步上位机显示的值）
#define SEND_START_CMD       0x04     // 发送启动指令（同步上位机按钮状态）
#define SEND_STOP_CMD        0x05     // 发送停止指令（同步上位机按钮状态）
#define SEND_PERIOD_CMD      0x06     // 发送周期（同步上位机显示的值）
#define CURVES_CH1      0x01
#define CURVES_CH2      0x02
#define CURVES_CH3      0x03
#define CURVES_CH4      0x04
#define CURVES_CH5      0x05
*/

PID PosionPID;
PID SpeedPID;

int16_t Encoder_Speed =0;
int16_t Position =0;
int16_t Speed;//实际速度
int Target_val=500;
void MotorControl(void)
{
	
 Encoder_Speed= Read_Position();//1.获取定时器3的编码器数值
 Position+=Encoder_Speed;    //2.速度积分得到位置
 Speed=PID_Position_Calc(&PosionPID, Target_val, Position);//3.输入位置式PID计算
 Speed= PID_Incremental_Calc(&SpeedPID, Speed, Encoder_Speed);//4.输入速度式PID计算
 Set_Pwm(Speed);  //4.PWM输出给电机
 
//指令/通道/发送数据/个数
 set_computer_value(SEND_FACT_CMD, CURVES_CH4, &Position, 1);   /*5.给上位机通道2发送实际的电机速度值*/
 set_computer_value(SEND_TARGET_CMD, CURVES_CH4, &Target_val, 1);	//发送目标值
}


void TIM2_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
	    MotorControl();
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
	}
}


int main()
{
	  PID_Init(&PosionPID, 1.0, 1.0, 1.0, 500);
	  PID_Init(&SpeedPID,1.0, 1.0, 1.0, 500);
	  protocol_init();   //该函数的定义在protocol.c里面。
	  while(1)
	  {
	  
	  }
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56

注意：如果上位机上不显示波形，一定要先关闭上位机重新打开才会显示！！且代码中我们将数据发送到通道4，所以我们在上位机上要使用通道4查看波形。

在这里插入图片描述
在这里插入图片描述

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/在线问答5/article/detail/924515
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号