Freertos-小车开发笔记 3 --PID控制编码电机

目录

前言

一、创建软件定时器

二、软件定时器回调函数

三、PID控制编码电机

1.前轮转向控制

2.串口1输出电机以及舵机的数据

3.串口3接收指令及其处理指令

---

前言

通过编码点击控制小车的后轮转动，通过舵机改变前轮的转向。

---

一、创建软件定时器

/* 创建软件定时器1-2 */
	timer1_handle = xTimerCreate( "timer1",
									10,			//定时时间10ms
									pdTRUE,//	周期定时器
									(void *)1,		//	定时器编号
									timer1_callback);		//定时器回调函数
	xTimerStart(timer1_handle,portMAX_DELAY);             //开启软件定时器
	//xTimerStop(timer1_handle,portMAX_DELAY); 			  //暂停软件定时器

二、软件定时器回调函数

/**************************************************************************
函数功能：软件定时器1中断服务函数 定时读取编码器数值并进行速度闭环控制 10ms进入一次
入口参数：无
返回  值：无
**************************************************************************/
//节省资源 -- 主要内存够多就可以无限个软件定时器
void timer1_callback( TimerHandle_t pxTimer )
{
	Encoder1=Read_Encoder((u8)4);   //读取当前编码器读数，即速度
	Encoder2=Read_Encoder((u8)2);   //读取当前编码器读数，即速度
	TIM_SetCompare4(TIM1, angle);
	if(MortorRun) //如果按键按下，运行电机控制程序   默认为1 开启电机
	{
		PWM1=Velocity1_FeedbackControl(TargetVelocity, Encoder1); //速度环闭环控制
		PWM2=Velocity2_FeedbackControl(TargetVelocity, Encoder2); //速度环闭环控制
		SetPWM(PWM1,PWM2); //设置PWM
	
	}
	else //如果按键再次按下，电机停止
	{
		PWM1 = 0;
		PWM2 = 0;
		SetPWM(0,0); //设置PWM		  
	}
}

三、PID控制编码电机

定时器中断计数，记录AB相的高低电平的跳转次数来判断编码电机的速度

/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM4_IRQHandler(void)
{ 		    		  			    
	if(TIM4->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM4->SR&=~(1<<0);//清除中断标志位 	    
}

/**************************************************************************
函数功能：TIM4中断服务函数
入口参数：无
返回  值：无
**************************************************************************/
void TIM2_IRQHandler(void)
{ 		    		  			    
	if(TIM2->SR&0X0001)//溢出中断
	{    				   				     	    	
	}				   
	TIM2->SR&=~(1<<0);//清除中断标志位 	    
}



/**************************************************************************
函数功能：读取TIM4编码器数值
入口参数：无
返回  值：无
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
	int Encoder_TIM;
	switch(TIMX)
	{
		case 2:
			Encoder_TIM=TIM2->CNT; //读取计数
			if(Encoder_TIM>0xefff)Encoder_TIM=Encoder_TIM-0xffff; //转化计数值为有方向的值，大于0正转，小于0反转。
	                                                      //TIM4->CNT范围为0-0xffff，初值为0。

			TIM2->CNT=0; //读取完后计数清零
		break;
		case 4:
			Encoder_TIM=TIM4->CNT; //读取计数
			if(Encoder_TIM>0xefff)Encoder_TIM=Encoder_TIM-0xffff; //转化计数值为有方向的值，大于0正转，小于0反转。
	                                                      //TIM4->CNT范围为0-0xffff，初值为0。
			TIM4->CNT=0; //读取完后计数清零
		break;
	}
	return Encoder_TIM; //返回值
}

pid控制速度，本次采用的速度闭环控制（适合电机控制，位置适合舵机等角度控制），本次只用到了PI，未用到 D--（PI已经可以做到很好的控制了）

 

/**************************************************************************
函数功能：速度闭环PID控制(实际为PI控制)
入口参数：目标速度 当前速度
返回  值：速度控制值
根据增量式离散PID公式 
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此类推 
ControlVelocity代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
ControlVelocity+=Kp[e（k）-e(k-1)]+Ki*e(k)
Velcity_Kp=20,  Velcity_Ki=5,  Velcity_Kd; //相关速度PID参数
**************************************************************************/
int Velocity1_FeedbackControl(int TargetVelocity, int CurrentVelocity) //目标值 当前值
{
		int Bias;  //当前误差
		static int ControlVelocity, Last_bias; //静态变量，函数调用结束后其值依然存在  需要的增量  上一次误差值
		
		Bias=TargetVelocity-CurrentVelocity; //求速度偏差   目标值-当前值
		
		ControlVelocity+=Velcity_Kp*(Bias-Last_bias)+Velcity_Ki*Bias;  //增量式PI控制器  
                                                                       //Velcity_Kp*(Bias-Last_bias) 作用为限制加速度
	                                                                   //Velcity_Ki*Bias             速度控制值由Bias不断积分得到 偏差越大加速度越大
		//pwm限幅
		if(ControlVelocity>=7200) ControlVelocity = 7200;
		if(ControlVelocity<=-7200) ControlVelocity = -7200;
		Last_bias=Bias;	
		return ControlVelocity; //返回速度控制值
}

int Velocity2_FeedbackControl(int TargetVelocity, int CurrentVelocity) //目标值 当前值
{
		int Bias;  //当前误差
		static int ControlVelocity, Last_bias; //静态变量，函数调用结束后其值依然存在  需要的增量  上一次误差值
		
		Bias=TargetVelocity-CurrentVelocity; //求速度偏差   目标值-当前值
		
		ControlVelocity+=Velcity_Kp*(Bias-Last_bias)+Velcity_Ki*Bias;  //增量式PI控制器  
                                                                       //Velcity_Kp*(Bias-Last_bias) 作用为限制加速度
	                                                                   //Velcity_Ki*Bias             速度控制值由Bias不断积分得到 偏差越大加速度越大
		//pwm限幅
		if(ControlVelocity>=7200) ControlVelocity = 7200;
		if(ControlVelocity<=-7200) ControlVelocity = -7200;
		Last_bias=Bias;	
		return ControlVelocity; //返回速度控制值
}

1.前轮转向控制

        前轮采用舵机转向控制，由于采用舵机PID控制的效果不明显则未采用，但其代码以写，在末尾给出。
        舵机控制需要50HZ的pwm信号，本次所用TIM1的通道4来输出PWM信号控制舵机，高级定时器1的频率为36MHz，

Servo_PWM_Init(9999,71); //=====初始化PWM50HZ驱动 舵机 500--2500 1500为0°

舵机初始化：
 

/*****************   *********************************************************
函数功能：舵机PWM以及定时中断初始化
入口参数：入口参数：arr：自动重装值  psc：时钟预分频数 
返回  值：无
**************************************************************************/
void Servo_PWM_Init(u16 arr,u16 psc) 
{		 	
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	
	TIM_OCInitTypeDef TIM_OCInitStruct;
  GPIO_InitTypeDef GPIO_InitStruct;                             
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);   //使能定时器1时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);  //使能GPIOA的时钟
	
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;          //复用推挽输出
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_11;                //PA11
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOA,&GPIO_InitStruct);
	
	TIM_TimeBaseInitStruct.TIM_Period = arr;              //设定计数器自动重装值 
	TIM_TimeBaseInitStruct.TIM_Prescaler  = psc;          //设定预分频器
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//TIM向上计数模式
	TIM_TimeBaseInitStruct.TIM_ClockDivision = 0;         //设置时钟分割
	TIM_TimeBaseInit(TIM1,&TIM_TimeBaseInitStruct);       //初始化定时器
	
//	TIM_ITConfig(TIM1,TIM_IT_Update,ENABLE);               //使能定时器中断
//	
//	NVIC_InitStruct.NVIC_IRQChannel = TIM1_UP_IRQn;        //使能外部中断通道
//	NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;           //使能外部中断通道
//	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1; //抢占优先级1
//	NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;        //响应优先级1
//	NVIC_Init(&NVIC_InitStruct);
	
	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;             //选择PWM1模式
	TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStruct.TIM_Pulse = 0;                            //设置待装入捕获比较寄存器的脉冲值
	TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;     //设置输出极性
	TIM_OC4Init(TIM1,&TIM_OCInitStruct);                       //初始化输出比较参数
	
	TIM_OC4PreloadConfig(TIM1,TIM_OCPreload_Enable);   //CH4使能预装载寄存器
	
	TIM_CtrlPWMOutputs(TIM1,ENABLE);  //高级定时器输出需要设置这句
	
	TIM_ARRPreloadConfig(TIM1, ENABLE);                //使能TIM1在ARR上的预装载寄存器
	
	TIM1->CCR4=1500;       //默认角度为90度
	
	TIM_Cmd(TIM1,ENABLE);                              //使能定时器1
} 

//extern float Position_Kp,  Position_Ki,  Position_Kd; //电机相关速度PID参数
//float position_PID1(float Encoder , float Target)
//{
//	static float Bias, Pwm, Integrat_bias, Last_Bias;
//	Bias = Target - Encoder;//计算误差
//	Integrat_bias += Bias;//求出偏差的积分
//	Pwm = Position_Kp*Bias/50+Position_Kd*(Bias-Last_Bias)/100;
//	Last_Bias = Bias;
//	return Pwm;
//}

设置舵机角度可有2种方法，
方法一：TIM1->CCR4=1500; //直接对寄存器写值，值的 范围为自动重装载值的最大值到最小值，本次为舵机控制，所以500<=TIM1->CCR4=>2500 即为0°-180°
方法二：TIM_SetCompare4(TIM1, 1500);
//舵机的响应速度不够快！！ 

解决办法
只有高级定时器才会这样

 

 

 

参考链接：STM32高级定时器PWM输出响应延迟解决办法_21考研人的博客-CSDN博客

2.串口1输出电机以及舵机的数据

创建串口1任务，任务优先级为2

//串口1 200ms发送数据到串口调试助手
#define U1_PRIN_STACK_SIZE	128		//128*4个字节
TaskHandle_t u1_prin_handle;
void u1_prin(void * pvParameters);

xTaskCreate( (TaskFunction_t    	) u1_prin,
				 (char * 				) "u1_prin", 
				 (configSTACK_DEPTH_TYPE) U1_PRIN_STACK_SIZE,
				 (void *				) NULL,
				 (UBaseType_t			) 2,
				 (TaskHandle_t *		) &u1_prin_handle );

void u1_prin(void * pvParameters)
{
	while(1)
	{
		printf("TargetVelocity：%d\r\n",TargetVelocity);
		printf("Encoder1：%d\tEncoder2: %d\r\n",Encoder1,Encoder2);
		printf("电机1转速：%.3fr/s\t电机2转速：%.3fr/s\r\n", Encoder1/10.04,Encoder2/10.04);
		printf("pwm1 is %d\tpwm2 is %d\r\n",PWM1,PWM2);
		vTaskDelay(200);
	}
}

 3.串口3接收指令及其处理指令

创建串口3任务，任务优先级为3：在正点原子串口中断函数里更改了一下，由于接收的是数据为json数据，所以需要做判断。
json 数据的格式为: {"data":"123"} 将串口接收结束标志符由0x0d 0x0a改成 ‘ } ’

为什么会是json的数据格式？

从服务器（阿里云物联网平台）下发的数据的格式为json

//串口3 接收无线数据
#define U3_RECE_STACK_SIZE	128		//128*4个字节
TaskHandle_t u3_rece_handle;
void u3_rece(void * pvParameters);


xTaskCreate( (TaskFunction_t    	) u3_rece,
				 (char * 				) "u3_rece", 
				 (configSTACK_DEPTH_TYPE) U3_RECE_STACK_SIZE,
				 (void *				) NULL,
				 (UBaseType_t			) 3,
				 (TaskHandle_t *		) &u3_rece_handle );

void u3_rece(void * pvParameters)
{
	u16 t;  
	u16 len;
	while(1)
	{
		if(USART3_RX_STA&0x8000)
		{	
			speed=angle =0;			
			len=USART3_RX_STA&0x3fff;//得到此次接收到的数据长度
			printf("\r\n您发送的消息为:\r\n\r\n");
//			for(t=0;t=6&&t<8){speed = speed*10 +(USART3_RX_BUF[t]-'0');}
				else if(t>=8&&t(USART3_REC_LEN-1))USART3_RX_STA=0;//接收数据错误,重新开始接收	  
				}		 
			}
	    }   		 
   
	}  											 
} 

 

---

本次所显示小车的舵机转向控制，编码电机的PI算法控制以及接收服务器发来的数据并进行解析数据