智能车电磁循迹模块化（进阶）

A_Frost

已于 2023-09-28 21:32:01 修改

阅读量1.7k

点赞数 9

分类专栏：智能车文章标签：开发语言单片机汽车

于 2023-07-09 11:29:57 首次发布

本文链接：https://blog.csdn.net/A_Frost/article/details/131620095

版权

智能车专栏收录该内容

5 篇文章 7 订阅

订阅专栏

大家应该都喜欢模块化后的函数，可以直接调用。我之前学习的时候经过总结，把大部分电磁循迹的函数模块化了，大家以这些为参考，提升自己的理解。

关于电磁循迹模块化的内容大家可以看看我之前写的博客(16条消息) 智能车电磁循迹模块化（差速舵机通用）_智能车电磁循迹程序_A_Frost的博客-CSDN博客我把PID等算法写成了模块化的函数，尽量使用局部变量和形参，大家可以方便调用。

电感归一化

好处是增强赛道的适应性

各电感值最大最小值最后是全局变量，方便全局有时候会用到。

再此之前得先对赛道进行扫描处理


float AD_L_Max=2000,AD_L_Min=1000,AD_R_Max=2000,AD_R_Min=1000;
float AD_M_Max=2000,AD_M_Min=1000;
float AD_LV_Max=100,AD_LV_Min=50,AD_RV_Max=100,AD_RV_Min=50;
void Inductor_Scan()
{
	
	uint16 L_temp[10],R_temp[10],M_temp[10],LV_temp[10],RV_temp[10];
	int i;
	 for(i=0;i<10;i++)  
     {    
		 Inductor_Smooth_Filter_Get_val();//获取电感值，在前一篇博客
		 L_temp[i]=AD_L;
       if(L_temp[i]>AD_L_Max) AD_L_Max=L_temp[i];
	   if(L_temp[i]<AD_L_Min) AD_L_Min=L_temp[i];
	   
		 R_temp[i]=AD_R;
       if(R_temp[i]>AD_R_Max) AD_R_Max=R_temp[i];
       if(R_temp[i]<AD_R_Min)AD_R_Min=R_temp[i];
		
		 M_temp[i]=AD_M;
      if(M_temp[i]>AD_M_Max)AD_M_Max=M_temp[i];
	  if(M_temp[i]<AD_M_Min)AD_M_Min=M_temp[i];
		 		 LV_temp[i]=AD_LV;//L1  AD_L
       if(LV_temp[i]>AD_LV_Max) AD_LV_Max=LV_temp[i];
	   if(LV_temp[i]<AD_LV_Min) AD_LV_Min=LV_temp[i];
	   
		 RV_temp[i]=AD_RV;
       if(RV_temp[i]>AD_RV_Max) AD_RV_Max=RV_temp[i];
       if(RV_temp[i]<AD_RV_Min)AD_RV_Min=RV_temp[i];
     } 	
	
		AD_L=(AD_L-AD_L_Min)/(AD_L_Max-AD_L_Min);
		AD_R=(AD_R-AD_R_Min)/(AD_R_Max-AD_R_Min);
		AD_M=(AD_M-AD_M_Min)/(AD_M_Max-AD_M_Min);
		AD_LV=(AD_LV-AD_LV_Min)/(AD_LV_Max-AD_LV_Min);
		AD_RV=(AD_RV-AD_RV_Min)/(AD_RV_Max-AD_RV_Min);
	
}

之后才是归一化


float AD_L=0,AD_R=0,AD_M=0;
float AD_LE=0;AD_RE=0;
//先对AD_L,AD_M,AD_R赋值之后再调用此函数
void Inductor_Get_Nomolize_val(void)
{	
		AD_L=(AD_L-AD_L_Min)/(AD_L_Max-AD_L_Min);
		AD_R=(AD_R-AD_R_Min)/(AD_R_Max-AD_R_Min);
		AD_M=(AD_M-AD_M_Min)/(AD_M_Max-AD_M_Min);
		AD_LV=(AD_LV-AD_LV_Min)/(AD_LV_Max-AD_LV_Min);
		AD_RV=(AD_RV-AD_RV_Min)/(AD_RV_Max-AD_RV_Min);	
}

这个函数可以在计算控制值之前在主函数调用。

我写过全部都是局部变量形参的，但是说实话这些变量，整个代码都有贯穿，全模块化的函数就不给出了。

串级PID(加速度环)

对于这种串级pid，写出函数更好理解，对于单一变量处理不易理解。我们一样写一个函数，参数为左右占空比，控制速度。之后再以此函数为基础再写一个速度环函数就行。

我的函数为 CarForward(duty_r,duty_l);（这种函数很好写的，就是填参数控制左右占空比）

（大家可以自己安库函数实现此功能，之前我的那一篇博客里面也有写上面这种函数的思路，可以直接参考。

假设你写好了这个函数 CarForward(duty_r,duty_l);

计算速度

一般都有库函数直接读取

//编码器
#define SPEEDL_PLUSE   CTIM0_P34
#define SPEEDR_PLUSE   CTIM3_P04
//定义方向引脚
#define L_DIR     P35
#define R_DIR     P53
//我买的是带方向编码器，根据L_DIR,R_DIR的数值就能判断方向，这个可以TFTshow出来，看看反了没
float VeL_actual = 0.0,VeR_actual = 0.0;
//ctimer_count_read   ctimer_count_clean 为库函数大家找找自己的单片机库，或者找厂家要

void  Velocity_Calu()
{
	
	VeL_actual =ctimer_count_read(SPEEDL_PLUSE); //右轮速度
	VeR_actual =ctimer_count_read(SPEEDR_PLUSE); //左轮速度	
	ctimer_count_clean(SPEEDL_PLUSE);//计数器清零
	ctimer_count_clean(SPEEDR_PLUSE);	
	if (L_DIR == 0)VeL_actual = -VeL_actual;//得自己尝试是0还是1
	if (R_DIR == 1)VeR_actual = -VeR_actual;		
}

速度环包含控制占空比函数

由于输出的占空比有时候可能有用，我把它当成全局变量，如果对于你们来说没啥用可以放函数里面。我之前写过一篇博客有介绍速度环函数。

/**
* @brief 增量式速度环
/*
优点：
①误动作时影响小，必要时可用逻辑判断的方法去掉出错数据。
②手动/自动切换时冲击小，便于实现无扰动切换。当计算机故障时，仍能保持原值。
③算式中不需要累加。控制增量Δu(k)的确定仅与最近3次的关。
缺点：
①积分截断效应大，有稳态误差；
②溢出的影响大。有的被控对象用增量式则不太好；
/*
* @param Goal_A目标左轮速度，Goal_B目标右轮速度
* @retval none
*/
//之后占空比就没用了，只是用编码器的值非线性表示
int16 duty_l=0,duty_r=0;//vr,vl
float V_Kp_L=29.2,V_Ki_L=4.69,V_Kp_R=41.1,V_Ki_R=2.80;//220 35 闭环

void Sudu_Huan_2(float Goal_A, float Goal_B) //增量式 (left,right);
{
	float VeL_out_Kp,VeL_out_Ki,VeR_out_Kp,VeR_out_Ki;
    static float error_l=0,error_r=0;last_error_l=0,last_error_r=0;
	Velocity_Calu();
	error_l = Goal_A - VeL_actual; 
     VeL_out_Kp=V_Kp_L * (error_l - last_error_l);
	 VeL_out_Ki=V_Ki_L * error_l;
	last_error_l=error_l;	
	duty_l=duty_l+VeL_out_Kp + VeL_out_Ki;
	
	error_r = Goal_B - VeR_actual; 
     VeR_out_Kp=V_Kp_R * (error_r - last_error_r);
	 VeR_out_Ki=V_Ki_R * error_r;
	last_error_r=error_r;	
	duty_r=duty_r+VeR_out_Kp + VeR_out_Ki;
      if(duty_l<-9000)duty_l=-9000;       
      if(duty_l>9000) duty_l=9000;
	  if(duty_r<-9000)duty_r=-9000;       
      if(duty_r>9000) duty_r=9000;
    CarForward(duty_r,duty_l);
		
}

方向环串速度环

之前忘记给方向环串速度环的函数了，现在加进去

//双电感控制算法
float Inductor_Fllow_Trail_Control_Val_2(float AD_input_L,float AD_input_R,float P,float D)
{
	static float last_error=0;
	float error;
	float output;
	error=(AD_input_R-AD_input_L)/(AD_input_L+AD_input_R);
	output=(P*error+D*(error-last_error));
	last_error=error;
	return output;
}

void Fllow_Trail_Done(float Vl0,float Vr0)
{ 			
	float Inductance_Control_val=0;
	Inductor_Getval();
	if(fg.Horizontal_Change_To_Vertical_Inductor_Fllow_Trail==0){
//	Inductance_Control_val=Inductor_Fllow_Trail_Control_Val(AD_L,AD_R,AD_M,I_Kp,I_Kd);//横三电感循迹
		Inductance_Control_val=Inductor_Fllow_Trail_Control_Val_2(AD_L,AD_R,I_Kp,I_Kd);//横双电感循迹
	}
	if(fg.Horizontal_Change_To_Vertical_Inductor_Fllow_Trail==1){
	Inductance_Control_val=Inductor_Fllow_Trail_Control_Val(AD_LV,AD_RV,AD_M,I_Kp_V,I_Kd_V);//竖三电感循迹
	}
	Correct_Val=Inductance_Control_val;
	//差速循迹
		
	Speed_Right=Vr0-Correct_Val;
	Speed_Left=Vl0+Correct_Val;
	
		if(fg.sudu_huan_2==1){//闭环
		if(Correct_Val>=2000) Correct_Val=2000;
		else if(Correct_Val<=-2000) Correct_Val=-2000;
		if(Speed_Right>=2000) Speed_Right=2000;
		if(Speed_Left>=2000) Speed_Left=2000;
	Sudu_Huan_2(Speed_Left,Speed_Right);//增量式速度环
	}
}

多段控制

有时候我们需要分段控制，多段控制的方法如下

先定义一些控制变量扔结构体里面，方面加很多标志位，写在头文件里面，里面你们很多是没用的。可以自行删减，我有时候并没有用到那么多标志位，多弄点，之后标志位多了再删去。方便修改。

typedef struct{
	//出界
	uint8 chujie;
	//出界不停车
	uint8 Chujie_Not_Stop;
	uint8 qishiwei,qscount_1,qscount_2;
	//菜单打开
	uint8 menu_open;
	//电感处理
	uint8 guiyi_hua,already_scan,gui_zhong,sao_miao;
	//坡道
	uint8 podao_1,podao_2,podao_3;
	//速度环
	uint8 sudu_huan,sudu_huan_2;
	//圆环
	uint8 yhcountL_1,yhcountL_2,yhcountL_3,yhcountL_4,yhcountL_5;
	uint8 yhcountR_1,yhcountR_2,yhcountR_3,yhcountR_4,yhcountR_5;
    uint8 yh_openL,yh_openR;
	//入库
	uint8 stop_1,stop_2,stop_3,already_ru,stop_4,stop_5,stop_6,stop_7,stop_8;
	uint8 ruku_open;
	//出库
	uint8 go_0,go_1,go_2,go_3,already_chu,L_chu,R_chu;uint8 chuku_open;
	uint16 go_ph_1,stop_ph_1;//平滑转弯
	//左右出入库控制
	uint8 chuku_L,chuku_R;
	//避障
	uint8 bz_1,bz_2,bz_3,bz_4,bz_5,bz_6,bz_7,bz_8,bz_9;
	uint8 bz_L,bz_R, bz_open;
	uint8 already_bz;
	//编码器积分测量测量,
	uint8 integral_ecod_open;
	//陀螺仪积分测量
	uint8 integral_gyro_open;
	uint8 Quaternion_Euler_angles_open;
	//历时测量
	float counter;uint8 time_open;
	//激光测距
	uint8 Jkg_open;
	//开关循迹
	uint8 xunji,Fllow_Trail;
		//转固定角启动位
		uint8 Turn_engle_Enable;
		//走固定时间启动位
		uint8 Go_Set_Time_Enable;
		//走固定路程启动位
		uint8 Go_Set_Distance_Enable;
		//for 进去
		uint8 For_To_Go;
	//蓝牙拟狗叫器
	uint16 Bt_counter;
	//切换竖电感循迹
	uint8 Horizontal_Change_To_Vertical_Inductor_Fllow_Trail;
	//确定速度
	uint8 Velocity_Have_Determined;
	//防止疯转
	uint8 Is_Crazy,Is_Crazy_Count;
	//单独获取电感值
	uint8 Obtained_Inductance_Separately;
	//堵转*
	uint8 Is_Stuck_In;	
}my;

之后定义结构体变量

my fg;//就是Flag我缩写了

我是使用if while 的形式多弄标志位就可以实现多段处理。

比如出库函数我是这样写的

中断函数里面


float distance_traveled,turn_engle;
//下面的东西放中断
if(fg.integral_ecod_open==1){
	Velocity_Calu();
	distance_traveled+=(VeL_actual+VeR_actual)/200;	
	}
else if(fg.integral_ecod_open==0)distance_traveled=0;


if(fg.integral_gyro_open==1){
	Get_Gyroval();		
	turn_engle+=gyro_z;//	
	}else if(fg.integral_gyro_open==0)turn_engle=0;

之后写一个模块化的函数随便在什么地方，通过标准位的关开就可以控制这个函数。


/**
* @brief 直接控制占空比走固定路程
* @param set_dutyL左占空比 set_dutyR右占空比 set_distance设定长度
/*@usuage fg.Go_Set_Distance_Enable=1;
	while(flag.next){
	Go_Fixed_Distance_Done(2,2000,2000);
		if(fg.Go_Set_Distance_Enable==0){
			//next Done
		}
	}
/*
* @retval none
*/

void Go_Fixed_Distance_Done(uint16 set_dutyL,uint16 set_dutyR,float set_distance)
//要关掉中断的循迹
{
	
	if(fg.Go_Set_Distance_Enable==1){
			fg.integral_ecod_open=1;
			if(distance_traveled<set_distance){
			CarForward(set_dutyR,set_dutyL);
			}
			else {
				fg.Go_Set_Distance_Enable=0;
				fg.integral_ecod_open=0;
			}
	}	
}





/**
* @brief 陀螺仪转向pid函数—>使差速车拥有类似舵机的转向
* @param Set_engle 设置角度负数右转，正数左转
Faster_Speed 快的速度，Slower_Speed 慢的速度 P D为参数
/*@usuage 
	fg.Turn_engle_Enable=1;
	在上一个判断置一，下一个循环执行转向函数
	Turn_engle_PD_Done(7000,3000,800,0.2,0.3);
/*
* @retval 无
*/
float PD_engle_val=0;
uint8 Control_enable_L,Control_enable_R;
void Turn_engle_PD_Done(float Set_engle,float Faster_Speed,float Slower_Speed,float P,float D)//要关闭中断的循迹
{
	static float last_engle_error=0;
	float engle_error;
	if(fg.Turn_engle_Enable==1){
//		fg.xunji=0;
		Out_Of_Bounds_Control();//
		fg.integral_gyro_open=1;
	if(Set_engle>0){//0开始变化 向左转动 左减右加
			if(turn_engle<Set_engle){
				engle_error=Set_engle-turn_engle;
				PD_engle_val=P*engle_error+D*(engle_error-last_engle_error);
				last_engle_error=engle_error;
				Control_enable_L=1;
				}
			else if(turn_engle>Set_engle){
				last_engle_error=0;	
				Control_enable_L=0;
				}
	}
	if(Set_engle<0){//-9000 now -6000//右转，左加右减
			if(turn_engle>Set_engle){
				engle_error=Set_engle-turn_engle;
				PD_engle_val=P*engle_error+D*(engle_error-last_engle_error);
				last_engle_error=engle_error;
				Control_enable_R=1;
				}
			else if(turn_engle<Set_engle){
				last_engle_error=0;	
				Control_enable_R=0;
				}
	}
	if(PD_engle_val>8000)PD_engle_val=8000;
	else if(PD_engle_val<-8000)PD_engle_val=-8000;

	if(Control_enable_L==1)//左转
	CarForward(Faster_Speed+PD_engle_val,Slower_Speed-PD_engle_val);//右加 左减
	else if(Control_enable_R==1)//右转
	CarForward(Slower_Speed+PD_engle_val,Faster_Speed-PD_engle_val);//R L
	
	if(Control_enable_R==0 ||Control_enable_L==0){
		fg.Turn_engle_Enable=0;//
		fg.integral_gyro_open=0;
//		CarForward(0,0);
	}
	}

当然也可以不模块化，按照下面的思路写也行。我差速车，控制的角度没四轮那么方便，上面的模块化大家看看就行，可以模仿写出类似的函数。

思路就是一段一段打开关闭标志位，循迹放中断。

大家可以看看我出入库的函数，以做参考

void chu_ku()//出库
{
	
	if(fg.already_chu==0){
		fg.xunji=0;
		fg.go_1=1;
		fg.Go_Set_Distance_Enable=1;
		while(fg.go_1){
			Go_Fixed_Distance_Done(3500,3500,800);
			if(fg.Go_Set_Distance_Enable==0){
			fg.go_1=0;
			fg.go_2=1;
			fg.Turn_engle_Enable=1;
			}	
		}
		while(fg.go_2){
			if(fg.chuku_R==1){//右出库				
			Turn_engle_PD_Done(-8000,3000,500,0.2,0.03);
			if(fg.Turn_engle_Enable==0){
			fg.go_2=0;
			fg.go_3=1;
			}	
			}
			else if(fg.chuku_L==1){
			Turn_engle_PD_Done(8000,3000,500,0.2,0.03);
			if(fg.Turn_engle_Enable==0){
				fg.go_2=0;
				fg.go_3=1;
			}	
			}			
		}
		if(fg.go_3==1){
			 fg.go_3=0;
			fg.already_chu=1;
			fg.xunji=1;
		}
	}
}

void Dao_Che_Ru_Ku()//参数不好调时，用多过程
{	//	//入库
	if(fg.already_chu==1){
	if(fg.qishiwei==1&&fg.stop_1==0&&fg.stop_2==0)
	{
		fg.stop_1=1;
		fg.xunji=0;
		fg.Go_Set_Time_Enable=1;
	}	
	while(fg.stop_1){
	Go_Set_Time_Done(-2000,-2000,15);
		if(fg.Go_Set_Time_Enable==0){
		fg.stop_1=0;
		fg.stop_2=1;
			fg.Turn_engle_Enable=1;
		}
	}
	while(fg.stop_2){
			Turn_engle_PD_Done(-2500,3000,-4000,0.2,0.03);
		if(fg.Turn_engle_Enable==0){
		fg.stop_2=0;
		fg.stop_3=1;
			fg.Go_Set_Time_Enable=1;
		}		
	}
	while(fg.stop_3){				
		if(fg.stop_3==1)Go_Set_Time_Done(-4000,8000,15);
		if(fg.Go_Set_Time_Enable==0){
		fg.stop_3=0;
		fg.stop_4=1;
			fg.Go_Set_Time_Enable=1;
		}			
	}
	while(fg.stop_4){
		if(fg.stop_4==1)Go_Set_Time_Done(-2000,2000,2);
		if(fg.Go_Set_Time_Enable==0){
		fg.stop_4=0;
		fg.stop_5=1;			
		}
	}
	while(fg.stop_5){
		
	CarForward(0,0);
	}
}	
}

模块化的写法确实好用，但是大家自己写的时候注意局部变量和静态变量的调用，有什么好的模块化代码也可以分享出来造福大家。