【单片机项目】平衡小车(三) 软件设计

版权声明:转载请注明出处 https://blog.csdn.net/a568713197/article/details/83019117

    前俩篇博客记录了平衡小车的控制流程和硬件设计,本篇博客将讲解平衡小车的部分驱动以及核心控制算法。

    1、电机驱动

        motor.h

#ifndef __MOTOR_H
#define __MOTOR_H
#include "sys.h"
#include <stm32f10x.h>


#define PWM_LEFT  		TIM1->CCR1
#define PWM_RIGHT 		TIM1->CCR4
#define AIN1  			PBout(13) 
#define AIN2  			PBout(12)
#define BIN1  			PBout(14)
#define BIN2  			PBout(15)


void MOTOR_PWM_Init(u16 arr,u16 psc);
void MOTOR_IO_Init(void);

#endif

       motor.c

#include "motor.h"
#include "delay.h"


//电机PWM初始化
//arr:自动重装值
//psc:时钟预分频数
//定时器1 通道1,通道4
void MOTOR_PWM_Init(u16 arr,u16 psc)
{   
	RCC->APB2ENR|=1<<11;    		//使能TIM1时钟	
	RCC->APB2ENR|=1<<2;    			//使能PORTA时钟
	RCC->APB2ENR|=1<<3;    			//使能PORTB时钟
	//电机PWM引脚使能
	GPIOA->CRH&=0XFFFF0FF0;			//PA8、11输出	
	GPIOA->CRH|=0X0000B00B;			//复用功能输出 	

	TIM1->ARR=arr;             	//设定计数器自动重装值 
	TIM1->PSC=psc;             		//预分频器不分频
	TIM1->CCMR2|=6<<12;        		//CH4 PWM1模式	
	TIM1->CCMR1|=6<<4;         		//CH1 PWM1模式	
	TIM1->CCMR2|=1<<11;        		//CH4预装载使能	 
	TIM1->CCMR1|=1<<3;         		//CH1预装载使能	  
	TIM1->CCER|=1<<12;         		//CH4输出使能	   
	TIM1->CCER|=1<<0;          		//CH1输出使能	
	TIM1->BDTR |= 1<<15;       		//TIM1必须要这句话才能输出PWM
	TIM1->CR1=0x8000;          		//ARPE使能 
	TIM1->CR1|=0x01;          		//使能定时器1 	
}


//电机IO初始化
//PB12、13、14、15
//AIN1->B13		AIN2->B12
//BIN1->B14		BIN2->B15
void MOTOR_IO_Init(void)
{
	RCC->APB2ENR|=1<<3;    			//使能PORTB时钟
	GPIOB->CRH&=0X0000FFFF;			//PB12、13、14、15输出	
	GPIOB->CRH|=0X33330000;			//推挽功能输出 	
}


     我把电机的驱动分了俩个部分,一部分是TIM1输出的俩路PWM的初始化,另一部分是控制电机转动方向的IO口的初始化,很常规的,就不细说了。

    2、编码器驱动

     编码器是平衡小车中至关重要的部分,来获取电机的转速,STM32的定时器有编码器模式,通过硬件四分频来读取编码器,使用起来也是简单方便的,比自己写输入捕获简单的多,每个编码器需要一个定时器,我这里只用了TIM2和TIM4。

    encoder.h

#ifndef __ENCODER_H
#define __ENCODER_H
#include <sys.h>	 




#define ENCODER_TIM_PERIOD (u16)(65535)   //不可大于65535 因为F103的定时器是16位的。
void Encoder_Init_TIM2(void);
void Encoder_Init_TIM4(void);
int Read_Encoder(u8 TIMX);




#endif

    encoder.c

#include "encoder.h"




/**************************************************************************
函数功能:把TIM2初始化为编码器接口模式(左编码器)
入口参数:无
返回  值:无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
	RCC->APB1ENR|=1<<0;     //TIM2时钟使能
	RCC->APB2ENR|=1<<2;    //使能PORTA时钟
	GPIOA->CRL&=0XFFFFFF00;//PA0 PA1
	GPIOA->CRL|=0X00000044;//浮空输入
	/* 把定时器初始化为编码器模式 */ 
	TIM2->PSC = 0x0;//预分频器
	TIM2->ARR = ENCODER_TIM_PERIOD-1;//设定计数器自动重装值 
  TIM2->CCMR1 |= 1<<0;          //输入模式,IC1FP1映射到TI1上
  TIM2->CCMR1 |= 1<<8;          //输入模式,IC2FP2映射到TI2上
  TIM2->CCER |= 0<<1;           //IC1不反向
  TIM2->CCER |= 0<<5;           //IC2不反向
	TIM2->SMCR |= 3<<0;	          //SMS='011' 所有的输入均在上升沿和下降沿有效
	TIM2->CR1 |= 0x01;    //CEN=1,使能定时器
}



/**************************************************************************
函数功能:把TIM4初始化为编码器接口模式(右编码器)
入口参数:无
返回  值:无
**************************************************************************/
void Encoder_Init_TIM4(void)
{
	RCC->APB1ENR|=1<<2;     //TIM4时钟使能
	RCC->APB2ENR|=1<<3;    //使能PORTb时钟
	GPIOB->CRL&=0X00FFFFFF;//PB6 PB7
	GPIOB->CRL|=0X44000000;//浮空输入
	/* 把定时器初始化为编码器模式 */ 
	TIM4->PSC = 0x0;//预分频器
	TIM4->ARR = ENCODER_TIM_PERIOD-1;//设定计数器自动重装值 
	TIM4->CCMR1 |= 1<<0;          //输入模式,IC1FP1映射到TI1上
	TIM4->CCMR1 |= 1<<8;          //输入模式,IC2FP2映射到TI2上
	TIM4->CCER |= 0<<1;           //IC1不反向
	TIM4->CCER |= 0<<5;           //IC2不反向
	TIM4->SMCR |= 3<<0;	          //SMS='011' 所有的输入均在上升沿和下降沿有效
	TIM4->CR1 |= 0x01;    //CEN=1,使能定时器
}



/**************************************************************************
函数功能:单位时间读取编码器计数
入口参数:定时器
返回  值:速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
    int Encoder_TIM;    
   switch(TIMX)
	 {
	   case 2:  Encoder_TIM= (short)TIM2 -> CNT;  TIM2 -> CNT=0;break;
		 case 3:  Encoder_TIM= (short)TIM3 -> CNT;  TIM3 -> CNT=0;break;	
		 case 4:  Encoder_TIM= (short)TIM4 -> CNT;  TIM4 -> CNT=0;break;	
		 default:  Encoder_TIM=0;
	 }
		return Encoder_TIM;
}


    3、ADC初始化以及获取电压值

     因为STM32的ADC输入电压最高为3.3V,所以我搭了一个分压电路来采集4分频以后的电压值,然后再通过计算来得出真实电压值。

     adc.h

#ifndef __ADC_H
#define __ADC_H	


#include "sys.h"

#define Battery_Ch 4
void Adc_Init(void);
u16 Get_Adc(u8 ch);
int Get_battery_volt(void);  

 
#endif 

   adc.c

#include "adc.h"





/**************************************************************************
函数功能:ACD初始化电池电压检测
入口参数:无
返回  值:无
**************************************************************************/
void  Adc_Init(void)
{    
  //先初始化IO口
 	RCC->APB2ENR|=1<<2;    //使能PORTA口时钟 
	GPIOA->CRL&=0XFFF0FFFF;//PA4 anolog输入 
	RCC->APB2ENR|=1<<9;    //ADC1时钟使能	  
	RCC->APB2RSTR|=1<<9;   //ADC1复位
	RCC->APB2RSTR&=~(1<<9);//复位结束	    
	RCC->CFGR&=~(3<<14);   //分频因子清零	
	//SYSCLK/DIV2=12M ADC时钟设置为12M,ADC最大时钟不能超过14M!
	//否则将导致ADC准确度下降! 
	RCC->CFGR|=2<<14;      	 
	ADC1->CR1&=0XF0FFFF;   //工作模式清零
	ADC1->CR1|=0<<16;      //独立工作模式  
	ADC1->CR1&=~(1<<8);    //非扫描模式	  
	ADC1->CR2&=~(1<<1);    //单次转换模式
	ADC1->CR2&=~(7<<17);	   
	ADC1->CR2|=7<<17;	   //软件控制转换  
	ADC1->CR2|=1<<20;      //使用用外部触发(SWSTART)!!!	必须使用一个事件来触发
	ADC1->CR2&=~(1<<11);   //右对齐	 
	ADC1->SQR1&=~(0XF<<20);
	ADC1->SQR1&=0<<20;     //1个转换在规则序列中 也就是只转换规则序列1 			   
	//设置通道4的采样时间
	ADC1->SMPR2&=0XFFF0FFFF; //采样时间清空	  
	ADC1->SMPR2|=7<<12;      // 239.5周期,提高采样时间可以提高精确度	 

	ADC1->CR2|=1<<0;	    //开启AD转换器	 
	ADC1->CR2|=1<<3;        //使能复位校准  
	while(ADC1->CR2&1<<3);  //等待校准结束 			 
    //该位由软件设置并由硬件清除。在校准寄存器被初始化后该位将被清除。 		 
	ADC1->CR2|=1<<2;        //开启AD校准	   
	while(ADC1->CR2&1<<2);  //等待校准结束 
}		

/**************************************************************************
函数功能:AD采样
入口参数:ADC1 的通道
返回  值:AD转换结果
**************************************************************************/
u16 Get_Adc(u8 ch)   
{
	//设置转换序列	  		 
	ADC1->SQR3&=0XFFFFFFE0;//规则序列1 通道ch
	ADC1->SQR3|=ch;		  			    
	ADC1->CR2|=1<<22;       //启动规则转换通道 
	while(!(ADC1->SR&1<<1));//等待转换结束	 	   
	return ADC1->DR;		//返回adc值	
}

/**************************************************************************
函数功能:读取电池电压 
入口参数:无
返回  值:电池电压 单位MV
**************************************************************************/
int Get_battery_volt(void)   
{  
	int Volt;//电池电压
	Volt=Get_Adc(Battery_Ch)*3.3*4.0*100/1.0/4096;	//电阻分压,四倍	
	if(Volt>1260)Volt=1260;
	return Volt;
}



    4、中断初始化

    exit.h

#ifndef __EXTI_H
#define __EXIT_H	 
#include "sys.h"



#define INT PBin(5)   //PB5连接到MPU6050的中断引脚
void EXTI_Init(void);	//外部中断初始化		


#endif

    exit.c

#include "exti.h"

/**************************************************************************
函数功能:外部中断初始化
入口参数:无
返回  值:无 
**************************************************************************/
void EXTI_Init(void)
{
	RCC->APB2ENR|=1<<3;    //使能PORTB时钟	   	 
	GPIOB->CRL&=0XFF0FFFFF; 
	GPIOB->CRL|=0X00800000;//PB5上拉输入
  GPIOB->ODR|=1<<5;      //PB5上拉	
	Ex_NVIC_Config(GPIO_B,5,FTIR);		//下降沿触发
	MY_NVIC_Init(2,1,EXTI9_5_IRQn,2);  	//抢占2,子优先级1,组2
}

   5、控制程序

/**************************************************************************
函数功能:所有的控制代码都在这里面
         5ms定时中断由MPU6050的INT引脚触发
         严格保证采样和数据处理的时间同步				 
**************************************************************************/
int EXTI9_5_IRQHandler(void) 
{    
	 if(PBin(5)==0)		
	{   
	
		EXTI->PR=1<<5;                                                      //清除LINE5上的中断标志位   
		Flag_Target=!Flag_Target;
		if(delay_flag==1)
		{
			if(++delay_50==10)										   //给主函数提供50ms的精确延时 以刷新OLED及发送数据
			{
				delay_50=0;
				delay_flag=0;
			}
		}			
		if(Flag_Target==1)                                                  	//5ms读取一次陀螺仪和加速度计的值,更高的采样频率可以改善卡尔曼滤波和互补滤波的效果
		{
			Get_Angle(Way_Angle);												//===更新姿态
			if(++Flash_R_Count==150&&Angle_Balance>30)
				Flash_Read();									//读取FLASH PID值
			Voltage_Temp=Get_battery_volt();					//读取电池电压
			Voltage_Count++;
			Voltage_All+=Voltage_Temp;
			if(Voltage_Count==100)												//100次采样计算平均值
			{
				Voltage=Voltage_All/100;
				Voltage_Count=0;
				Voltage_All=0;				
			}
			return 0;	                                               
		}                                                                   	//10ms控制一次,为了保证M法测速的时间基准,首先读取编码器数据
		Encoder_Left=Read_Encoder(2);                                      		//===读取编码器的值,因为两个电机的旋转了180度的,所以对其中一个取反,保证输出极性一致
		Encoder_Right=Read_Encoder(4);                                      	//===读取编码器的值
	  	Get_Angle(Way_Angle);                                                   //===更新姿态	 
		HCER_04_Get_Length();													//===读取距离值
		if(Bi_zhang==0)
			Led_Flash(100);                                      				//===LED闪烁;常规模式 1s改变一次指示灯的状态	
		if(Bi_zhang==1)
			Led_Flash(0);                                        				//===LED闪烁;避障模式 指示灯常亮	
 		Balance_Pwm =balance(Angle_Balance,Gyro_Balance);                   	//===平衡PID控制	
		Velocity_Pwm=velocity(Encoder_Left,Encoder_Right);                  	//===速度环PID控制	 记住,速度反馈是正反馈,就是小车快的时候要慢下来就需要再跑快一点
		Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn);          			//===转向环PID控制
		//printf("Gyro_Turn: %f  Turn_Pwm: %d\r\n",Gyro_Turn,Turn_Pwm);
 		Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm;                            	//===计算左轮电机最终PWM
 	  	Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm;                            	//===计算右轮电机最终PWM
   		Xianfu_Pwm();                                                       	//===PWM限幅
        if(Turn_Off(Angle_Balance,Voltage)==0)                                      		//===如果不存在异常
 			Set_Pwm(Moto1,Moto2);                                               //===赋值给PWM寄存器  
		if(Bi_zhang==0)
			Led_Flash(100);														//===LED闪烁;常规模式 1s改变一次指示灯的状态
		else
			Led_Flash(0);														//===LED闪烁;避障模式 指示灯常亮
		if(Pick_Up(Acceleration_Z,Angle_Balance,Encoder_Left,Encoder_Right))//===检查是否小车被那起
			Flag_Stop=1;	                                                      //===如果被拿起就关闭电机
		if(Put_Down(Angle_Balance,Encoder_Left,Encoder_Right))              //===检查是否小车被放下
			Flag_Stop=0;	                                                      //===如果被放下就启动电机
		if(Turn_Off(Angle_Balance,Voltage)==0)                              //===如果不存在异常
 			Set_Pwm(Moto1,Moto2);                                               //===赋值给PWM寄存器  
	}       	
	 return 0;	  
} 

    6、直立环

/**************************************************************************
函数功能:直立PD控制
入口参数:角度、角速度
返回  值:直立控制PWM
**************************************************************************/
int balance(float Angle,float Gyro)
{  
   float Bias,kp=410,kd=1.4;
	 int balance;
	 Bias=Angle-ZHONGZHI;       //===求出平衡的角度中值 和机械相关
	 balance=kp*Bias+Gyro*kd;   //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
	 return balance;
}

   7、速度环

/**************************************************************************
函数功能:速度PI控制 修改前进后退速度,请修Target_Velocity,比如,改成60就比较慢了
入口参数:左轮编码器、右轮编码器
返回  值:速度控制PWM
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{  
    static float Velocity,Encoder_Least,Encoder,Movement;
	static float Encoder_Integral,Target_Velocity;
	float kp=-120,ki=-0.6;
	//============遥控前进后退部分====================//
	if(Bi_zhang!=0&&Flag_sudu==1)												//进入避障模式,自动进入低速模式
		Target_Velocity=55;
	else
		Target_Velocity=110;
	if(Flag_Qian==1)															//前进标志位置1
		Movement=-Target_Velocity/Flag_sudu;		
	else if(Flag_Hou==1)														//后退标志位置1
		Movement=Target_Velocity/Flag_sudu;
	else																		//停止
		Movement=0;
	if(Bi_zhang==1&&Flag_Left!=1&&Flag_Right!=1)
	{
		if(Distance<500)
			Movement=Target_Velocity/Flag_sudu;
	}
	//=============速度PI控制器=======================//	
	Encoder_Least =(Encoder_Left+Encoder_Right)-0;                    			//===获取最新速度偏差==测量速度(左右编码器之和)-目标速度(此处为零) 
	Encoder *= 0.8;		                                                		//===一阶低通滤波器       
	Encoder += Encoder_Least*0.2;	                                    		//===一阶低通滤波器    
	Encoder_Integral +=Encoder;                                       			//===积分出位移 积分时间:10ms
	Encoder_Integral=Encoder_Integral-Movement;                       			//===接收遥控器数据,控制前进后退
	if(Encoder_Integral>10000)  	
		Encoder_Integral=10000;             									//===积分限幅
	if(Encoder_Integral<-10000)	
		Encoder_Integral=-10000;              									//===积分限幅	
	Velocity=Encoder*kp+Encoder_Integral*ki;                          			//===速度控制	
	if(Turn_Off(Angle_Balance,Voltage)==1||Flag_Stop==1)						//电机关闭后清除积分
		Encoder_Integral=0;
	return Velocity;
}

   8、转向环

/**************************************************************************
函数功能:转向控制  修改转向速度,请修改Turn_Amplitude即可
入口参数:左轮编码器、右轮编码器、Z轴陀螺仪
返回  值:转向控制PWM
**************************************************************************/
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
	static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
	float Turn_Amplitude=88/Flag_sudu,Kp=42,Kd=-0.7;     
	//=============遥控左右旋转部分=======================//
  	if(1==Flag_Left||1==Flag_Right)                      //这一部分主要是根据旋转前的速度调整速度的起始速度,增加小车的适应性
	{
		if(++Turn_Count==1)
		Encoder_temp=myabs(encoder_left+encoder_right);
		Turn_Convert=50/Encoder_temp;
		if(Turn_Convert<0.6)
			Turn_Convert=0.6;
		if(Turn_Convert>3)
			Turn_Convert=3;
	}	
	else
	{
		Turn_Convert=0.9;
		Turn_Count=0;
		Encoder_temp=0;
	}			
	if(1==Flag_Left)	           
		Turn_Target-=Turn_Convert;
	else if(1==Flag_Right)	     
		Turn_Target+=Turn_Convert; 
	else Turn_Target=0;
    if(Turn_Target>Turn_Amplitude)  
		Turn_Target=Turn_Amplitude;    //===转向速度限幅
	if(Turn_Target<-Turn_Amplitude) 
		Turn_Target=-Turn_Amplitude;
	if(Flag_Qian==1||Flag_Hou==1)  
		Kd=-0.7;        
	else Kd=0;   //转向的时候取消陀螺仪的纠正 有点模糊PID的思想
  	//=============转向PD控制器=======================//
	Turn=-Turn_Target*Kp -gyro*Kd;                 //===结合Z轴陀螺仪进行PD控制
	return Turn;
}

     以上就是平衡小车项目中的吸血核心代码,注释也是写的很清楚的,其他的比如MPU6050,它的驱动是相当多的,我们不需要去完全理解它的所有代码,前期只要能读出6个数据(三个方向的角度及角加速度)就可以了,还有比如OLED、蓝牙、超声波的驱动都是很简单的,如果想要看的话可以去看我以前智能小车的博客,里面有详细的代码。

    平衡小车的总结博客就结束了,平衡小车是我学了PID后做的第一个项目,在代码上借鉴了好多平衡小车之家的资料,在项目中也学到了好多东西,受益匪浅。

    我平衡小车的演示视频再B站:链接点我

展开阅读全文

没有更多推荐了,返回首页