智能车基础车型-电磁车的硬件介绍和操作系统（2）

最新推荐文章于 2023-12-02 15:39:42 发布
最新推荐文章于 2023-12-02 15:39:42 发布
阅读量622 收藏 20
点赞数 10
文章标签： 单片机 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Fan258000/article/details/134678106
版权

                                                     电感寻迹及元素判断

                                

一、元素判断（重难点问题）

1、理论分析

从此图可以看出理论上小车在经过三叉、环岛和十字时，左右两个电感的电感值都会接近磁感线，检测这个特征就可以识别到了，为了防止误判还可以增加一个中间电感或者斜放置电感来辅助判断。电感只有线圈切割磁场时才能产生感应电动势，线圈和磁场平行时是没有感应电动势的，但是在这些特殊元素是，原本没有切割磁场的电感可能突然就切割了磁感线，这里留给大家自己去发挥。

此处提供一个思路，直接看图：

弯道：L-R>0/<0

2、代码实现

元素判断是智能车能够正确运行的重中之重，如果想让智能车全部沿着正确的元素快速运行，那你的元素判断这部分代码一定要正确！准确！实用!快速反应！元素判断主要包括的元素为：直线、环岛、路障、坡道以及其他的操作系统如：倒车、出库、停车等。那么接下来我们将一起探索元素判断的代码的神奇世界........

(1)完整代码实现

/赛道位置偏差判断及元素识别
//                                                                           //
//                                                                           //
//                                                                           //
///
 
#include "math.h"
#include "elec_tracking.h"
#include "elec_sensor.h"
#include "SEEKFREE_WIRELESS.h"
#include "motor.h"
#include "pid.h"
#include "headfile.h"
static uint8 pst_count = 9;
struct position pst = {0,0,0,0,0,0,0,0,0,0};
struct type roadtype = {0,0,0,0,0,0};
int16 bizhang_f = 1;
int16 stamp_f = 1;
int16 block_flag = 0;
int16 block_count = 0;
int16 stop_f = 1;
int16 stop_count = 0;
int16 stop_count_2 = 0;
int16 chuku_flag = 0;
int16 chuku_f = 1;
int16 roundabout_flag_l = 0;
int16 roundabout_flag_r = 0;
 
int16 roundabout_speed = 230; //环岛初始速度
 
uint8 block_sum = 1; //路障或者坡道的总的个数
int16 exit = -1;  //  -1为右边出库，1为左边出库
int16 block_dir = -1;   //-1为从右边过避障，1为从左边过避障  
 
 
uint8 block1 = 0;
uint8 block2 = 0;
uint8 block3 = 0;
uint8 block4 = 0;
uint8 block5 = 0;
uint8 block6 = 0;
uint8 block7 = 0;
 
int16 block_distance1 = 300;
int16 block_distance2 = 160;
int16 block_distance3 = 350;
int16 block_distance4 = 180;
 
int16 huang_distance = 700;
int16 block_mm = 980;
int16 daoche_distance = 130;
uint8 roundabout_off = 0;
 
int16 count = 0;
int16 stop_off = 0;
int block_arrange[7]={0,0,0,0,0,0,0};
 
void block_sort()  //路障坡道排序函数
{
	block_arrange[0] = block1;
	block_arrange[1] = block2;
	block_arrange[2] = block3;
	block_arrange[3] = block4;
	block_arrange[4] = block5;
	block_arrange[5] = block6;
	block_arrange[6] = block7;
}
 
uint8 roundabout_on = 0;  //环岛开关
uint8 block_on = 0;       //路障开关
uint8 chuku_on = 0;    //出库开关 
uint8 stop_on = 0; 
uint8 stop_on_2 = 0; 
 
uint8 stop_flag_on = 0;   //停车开关
uint8 stop_flag_on_2 = 0;
 
bit stop_flag = 0;
bit stop_flag_2 = 0;
bit stop_status = 1;
bit stop_status_1 = 0;
bit stop_status_2 = 0;
//上一次开关状态变量
bit stop_last_status = 1;
bit stop_last_status_1 = 0;
bit stop_last_status_2 = 0;
 
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      计算位置偏差
//  @param      struct position *pst,struct buff *value_buff,struct type *roadtype    
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	
void pst_judgment(struct position *pst,struct buff *value_buff,struct type *roadtype) 
{	
    uint8 i,max;
	max = value_buff->value[0];
	pst->maxnum = 0;
//	if (value_buff->value[0]>=5||value_buff->value[4]>=5)
//	{
//		wangdao_flag = 0;
//	}
//	else
//	{
//			wangdao_flag = 1;
//	}
	if (roadtype->type == 6 || roadtype->type == 5)   //圆环  左环岛或者右环岛
	{
		max = value_buff->value[1];
		value_buff->value[2] = 0.72*value_buff->value[2];  //模拟正常循迹中间电感大小
		if (value_buff->value[1]>=10||value_buff->value[3]>=10)
		{			
			
			for (i=1;i<4;i+=1)
			{
				if (value_buff->value[i] > max)//0=3   4=1
				{
					pst->maxnum = i;
					max = value_buff->value[i];
				}
			}				
			if(((pst->last_maxnum==2)&&(pst->maxnum==3))&&abs(value_buff->value[3]-value_buff->value[2])<=3)  //车身右偏
			{
				if(pst->maxnum==3||(abs(value_buff->value[3]-value_buff->value[2])<=3))
					pst->L12=(value_buff->value[3]+value_buff->value[2])>>1;//40左右  偏离电磁线的位置数据 车身右偏即偏离左电磁线
				if(pst->L12>=60) pst->L12=60;   
				if(pst->L12<=35) pst->L12=35;   
			}
		
		
			if(((pst->last_maxnum==2)&&(pst->maxnum==1))&&abs(value_buff->value[1]-value_buff->value[2])<=3)//车身左偏
			{
				if(pst->maxnum==1||(abs(value_buff->value[1]-value_buff->value[2])<=3))
					pst->R12=(value_buff->value[2]+value_buff->value[1])>>1; //40左右  偏离电磁线的位置数据 车身左偏即偏离右电磁线
				if(pst->R12>=60) pst->R12=60; 
				if(pst->R12<=35) pst->R12=35; 
			}		
	
//=====================pst0计算===========================//
			if(pst->maxnum==2)//中间电感2最大 
			{
 
					pst->E_pst0=(int16)(value_buff->value[3]-value_buff->value[1])>>2;          //pst0:-8~8    
				
				if(pst->E_pst0<=-pst_count)    pst->E_pst0=-pst_count;            
				if(pst->E_pst0>= pst_count)    pst->E_pst0= pst_count;
				
			}
			else if(pst->maxnum==3)//左边电感0最大    ？  
			{				
				pst->E_pst0=(int16)(pst_count+(pst->L12 - value_buff->value[2])>>1);   //pst0:9~18
				if(pst->E_pst0<=pst_count)   pst->E_pst0=pst_count; 
				
			}
			
			else //右边电感4最大
			{				
				pst->E_pst0=(int16)(0-(pst_count+(pst->R12-value_buff->value[2])>>1));
				if(pst->E_pst0 >= -pst_count)  pst->E_pst0 = -pst_count;  
			}
							
	}
		else
		{
			pst->E_pst0=pst->E_last_pst0;
 
      pst->maxnum=pst->last_maxnum;
		}
 
	pst->llast_maxnum = pst->last_maxnum;
	pst->last_maxnum = pst->maxnum;  
	pst->E_llast_pst0=pst->E_last_pst0;
	pst->E_last_pst0=pst->E_pst0;
		//P75 = 1;
	}		
 
	else  //非圆环
	{
		if (value_buff->value[0]>=2||value_buff->value[4]>=2)
		{			
			for (i=0;i<5;i+=2)
			{
				if (value_buff->value[i] > max)
				{
					pst->maxnum = i;
					max = value_buff->value[i];
				}
			}				
			if(((pst->last_maxnum==2)&&(pst->maxnum==0))&&abs(value_buff->value[0]-value_buff->value[2])<=3)  //车身右偏
			{
				if(pst->maxnum==0||(abs(value_buff->value[0]-value_buff->value[2])<=1))
					pst->L12=(value_buff->value[0]+value_buff->value[2])>>1;//40左右
				if(pst->L12>=60) pst->L12=60;   
				if(pst->L12<=40) pst->L12=40;   
			}
		
		
			if(((pst->last_maxnum==2)&&(pst->maxnum==4))&&abs(value_buff->value[4]-value_buff->value[2])<=3)//车身左偏
			{
				if(pst->maxnum==4||(abs(value_buff->value[4]-value_buff->value[2])<=1))
					pst->R12=(value_buff->value[2]+value_buff->value[4])>>1;
				if(pst->R12>=60) pst->R12=60; 
				if(pst->R12<=40) pst->R12=40; 
			}		
	
//=====================pst0计算===========================//
			if(pst->maxnum==2)//中间电感2最大 
			{
 
					pst->E_pst0=(int16)(value_buff->value[0]-value_buff->value[4])>>2;          //pst0:-8~8    
				
				if(pst->E_pst0<=-pst_count)    pst->E_pst0=-pst_count;            
				if(pst->E_pst0>= pst_count)    pst->E_pst0= pst_count;
				
			}
			else if(pst->maxnum==0)//左边电感0最大      
			{				
				pst->E_pst0=(int16)(pst_count+(pst->L12-value_buff->value[2])>>1);   //pst0:9~18
				if(pst->E_pst0<=pst_count)   pst->E_pst0=pst_count; 
				
			}
			
			else //右边电感4最大
			{				
				pst->E_pst0=(int16)(0-(pst_count+(pst->R12-value_buff->value[2])>>1));
				if(pst->E_pst0 >= -pst_count)  pst->E_pst0 = -pst_count;  
			}
							
	}
		else
		{
			pst->E_pst0=pst->E_last_pst0;
 
      pst->maxnum=pst->last_maxnum;
		}
	if(pst->E_pst0>=30)     
	{
		pst->E_pst0=30; 
	}		
    if(pst->E_pst0<=-30)    
	{
		pst->E_pst0=-30;
	}
		if(pst->L12<=40)
	{		
		pst->L12=40; 
	}
	if(pst->R12<=40)
	{		
		pst->R12=40; 
	}
	pst->llast_maxnum = pst->last_maxnum;
	pst->last_maxnum = pst->maxnum;  
	pst->E_llast_pst0=pst->E_last_pst0;
	pst->E_last_pst0=pst->E_pst0;
  }
}
//-------------------------------------------------------------------------------------------------------------------
//  @brief      判断环岛
//  @param      struct buff *value_buff,struct type *roadtype,uint16 distance
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	 
void roundabout_identification(struct buff *value_buff,struct type *roadtype,int16 *distance)
{
	if (roadtype->roundabout_flag == 1 && (value_buff->value[0]>=60 || value_buff->value[4]>=60 || value_buff->value[2]>=70) && roadtype->roundabout_count == 1)    //清环岛标志位
	{
			   
			roadtype->roundabout_flag = 0;
			roadtype->roundabout_count = 0;
			roundabout_off = 1;
			set_speed = flag_speed;  //把速度标志位赋值给设定速度
			P75 = 1;	
	}
 
	if (roadtype->roundabout_flag ==0 && value_buff->value[2]>=80 && block_flag ==0 && roundabout_off == 0)
	{
		set_speed = roundabout_speed; //将环岛的速度赋值给设定速度
		roadtype->roundabout_flag = 1; //判别为环岛
		P75 = 0;
		roadtype->type = 6;
	}
	if(*distance >= huang_distance && roadtype->roundabout_flag == 1)  //入环后
		{
			roadtype->type = 0;
			roadtype->roundabout_count = 1;
			set_speed = 450;   //快速加速恢复直道时的速度  
			
			*distance = 0;
		}
	if(*distance >=1000 && roundabout_off == 1)
	{
		roundabout_off = 0;
		*distance = 0;
	}
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      判断路障或坡道
//  @param      int16 distance
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	
void block_judge(int16 distance) 
{
	if((distance < block_mm)&&(distance>300)&&(abs(pst.E_pst0)<9) && chuku_on == 0)
//		block_count ++;
//	else if(distance>800||distance<800)
//		block_count=0;
//	if(block_count>0)  //此处的判断条件可用于 延时处理
	{
		block_flag = 1;
//		//block_count = 0;
//		//set_speed = 250;
		P75 = 0;
	}
}	
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      停车检测
//  @param      stop_judge()
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	
void stop_judge(int16 *distance)
{
		if(stop_count == 1 &&	stop_flag == 1)
		{
			roadtype.type = 0;
			*distance = 0;
			set_speed = -500;
			stop_flag_on = 1;
			dir.p = 100;
			pst.E_pst0 = 30*exit;
			brushless_buty = 3200;
			P75 = 1;
			stop_count = 0;
		}
	  if( P76 == 1 && chuku_on == 0 && (abs(pst.E_pst0)<8) && stop_flag == 0 && stop_off == 0)    
	 {
		 P75 = 0;
		stop_flag = 1;
		 roadtype.type = 5;
	 }
 	 if (stop_flag == 1 && stop_count < 1)
		{
			if(*distance > 50 && P76 == 1)
			{
				P75 = 1;
				set_speed = flag_speed;
				stop_flag = 0;
				roadtype.type = 0;
				*distance = 0;
				stop_off = 1; 
			}
			if(*distance > 50&& P76 == 0)
			{
				roadtype.type = 0;
				stop_flag = 0;
				*distance = 0;
			}
		}
		if(stop_off == 1  && P76 == 0)
		{
			stop_count = 1;
			stop_off = 0;
		}	
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      倒车函数
//  @param      NULL
//  @return     void
//  Sample usage:  exit表示打角的方向，-1为右边，1为左边           
//-------------------------------------------------------------------------------------------------------------------
void back_off(int16 *distance)//入库函数
{		
			if(stop_f ==1 && (l_cur_speed < 70 || r_cur_speed < 70))
			{
				//
				set_speed = 60;
				pst.E_pst0 = 30*exit;//
				if(*distance>300)
				{
//				brushless_on = 0;
//				motor_on = 0;
//				set_speed = 0;
				*distance = 0;
				//stop_flag_on = 0;
				stop_f = 2;
				}			
			}
			if(stop_f == 2)
			{
				set_speed = 50;
				pst.E_pst0 = 0;
				if(*distance > 30)
				{
					brushless_on = 0;
					motor_on = 0;
					set_speed = 0;
					*distance = 0;
					stop_flag_on = 0;
					//stop_count = 1;
					stop_f = 0;
				}			
			}
			if(stop_f ==3)
			{			
				set_speed = -100;
				pst.E_pst0 = 20*exit;
				//set_speed = 0;
				if(*distance<-230)
				{
					*distance = 0;
					stop_f = 4;
					//P75 = 1;					
				}
			}
			if(stop_f ==4)
			{						
				dir_ctrl(&dir,0);
			}
}
 
 
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      路障函数
//  @param      NULL 
//  @return     void
//  Sample usage:   block_dir表示避障的方向，-1为右边，1为左边       
//-------------------------------------------------------------------------------------------------------------------	
void barrier(int16 *distance) //路障
{
	if(bizhang_f == 1)
	{	
		set_speed = 300;
		pst.E_pst0 =block_dir*25;
		if(*distance > block_distance1)
		{
			bizhang_f = 2;
			*distance = 0;
		}
	}
	if(bizhang_f == 2)
	{
		pst.E_pst0 =-block_dir*12;
		if(*distance > block_distance2)
		{
			bizhang_f = 3;
			*distance = 0;
		}
	}
	if(bizhang_f == 3)
	{
		pst.E_pst0=-block_dir*25;
		if(*distance > block_distance3)
		{
			bizhang_f = 4;
			*distance = 0;
		}
	}	
	if(bizhang_f == 4)
	{
		pst.E_pst0=block_dir*15;
		if(*distance > block_distance4)
		{
			//set_speed = 0;
			block_flag = 0;
			count ++;
			//block_on = 0;
			set_speed = flag_speed;
			P75 = 1;
			*distance = 0;
			bizhang_f = 1;
		}	
	}
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      坡道函数
//  @param      NULL
//  @return     void
//  Sample usage:  上坡做减速处理         
//-------------------------------------------------------------------------------------------------------------------	
void ramp_sign(int16 *distance) //坡道函数
{
	if(stamp_f == 1)
	{	
		set_speed = 280;
		if(*distance>300)
		{
			stamp_f = 2;
			*distance = 0;
		}
	}
//	if(stamp_f == 2)
//	{
//		set_speed = 300;
//		if(*distance>700)
//		{
//			stamp_f = 3;
//			*distance = 0;
//		}
//	}	
	if(stamp_f == 2)
	{
		set_speed = 320;
		if(*distance>1400 )
		{
			set_speed = flag_speed;
			count ++;
			P75 = 1;
			*distance = 0;
			block_flag = 0;
			stamp_f = 1;
		}	
	}
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      将坡道和路障分开
//  @param      NULL
//  @return     void
//  Sample usage:  0执行坡道函数，1执行避障函数         
//-------------------------------------------------------------------------------------------------------------------	
void block()  //将坡道和路障分开   
{
		if(block_arrange[count]==0)
		{
			ramp_sign(&distance);
		}
		else
		{
			barrier(&distance);
		}	
 
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      出库函数
//  @param      NULL
//  @return     void
//  Sample usage:  打一定的角度出库         
//-------------------------------------------------------------------------------------------------------------------
void exit_off(int16 *distance) // 出库函数
{
	if(chuku_f == 1 && chuku_on == 1)
	{	
		pst.E_pst0=15*exit;
		set_speed = 100;
		if(*distance>30)
		{
			chuku_f = 2;
			*distance = 0;
		}
	}
	if(chuku_f == 2)
	{
		pst.E_pst0=20*exit;
		set_speed = 200;
		if(*distance>175)
		{
			
			*distance = 0;
			chuku_f = 3;
		}
	}	
	if(chuku_f == 3)
	{
		set_speed = 300;
		if(*distance>100)
		{
			
			*distance = 0;
			chuku_f = 0;
			set_speed = flag_speed ;
			chuku_on = 0;
		}
	}	
}
 
void stop_judge_2()//霍尔检测停车模块  磁铁
{
	//读取上一次状态
	  stop_last_status = stop_status;
	
   //读取当前按键状态
    stop_status = STOP_PIN;
	  if(!stop_status && stop_last_status && stop_count_2 == 0 && chuku_on == 0)    
	 {
		 stop_flag_on_2 = 1;
		 set_speed = -100;
		 stop_flag_2 = 1;		 	 
	 }
 	 if (stop_flag_2 == 1)
		{		
			stop_count_2 = 1;			
			stop_flag_2 = 0;
		}	
		
}
 
//-------------------------------------------------------------------------------------------------------------------
//  @brief      倒车函数
//  @param      NULL
//  @return     void
//  Sample usage:  exit表示打角的方向，-1为右边，1为左边           
//-------------------------------------------------------------------------------------------------------------------
void back_off_2(int16 *distance)//倒车函数
{		
			if(stop_f == 1 && (l_cur_speed < 60 || r_cur_speed < 60))
			{
				//pst.E_pst0 = 0;
				set_speed = 40;
				pst.E_pst0 = 0;
				if(*distance>50)
				{
					*distance = 0;
					//stop_count = 1;
					stop_f = 2;
				}			
			}
			if(stop_f == 2)
			{
				//pst.E_pst0 = 0;
				set_speed = -50;
				pst.E_pst0 = 0;
				if(*distance < -200)
				{
					*distance = 0;
					stop_f = 3;
				}			
			}
			if(stop_f ==3)
			{			
				set_speed = -200;
				pst.E_pst0 = 20*exit;
				if(*distance<-180)
				{
					*distance = 0;
					stop_f = 4;					
				}
			}
			if(stop_f ==4)
			{			
				set_speed = -250;
				pst.E_pst0 = 10*exit;
				if(*distance<-220)
				{
					brushless_on = 0;
					motor_on = 0;
					stop_flag_on = 0;
					set_speed = 0;
					*distance = 0;
					stop_f = 0;
					//P75 = 1;					
				}
			}
}

（2）计算位置偏差

//-------------------------------------------------------------------------------------------------------------------
//  @brief      计算位置偏差
//  @param      struct position *pst,struct buff *value_buff,struct type *roadtype    
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	
void pst_judgment(struct position *pst,struct buff *value_buff,struct type *roadtype) 
{	
    uint8 i,max;
	max = value_buff->value[0];
	pst->maxnum = 0;
//	if (value_buff->value[0]>=5||value_buff->value[4]>=5)
//	{
//		wangdao_flag = 0;
//	}
//	else
//	{
//			wangdao_flag = 1;
//	}
	if (roadtype->type == 6 || roadtype->type == 5)   //圆环  左环岛或者右环岛
	{
		max = value_buff->value[1];
		value_buff->value[2] = 0.72*value_buff->value[2];  //模拟正常循迹中间电感大小
		if (value_buff->value[1]>=10||value_buff->value[3]>=10)
		{			
			
			for (i=1;i<4;i+=1)
			{
				if (value_buff->value[i] > max)//0=3   4=1
				{
					pst->maxnum = i;
					max = value_buff->value[i];
				}
			}				
			if(((pst->last_maxnum==2)&&(pst->maxnum==3))&&abs(value_buff->value[3]-value_buff->value[2])<=3)  //车身右偏
			{
				if(pst->maxnum==3||(abs(value_buff->value[3]-value_buff->value[2])<=3))
					pst->L12=(value_buff->value[3]+value_buff->value[2])>>1;//40左右  偏离电磁线的位置数据 车身右偏即偏离左电磁线
				if(pst->L12>=60) pst->L12=60;   
				if(pst->L12<=35) pst->L12=35;   
			}
		
		
			if(((pst->last_maxnum==2)&&(pst->maxnum==1))&&abs(value_buff->value[1]-value_buff->value[2])<=3)//车身左偏
			{
				if(pst->maxnum==1||(abs(value_buff->value[1]-value_buff->value[2])<=3))
					pst->R12=(value_buff->value[2]+value_buff->value[1])>>1; //40左右  偏离电磁线的位置数据 车身左偏即偏离右电磁线
				if(pst->R12>=60) pst->R12=60; 
				if(pst->R12<=35) pst->R12=35; 
			}		
	
//=====================pst0计算===========================//
			if(pst->maxnum==2)//中间电感2最大 
			{
 
					pst->E_pst0=(int16)(value_buff->value[3]-value_buff->value[1])>>2;          //pst0:-8~8    
				
				if(pst->E_pst0<=-pst_count)    pst->E_pst0=-pst_count;            
				if(pst->E_pst0>= pst_count)    pst->E_pst0= pst_count;
				
			}
			else if(pst->maxnum==3)//左边电感0最大    ？  
			{				
				pst->E_pst0=(int16)(pst_count+(pst->L12 - value_buff->value[2])>>1);   //pst0:9~18
				if(pst->E_pst0<=pst_count)   pst->E_pst0=pst_count; 
				
			}
			
			else //右边电感4最大
			{				
				pst->E_pst0=(int16)(0-(pst_count+(pst->R12-value_buff->value[2])>>1));
				if(pst->E_pst0 >= -pst_count)  pst->E_pst0 = -pst_count;  
			}
							
	}
		else
		{
			pst->E_pst0=pst->E_last_pst0;
 
      pst->maxnum=pst->last_maxnum;
		}
 
	pst->llast_maxnum = pst->last_maxnum;
	pst->last_maxnum = pst->maxnum;  
	pst->E_llast_pst0=pst->E_last_pst0;
	pst->E_last_pst0=pst->E_pst0;
		//P75 = 1;
	}		
 
	else  //非圆环
	{
		if (value_buff->value[0]>=2||value_buff->value[4]>=2)
		{			
			for (i=0;i<5;i+=2)
			{
				if (value_buff->value[i] > max)
				{
					pst->maxnum = i;
					max = value_buff->value[i];
				}
			}				
			if(((pst->last_maxnum==2)&&(pst->maxnum==0))&&abs(value_buff->value[0]-value_buff->value[2])<=3)  //车身右偏
			{
				if(pst->maxnum==0||(abs(value_buff->value[0]-value_buff->value[2])<=1))
					pst->L12=(value_buff->value[0]+value_buff->value[2])>>1;//40左右
				if(pst->L12>=60) pst->L12=60;   
				if(pst->L12<=40) pst->L12=40;   
			}
		
		
			if(((pst->last_maxnum==2)&&(pst->maxnum==4))&&abs(value_buff->value[4]-value_buff->value[2])<=3)//车身左偏
			{
				if(pst->maxnum==4||(abs(value_buff->value[4]-value_buff->value[2])<=1))
					pst->R12=(value_buff->value[2]+value_buff->value[4])>>1;
				if(pst->R12>=60) pst->R12=60; 
				if(pst->R12<=40) pst->R12=40; 
			}		
	
//=====================pst0计算===========================//
			if(pst->maxnum==2)//中间电感2最大 
			{
 
					pst->E_pst0=(int16)(value_buff->value[0]-value_buff->value[4])>>2;          //pst0:-8~8    
				
				if(pst->E_pst0<=-pst_count)    pst->E_pst0=-pst_count;            
				if(pst->E_pst0>= pst_count)    pst->E_pst0= pst_count;
				
			}
			else if(pst->maxnum==0)//左边电感0最大      
			{				
				pst->E_pst0=(int16)(pst_count+(pst->L12-value_buff->value[2])>>1);   //pst0:9~18
				if(pst->E_pst0<=pst_count)   pst->E_pst0=pst_count; 
				
			}
			
			else //右边电感4最大
			{				
				pst->E_pst0=(int16)(0-(pst_count+(pst->R12-value_buff->value[2])>>1));
				if(pst->E_pst0 >= -pst_count)  pst->E_pst0 = -pst_count;  
			}
							
	}
		else
		{
			pst->E_pst0=pst->E_last_pst0;
 
      pst->maxnum=pst->last_maxnum;
		}
	if(pst->E_pst0>=30)     
	{
		pst->E_pst0=30; 
	}		
    if(pst->E_pst0<=-30)    
	{
		pst->E_pst0=-30;
	}
		if(pst->L12<=40)
	{		
		pst->L12=40; 
	}
	if(pst->R12<=40)
	{		
		pst->R12=40; 
	}
	pst->llast_maxnum = pst->last_maxnum;
	pst->last_maxnum = pst->maxnum;  
	pst->E_llast_pst0=pst->E_last_pst0;
	pst->E_last_pst0=pst->E_pst0;
  }
}

 （3）判断环岛

//-------------------------------------------------------------------------------------------------------------------
//  @brief      判断环岛
//  @param      struct buff *value_buff,struct type *roadtype,uint16 distance
//  @return     void
//  Sample usage:           
//-------------------------------------------------------------------------------------------------------------------	 
void roundabout_identification(struct buff *value_buff,struct type *roadtype,int16 *distance)
{
	if (roadtype->roundabout_flag == 1 && (value_buff->value[0]>=60 || value_buff->value[4]>=60 || value_buff->value[2]>=70) && roadtype->roundabout_count == 1)    //清环岛标志位
	{
			   
			roadtype->roundabout_flag = 0;
			roadtype->roundabout_count = 0;
			roundabout_off = 1;
			set_speed = flag_speed;  //把速度标志位赋值给设定速度
			P75 = 1;	
	}
 
	if (roadtype->roundabout_flag ==0 && value_buff->value[2]>=80 && block_flag ==0 && roundabout_off == 0)
	{
		set_speed = roundabout_speed; //将环岛的速度赋值给设定速度
		roadtype->roundabout_flag = 1; //判别为环岛
		P75 = 0;
		roadtype->type = 6;
	}
	if(*distance >= huang_distance && roadtype->roundabout_flag == 1)  //入环后
		{
			roadtype->type = 0;
			roadtype->roundabout_count = 1;
			set_speed = 450;   //快速加速恢复直道时的速度  
			
			*distance = 0;
		}
	if(*distance >=1000 && roundabout_off == 1)
	{
		roundabout_off = 0;
		*distance = 0;
	}
}

（4）判断路障

//-------------------------------------------------------------------------------------------------------------------
//  @brief      路障函数
//  @param      NULL 
//  @return     void
//  Sample usage:   block_dir表示避障的方向，-1为右边，1为左边       
//-------------------------------------------------------------------------------------------------------------------	
void barrier(int16 *distance) //路障
{
	if(bizhang_f == 1)
	{	
		set_speed = 300;
		pst.E_pst0 =block_dir*25;
		if(*distance > block_distance1)
		{
			bizhang_f = 2;
			*distance = 0;
		}
	}
	if(bizhang_f == 2)
	{
		pst.E_pst0 =-block_dir*12;
		if(*distance > block_distance2)
		{
			bizhang_f = 3;
			*distance = 0;
		}
	}
	if(bizhang_f == 3)
	{
		pst.E_pst0=-block_dir*25;
		if(*distance > block_distance3)
		{
			bizhang_f = 4;
			*distance = 0;
		}
	}	
	if(bizhang_f == 4)
	{
		pst.E_pst0=block_dir*15;
		if(*distance > block_distance4)
		{
			//set_speed = 0;
			block_flag = 0;
			count ++;
			//block_on = 0;
			set_speed = flag_speed;
			P75 = 1;
			*distance = 0;
			bizhang_f = 1;
		}	
	}
}

（4） 判断坡道

//-------------------------------------------------------------------------------------------------------------------
//  @brief      坡道函数
//  @param      NULL
//  @return     void
//  Sample usage:  上坡做减速处理         
//-------------------------------------------------------------------------------------------------------------------	
void ramp_sign(int16 *distance) //坡道函数
{
	if(stamp_f == 1)
	{	
		set_speed = 280;
		if(*distance>300)
		{
			stamp_f = 2;
			*distance = 0;
		}
	}
//	if(stamp_f == 2)
//	{
//		set_speed = 300;
//		if(*distance>700)
//		{
//			stamp_f = 3;
//			*distance = 0;
//		}
//	}	
	if(stamp_f == 2)
	{
		set_speed = 320;
		if(*distance>1400 )
		{
			set_speed = flag_speed;
			count ++;
			P75 = 1;
			*distance = 0;
			block_flag = 0;
			stamp_f = 1;
		}	
	}
}

（5）倒车

//-------------------------------------------------------------------------------------------------------------------
//  @brief      倒车函数
//  @param      NULL
//  @return     void
//  Sample usage:  exit表示打角的方向，-1为右边，1为左边           
//-------------------------------------------------------------------------------------------------------------------
void back_off(int16 *distance)//入库函数
{		
			if(stop_f ==1 && (l_cur_speed < 70 || r_cur_speed < 70))
			{
				//
				set_speed = 60;
				pst.E_pst0 = 30*exit;//
				if(*distance>300)
				{
//				brushless_on = 0;
//				motor_on = 0;
//				set_speed = 0;
				*distance = 0;
				//stop_flag_on = 0;
				stop_f = 2;
				}			
			}
			if(stop_f == 2)
			{
				set_speed = 50;
				pst.E_pst0 = 0;
				if(*distance > 30)
				{
					brushless_on = 0;
					motor_on = 0;
					set_speed = 0;
					*distance = 0;
					stop_flag_on = 0;
					//stop_count = 1;
					stop_f = 0;
				}			
			}
			if(stop_f ==3)
			{			
				set_speed = -100;
				pst.E_pst0 = 20*exit;
				//set_speed = 0;
				if(*distance<-230)
				{
					*distance = 0;
					stop_f = 4;
					//P75 = 1;					
				}
			}
			if(stop_f ==4)
			{						
				dir_ctrl(&dir,0);
			}
}

（6）出库

//-------------------------------------------------------------------------------------------------------------------
//  @brief      出库函数
//  @param      NULL
//  @return     void
//  Sample usage:  打一定的角度出库         
//-------------------------------------------------------------------------------------------------------------------
void exit_off(int16 *distance) // 出库函数
{
	if(chuku_f == 1 && chuku_on == 1)
	{	
		pst.E_pst0=15*exit;
		set_speed = 100;
		if(*distance>30)
		{
			chuku_f = 2;
			*distance = 0;
		}
	}
	if(chuku_f == 2)
	{
		pst.E_pst0=20*exit;
		set_speed = 200;
		if(*distance>175)
		{
			
			*distance = 0;
			chuku_f = 3;
		}
	}	
	if(chuku_f == 3)
	{
		set_speed = 300;
		if(*distance>100)
		{
			
			*distance = 0;
			chuku_f = 0;
			set_speed = flag_speed ;
			chuku_on = 0;
		}
	}	
}

（7） 停车

oid stop_judge_2()//霍尔检测停车模块  磁铁
{
	//读取上一次状态
	  stop_last_status = stop_status;
	
   //读取当前按键状态
    stop_status = STOP_PIN;
	  if(!stop_status && stop_last_status && stop_count_2 == 0 && chuku_on == 0)    
	 {
		 stop_flag_on_2 = 1;
		 set_speed = -100;
		 stop_flag_2 = 1;		 	 
	 }
 	 if (stop_flag_2 == 1)
		{		
			stop_count_2 = 1;			
			stop_flag_2 = 0;
		}	
		
}

 以上为元素判断的相关代码，内容属实比较多，这点也确实没办法。因为智能车需要走的轨迹和现实生活中的汽车的轨迹是一样的，都是存在很多元素的。还请各位感兴趣的同学能够耐下心来，慢慢品味。

如果您有任何疑问，请在评论区提出问题，看到即会回复，谢谢！！

单车漫游中国
关注
  • 10
    点赞
  • 踩
  • 20
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
基于MM32的电磁循迹小车制作
2303_78449478的博客
07-28 1726
电磁循迹是通过小车的前端电感对赛道上的电磁导线产生电磁感应，在不同的位置，电感感应到的电流值是不相同的，并通过导线传输到运放（运算放大器）模块中，在通过导线传输到MM32单片机的ADC接口中，反馈到主控芯片内部。嘿嘿，其他的就是镊子，剥线钳，斜口钳，锡丝，助焊剂，吸锡枪，万用表，示波器等等，想必各位看官肯定是工具齐全，啥时候我这可怜的娃也能过上这好日子啊！小编所使用的TFT的功能是主要用于显示各个电感所采集的电感值，已经我通过算法计算出的输出值，方便进行判断是否正确，以及我的小车运行状况是否正确等等。
飞思卡尔智能车电磁组例程
01-01
7. **实时操作系统（RTOS）**：在复杂系统中，RTOS如FreeRTOS可能会被用于管理任务调度、中断处理和内存管理，保证多任务的并行执行和系统响应性。 8. **通信协议**：例如SPI、I2C或UART，用于微控制器与其他组件...
智能车基础车型-电磁车的硬件介绍和操作系统（1）
Fan258000的博客
11-28 1324
在这一次的理解分享中，我们首先总体上大致介绍一下电磁车的运行原理，然后再从代码入手，具体分析代码的作用。即原理与代码相结合！
恩智浦智能车主控板分享
qq_53016054的博客
10-30 2524
第十七届智能车大赛摄像头组
智能车电磁组元素处理（圆环、三岔、路障）
m0_69153234的博客
09-02 5762
（因此此处电感值相比其它的地方也会有明显区别，找到这个特殊点即可识别岔路），当小车的电感提前识别到岔路以后，再像圆环处理那样给一个固定差值打脚一段时间，进入岔路后切换回正常循迹，同样的速度不快时，出岔路也不用处理，与出环岛不同的是出岔路以后不仅要把各个岔路标志位清零，还要把一个次数标志位置1（即第一次识别到岔路时，该次数标志位为0向左打脚入左岔路，第二次识别到岔路时，该次数标志位为1向右打脚入右岔路），这样即可。，我还没说到它的用处，不会用的小伙伴可以先定义一个time，，下面给出我的识别岔路的参考代码。
STC32G 三电感电磁循迹小车
xht2403267701的博客
01-05 1万+
准备18届的负压电磁，趁现在考试延期赶紧把车子给调了。现在速度就只能提到1.5m，再往上调就有点打滑了，只能等后面逐飞把负压电机的做出来了之后看能不能让车子抓地更好，再往上调调。硬件的主板目前使用逐飞的STC母板，电感排布为左中右。
行业文档-设计装置-汽车多媒体按键智能转换器.zip
09-11
在现代汽车领域，车载多媒体系统已经成为不可或缺的一部分，而汽车多媒体按键智能转换器则是连接传统汽车硬件与现代化多媒体设备的关键技术。本文将深入探讨这一装置的设计原理、功能特点以及其在汽车行业中的应用。...
AMT系统原理和操作方法介绍学习教案.pptx
09-29
### AMT系统原理和操作方法介绍 #### 一、AMT系统概述 AMT（Automated Manual Transmission），即自动手动变速器，是一种基于传统手动变速箱技术，并通过电子控制系统实现自动换挡功能的新型变速器。它结合了手动...
基于语音识别的汽车空调控制系统
02-03
- **硬件结构**：主要包括语音输入模块（包含麦克风和LD3320）、微控制器STM32、空调执行机构（如风机、电磁阀等）以及人机交互界面（如LED显示屏幕）等。 - **软件设计**：系统软件部分主要包括语音识别算法、空调...
商用车整车控制器技术方案.pdf
05-07
**软件方案**：VCU软件包含实时操作系统（RTOS）、控制算法、故障诊断和数据通信协议等，确保精确控制车辆各项功能并能快速响应各种工况。 **功能安全方案**：按照ISO 26262功能安全标准，VCU需进行风险评估、故障...
小车电感循迹模块原理图
10-19
基于电感传感器的自动循迹小车系统----电感传感器部分
基于STC12C5A60S2的电感循迹小车程序
06-21
基于STC12C5A60S2的电感寻迹小车程序，曾在校级比赛中使用。
智能车PID循迹与电磁循迹的切换
最新发布
qq_65650639的博客
12-02 248
最近在参加18届智能车竞赛时调试遇到如何准确识别断路以及在断路和基于PID算法的摄像头循迹之间的切换之间遇到了些许问题，总是会出现识别错误，误识别的问题，这里记载以下我的处理，以便与我之后的问题参考。
智能车浅谈 电磁组——环岛处理
qq_41954556的博客
09-03 9626
第十七届全国大学生智能车电磁四轮部分经验分享
【电磁循迹】从0到1
YaoJinTao2003的博客
01-03 8130
智能车电磁循迹从0到1
电感传感器循迹——智能小车
Codes_Master的博客
03-15 2384
/* * @file main.c * @brief 主函数 */ #include "common.h" #include "include.h" #include "time.h" #define N 1 //滑动滤波窗口 #define n 0 //异常值个数剔除 //130--1, 90--2 142-3 int servor_middle=120; //舵机摆臂回正的脉宽，需要根据实际情况修改，现在是(155/1000)*10ms=1.55ms 1...
电磁传感器 电磁循迹 自制电感参数整定
qq_44645174的博客
11-13 4074
融合电感计算式，和极限小距离下电感感生电动势极值，根据极限小距离下自制电感的圈数，半径，长度计算出极限电感值。
【智能车学习】电磁循迹中的基本控制算法
热门推荐
Carry_qing的博客
10-04 5万+
今天心血来潮，突然想总结一下电磁车的一些基本算法，也希望可以给新手同学们一点帮助和参考。本篇主要讲述智能车电磁循迹中涉及到的基本算法，包括滤波算法，归一化算法，PID控制等
AI电磁组训练前期工作--传统电磁方案（循迹小车）的电感小修小改
xufengxing1的博客
08-04 4808
跟你说明这个的原因是因为在电感1压到电磁线的时候，我们收集到的数据存在不少变化，这个变化集中于电感2.主要看excel表里。 （电感一在电磁线正上方的时候，电感2的值会从我们判断转弯的电感2的值（20左右）陡变到-60到-100，大概是因为上述的物理缺陷） 我们晚上商量了一个晚上，又对电感做了一些位置上的测试，发现了上述恶心的现象。我暂且定义为物理缺陷。 为了让你了解到我们车的整体状况。 我大致描述一下小车进入弯道的数据情况： 首先肯定是...
飞思卡尔智能车竞赛：电磁组直立行车设计方案
"该文档是关于第七届全国大学生‘飞思卡尔’杯智能汽车竞赛电磁组直立智能车的设计方案，涵盖了直立控制、速度控制、方向控制等关键技术和硬件、软件设计。" 在这个直立智能车的设计方案中，首先提到了比赛的要求，...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

单车漫游中国

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值