#include "zf_common_headfile.h"
#include "CAMERA.h"








double num_qipao;
int cross_flag_dir;//默认左环岛，也可以是右环岛，或者先左后右，看后期需要           1 左  2右
int shizi_dir_queding[10]={2,2,1};//默认左，也可以是右环岛，或者先左后右，看后期需要           1 左  2右  
int shizi_dir_queding_flag;





///////////////////////////////////////////////////////////////////////////////////////////////散落照片
int sanluo_picing;
int num;
int num_pic;
double coord_aim[2];
double encoder_pic_X;
double encoder_pic_y;
double encoder_pic_angle;
double encoder_pic_angle_shizi;
int sevor_right_flag=2;
int sevor_left_flag=2;
int stop_flag;
double coord_aim_pic_x;
double coord_aim_pic_y;
double coord_aim_pic_x_shizi;
double coord_aim_pic_y_shizi;	
double coord_aim_pic_x_shizi_zebra;
double coord_aim_pic_y_shizi_zebra;	
extern int state_4;


int shuzi[3]={0};   //终点前数字排序flag
int camera_flag;  //camera_flag = 1 时候开启左边 camera_flag = 2时候开启右边    优先级左边大于右边 
int shizi_zimu[5];

int	shizi_zimu_num=-1;//应该是-1，调试改成5
//int	shizi_zimu_num=5;//应该是-1，调试改成5
int roate_shun_ni_flag=0;//车头朝向标志位                  0为正     1为反
int left_shizi_flag;//判断左十字进程
int left_shizi_flag_flag;////判断左十字标志位
int right_shizi_flag=0;
int shizi_flag_flag;



////////////////////////////////////////////////////////////////////////////////////////////////////////

//int pic_cunchu[30]={4,6,6,4,12,11,4,8,4,8};
//int shizi_zimu[5]={4,5,6,7,8};//4 5 7 8 6
//int huandao_zimu[5]={4,5,6,7,8};

int pic_cunchu[30];
int shizi_zimu[5]={4,18,13,16,15};//4 5 7 8 6                //A 4
int huandao_zimu[5]={14,8,9,5,7};            //B 5
int shuzi_pic[3]={1,2,3};                       //C 6
int shi_zi_pic_num=0;                           //D 7
int huandao_pic_num=0;                          //E 8
int pic_cunchu_num=0;     //照片存储数量flag     //F 9
                                                //G 10 
int chakanshuzuneirong_flag=0;                  //H 11
												//I 12
												//J 13
int eiq_mode_left;                              //K 14
int eiq_mode_right;                             //L 15
int eiq_mode_left_flag;                         //M 16
int eiq_mode_right_flag;                        //N 17
int eiq_mode_flag;                              //O 18
int eiq_chuli;                                  
///////////////////////////////////////////////////////////////////////////////////////////////////////
int cross;				//0 无十字   1左十字  2右十字
double roate_determine_angle;//旋转初始角度
int roate_determine_angle_flag;//旋转初始角度标志位
int angle_shizi_left;
int angle_shizi_right;
int xyz;
int left_huandao_flag;
int shizi_wanqian_flag;
/////////////////////////////////////////
int right_huandao_flag;
int right_huandao_flag_flag;
/////////////////////////////////////////

int camera_determine_flag;
int count_flag = 0;
///////////////////////////////////////camera
int Miss_Left_lines_flag;
int Miss_Right_lines_flag;
int ImageStatus_num=10000;
////////////////////////////////////////斑马线放照片
int Zebra_flag;
int Zebra_flag_dir;                                 //                 1      左          2         右
//int shuzi_pic[3]={1,2,3};                                   //                 1	武器放置区            2	物资放置区          3	交通工具放置区

int shuzi_pic_num;; 
int Zebra_flag_flag;
int Zebra_determine_flag;
int Zebra_position;
int shuzi_pic_num_flag;
int xyz_flag;

int eiq_one_flag = 0;


bool isProcessed = false; // 标志位，初始值为false
bool isInitialPositionRecorded = false; // 标志位，初始值为false			
bool isInitialPositionRecorded_pi = false; // 标志位，初始值为false

int num_kong=10000;//判断是否左右为空的十字或者是空环岛
int num_kong_flag;
///*************************************/
///**************获取位置偏差*************/
///*************************************/
//////////////////////////////////////////////////////////////////////////舵机代码
int initial_pwm[4] = {4900, 7000, 3500, 6800};

int middle_pwm1[4] = {4900,8000,4500,8100};//7700 - 5000
int middle_pwm2[4] = {1500,8000,4500,8100};//7700 - 5000
int middle_pwm3[4] = {1500,5500,7500,8100};//7700 - 5000
int target_pwm[4] =  {1500, 3500, 7500, 8100};

int middle_pwm1_right[4] = {4900,8000,4500,8100};//7000 - 5000
int middle_pwm2_right[4] = {8000,8000,4500,8100};//7700 - 5000
int middle_pwm3_right[4] = {8000,5500,7500,8100};//7700 - 5000
int target_pwm_right[4] =  {8000, 3500, 7500, 8100};

int current_pwm[4] = {4900, 7000, 3500, 6800};//改变    int current_pwm[4] = 0, initial_pwm[1], initial_pwm[2], initial_pwm[3]};
int step_size = 50;
int move_delay = 4;
int servos[4] = {Sevor_1, Sevor_2, Sevor_3, Sevor_4};
int flag_1 = 0;
int state_flag;
int state_flag_flag;


int camera_determine_left=0;
int camera_determine_right=0;



int left_roate_determine_flag;
int angle_shizi;
int hzw_flag = 0;
////////////////////////////////////////////////////////////////环岛
int encoder_pic_angle_huandao;
int left_huandao_flag_flag;


int huandao_zimu_num=-1;
int raoquan_determine_flag;
int angle_huandao;
int angle_huandao_flag;
int coord_now_flag;//判断是否需要减去360
int coord_now_flag_right;//判断是否需要加去360
int huandao_wangqian;
////////////////////////////////////////////////////////////////
extern int state_right;
extern int x_pic_right,y_pic_right;
extern int open_close_right;
extern int eiq_right;
/////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////

//ImageFlag.Zebra_Flag斑马线
//ImageFlag.image_element_rings /*0 :无圆环          1 :左圆环       2 :右圆环*/
//cross_flag 十字 /*0: 无   1: 有*/
//uart_write_byte(UART_1, 1);	矩形识别
//uart_write_byte(UART_1, 2);	无
//uart_write_byte(UART_1, 3);	再次矩形识别
//uart_write_byte(UART_1, 4);	深度学习
int angle_banmaxian;

int test_test;

double coord_aim_pic_x_shizi_zebra_1;
double coord_aim_pic_y_shizi_zebra_1;
double coord_aim_pic_x_shizi_zebra_2;
double coord_aim_pic_y_shizi_zebra_2;
double coord_aim_pic_x_shizi_zebra_3;
double coord_aim_pic_y_shizi_zebra_3;
int error_jisuan;
int num_pic_zhongduan;
int sda;
int ssss;
int dasdsadadasdadasda;
void move_coord()
{	
//////////////////////////屏显///////////////////////////////////
        ips200_show_int(90, 0, num_pic , 5);
		ips200_show_int(90, 20, y_pic , 3);
		ips200_show_int(90, 40, x_pic , 5);
		ips200_show_int(90, 60, state_left , 5);
		ips200_show_int(90, 80, open_close_left , 5);
		ips200_show_int(90, 100, eiq_left , 5);
		ips200_show_int(120, 20, cross_flag , 5);
	
		ips200_show_int(160, 20, y_pic_right , 5);
		ips200_show_int(160, 40, x_pic_right , 5);
		ips200_show_int(160, 60, state_right , 5);
		ips200_show_int(160, 80, open_close_right , 5);
		ips200_show_int(160,100, eiq_right , 5);
		
		
		ips200_show_float(25, 120, encoder_pic_angle , 4,2);
        ips200_show_float(25, 140, coord_now[2] , 4,2);  
		ips200_show_int(90, 180, pic_cunchu_num , 5);
		ips200_show_int(90, 200, shizi_zimu_num , 5);
		

 		ips200_show_int(0,80,camera_determine_left,1);
 		ips200_show_int(0,100,camera_determine_right,1);

				
//		pic_cunchu[0]=4;
//		pic_cunchu[1]=6;
//		pic_cunchu[2]=6;
//		pic_cunchu[3]=4;
//		pic_cunchu[4]=8;
//		pic_cunchu[5]=5;
//		pic_cunchu[6]=4;
//		pic_cunchu[7]=8;
//		pic_cunchu[8]=7;
//		pic_cunchu[9]=5;
				

		
		//环岛调试屏幕显示
		//ips200_show_int(0,0,roate_shun_ni_flag,1);
		
		
		ips200_show_int(0,0,ImageFlag.Zebra_Flag,3);
//		ips200_show_int(0,20,angle_huandao,3);
//		ips200_show_int(0,40,coord_now[2],3);
//		ips200_show_int(20, 0,  jianzhaopian_flag , 5); 
//		ips200_show_int(20, 20,  ImageFlag.image_element_rings_flag , 5); 

//		ips200_show_int(0,0,ImageFlag.image_element_rings,3);
//		ips200_show_int(0,20,Miss_Left_lines_flag,3);
//		ips200_show_int(0,40,Miss_Right_lines_flag,3);
//		ips200_show_int(0,60,ImageStatus_num,5);
//		ips200_show_int(20,0,ImageStatus.Miss_Right_lines,3);
//		ips200_show_int(20,20,ImageStatus.Miss_Left_lines,3);

//		ips200_show_int(0,0,shizi_wanqian_flag,3);
//		ips200_show_int(0,20,cross_flag,3);
//		ips200_show_int(0,40,cross_flag_dir,3);
//		ips200_show_int(0,60,right_shizi_flag,5);



//ips200_show_int(0,0, Zebra_flag,3);            //      2  -183 2 3
//ips200_show_int(0,20,encoder_pic_angle,4);
//ips200_show_int(0,40,roate_determine_angle,4);
//ips200_show_int(0,60,coord_now[2],4);
//ips200_show_int(40,20,test_test,4);

//ips200_show_int(0,0 ,shuzi_pic[0],1);
//ips200_show_int(0,20,shuzi_pic[1],1);
//ips200_show_int(0,40,shuzi_pic[2],1);
//ips200_show_int(0,60,shuzi_pic_num,1);

//ips200_show_int(15,0 ,shizi_zimu[0],2);
//ips200_show_int(15,20,shizi_zimu[1],2);
//ips200_show_int(15,40,shizi_zimu[2],2);
//ips200_show_int(15,60,shizi_zimu[3],2);

//ips200_show_int(30,0 ,shizi_zimu[4],2);
//ips200_show_int(30,20,shizi_zimu_num,2);
//ips200_show_int(30,40,shi_zi_pic_num,2);

//ips200_show_int(45,0 ,huandao_zimu[0],2);
//ips200_show_int(45,20,huandao_zimu[1],2);
//ips200_show_int(45,40,huandao_zimu[2],2);
//ips200_show_int(45,60,huandao_zimu[3],2);

//ips200_show_int(60,0 ,huandao_zimu[4],2);
//ips200_show_int(60,20,huandao_zimu_num,2);
//ips200_show_int(60,40,huandao_pic_num,2);
//ips200_show_int(60,60,pic_cunchu_num,2);

ips200_show_int(0,140,pic_cunchu[0],2);
ips200_show_int(0,160,pic_cunchu[1],2);
ips200_show_int(0,180,pic_cunchu[2],2);
ips200_show_int(0,200,pic_cunchu[3],2);		
ips200_show_int(0,220,pic_cunchu[4],2);	

ips200_show_int(20,80,xyz,2);
///////////////////////////////////////////////////////////////////////////ZBERA
//ips200_show_int(0,0, Zebra_flag,3);            //      2  -183 2 3
//ips200_show_int(0,20,encoder_pic_angle,4);
//ips200_show_int(0,40,roate_determine_angle,4);
//ips200_show_int(0,60,coord_now[2],4);
//ips200_show_int(40,20,test_test,4);

//ips200_show_int(60,0 ,shuzi_pic[0],1);
//ips200_show_int(60,20,shuzi_pic[1],1);
//ips200_show_int(60,40,shuzi_pic[2],1);
//ips200_show_int(60,60,shuzi_pic_num,4);



//////////////////////////////////////////////////////////////////////////ZEBRA
//		ips200_show_int(0,40,cross_flag,1);
//		ips200_show_int(0,60,cross_flag_dir,1);
		
//		ips200_show_int(20,0,ImageFlag.image_element_rings,1);
//		ips200_show_int(20,20,angle_shizi,5);
		
		//ImageFlag.image_element_rings0/*0 :无圆环          1 :左圆环       2 :右圆环*/
        //ips200_show_int(90, 140, state_flag , 5);		
//////////////////////////屏显/////////////////////////////////////	

//////////////////////////////////////////////////////////////////////////////////////////////////////////////斑马线放照片	
//////////////////////////////////////////////////////////////////////////////////////////////////////////////斑马线放照片	
//////////////////////////////////////////////////////////////////////////////////////////////////////////////斑马线放照片	

/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//十字确定
		if(shizi_dir_queding[shizi_dir_queding_flag]==1)
		{
			cross_flag_dir=1;
		}
		if(shizi_dir_queding[shizi_dir_queding_flag]==2)
		{
			cross_flag_dir=2;
		}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
		if(ImageFlag.Zebra_Flag == 1)
		{	
			if(Zebra_flag == 0)
			{
				coord_aim_pic_x_shizi_zebra = coord_now[0]; // 后续考虑是否将其清除
				coord_aim_pic_y_shizi_zebra = coord_now[1];
			
				if(!isProcessed) // 检查是否已经处理过    已检查 确实只进一次
				{
					test_test++;
					//////////////////////////////////////////////////////////////////////////
					for(int i = 0; i < pic_cunchu_num; i++) 
					{
						if (4 <= pic_cunchu[i] && pic_cunchu[i] <= 8) 
						{
							pic_cunchu[i] = 1;
						} 
						else if (9 <= pic_cunchu[i] && pic_cunchu[i] <= 14) 
						{
							pic_cunchu[i] = 2;
						} 
						else if (15 <= pic_cunchu[i] && pic_cunchu[i] <= 18) 
						{
							pic_cunchu[i] = 3;
						}
					}
					//////////////////////////////////////////////////////////////////////////
					isProcessed = true; // 设置标志位为true，表示已经处理过
					
					
					encoder_pic_angle = coord_now[2]-180;
				}					
				roate_shun();		
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag = 0;
					Zebra_flag = 1;
				}			
			}
			if(Zebra_flag==1)				
			{
				if(pic_cunchu_num==0)//后续考虑是否将其清除
				{
					Zebra_flag=3;//想清零后续再考虑一下
				}	
				eiq_mode_left=2;                		//将摄像头调车放照片模式
				eiq_mode_right=2;						//将摄像头调车放照片模式
				if(state_left==0&&state_right==0)       //没有很好打脚或者前进可能是因为转圈途中繁盛故障
				{
					if(shuzi_pic_num==0)
					{
						vector_y = 10;				
						speed_coord_xun();		
					}
					else
					{
						stop();
					}						
				}
				if(state_left==1||state_right==1)
				{					
					if(shuzi_pic_num==0)
					{
						vector_y = 5;				
						speed_coord_xun();		
					}
					else
					{
						stop();
					}	
				}	
/*左数字*/		if(state_left==2)//左边有照片
				{			
					pic_speed_coord(0,0,0);					//停车							
				}
				if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
				{    
					pic_speed_coord(0,0,0);	
					xyz=0;
					shuzi_pic_num_flag=0;
					uart_write_byte(UART_1, 1);	
					camera_determine_left=0;               //开关摄像头
					camera_determine_right=1;		
					
					if(!isInitialPositionRecorded) // 检查是否已经记录初始位置
					{
						coord_aim_pic_x_shizi = coord_now[0]; // 记录初始位置
						coord_aim_pic_y_shizi = coord_now[1]; // 可以共用
						isInitialPositionRecorded = true; // 设置标志位为true，表示已经记录初始位置
					}
					
				}			
				if(state_left==4)                           //保持车身静止，方便进行eiq深度学习并捡照片
				{
					speed_coord_color(0,0);                 //矩形识别进行矫正
					if(shuzi_pic_num==0)
					{
						coord_aim_pic_x_shizi_zebra_1 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_1 = coord_now[1];

					}
						
					if(shuzi_pic_num==1)
					{
						coord_aim_pic_x_shizi_zebra_2 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_2 = coord_now[1];
			
					}
						
					if(shuzi_pic_num==2)
					{
						coord_aim_pic_x_shizi_zebra_3 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_3 = coord_now[1];							
					}
				}		
				if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
				{
					speed_target[0] = 0;
					speed_target[1] = 0;
					speed_target[2] = 0;
					speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	  可能要放中断里
					v1 = 0;
					v2 = 0;
					v3 = 0;
					v4 = 0;
				}
				
				if(state_left==6)//条件突然不满足了                           //eiq深度学习
				{	
					speed_target[0] = 0;                    //车身静止不动
					speed_target[1] = 0;
					speed_target[2] = 0;
					speed_target[3] = 0;  
					v1 = 0;
					v2 = 0;
					v3 = 0;
					v4 = 0;		
					
					if(xyz!=27)//只给其赋值一次，确保准确性
					{
						//shuzi_pic[shuzi_pic_num]=eiq_left;              //因为测试所以被注释掉了
						eiq_one_flag=1;
					}		
					if(shuzi_pic[shuzi_pic_num]==pic_cunchu[(pic_cunchu_num-1)]&&xyz!=2)
					{
						sevor_left_flag=1;//放照片
						sevor_set();	
					}										
					else
					{	
						if(((myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)||xyz==2)&&xyz!=0)
						{
							xyz=2;
							//pic_speed_coord(0,0,0);                  //保持车身正直							
							uart_write_byte(UART_1, 8);              //强制清除数据
							camera_determine_left=0;               //开关摄像头
							camera_determine_right=0;
							if(shuzi_pic_num_flag==0)
							{
								if(shuzi_pic_num==0)
								{
									shuzi_pic_num=1;
								}							
								else if(shuzi_pic_num==1)
								{
									shuzi_pic_num=2;
								}							
								else if(shuzi_pic_num==2)
								{
									shuzi_pic_num=3;//只用于判断读取字母过程已经完成了
								}
								shuzi_pic_num_flag=1;
						    }							
							if(shuzi_pic_num==3)
							{
								Zebra_flag_flag=1;
							}
						}
						else if(xyz!=2)
						{
							xyz=1;
							if(shuzi_pic_num==0)
							{
								coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
								coord_aim[1] = coord_aim_pic_y_shizi+2;	 
							}
							if(shuzi_pic_num==1)
							{
								coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
								coord_aim[1] = coord_aim_pic_y_shizi+4;	 
							}
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}
					}						
				}	
				
/*右数字*/		if(state_right==2)//右边有数字
				{		
					pic_speed_coord(0,0,0);					//停车		
				}
				if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
				{     
					pic_speed_coord(0,0,0);	
					xyz=0;
					shuzi_pic_num_flag=0;
					uart_write_byte(UART_4, 1);
					camera_determine_left=1;               //开关摄像头
					camera_determine_right=0;					
					if(!isInitialPositionRecorded) // 检查是否已经记录初始位置
					{

						coord_aim_pic_x_shizi = coord_now[0]; // 记录初始位置
						coord_aim_pic_y_shizi = coord_now[1]; // 可以共用
						isInitialPositionRecorded = true; // 设置标志位为true，表示已经记录初始位置
					}
				}
				if(state_right==4)                           //保持车身静止，方便进行eiq深度学习并捡照片
				{			
					if(shuzi_pic_num==0)
					{
						coord_aim_pic_x_shizi_zebra_1 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_1 = coord_now[1];

					}
						
					if(shuzi_pic_num==1)
					{
						coord_aim_pic_x_shizi_zebra_2 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_2 = coord_now[1];
			
					}
						
					if(shuzi_pic_num==2)
					{
						coord_aim_pic_x_shizi_zebra_3 = coord_now[0];
						coord_aim_pic_y_shizi_zebra_3 = coord_now[1];							
					}					
					speed_coord_color_right(0,0);                 //矩形识别进行矫正
				}		
				if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
				{
					speed_target[0] = 0;
					speed_target[1] = 0;
					speed_target[2] = 0;
					speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
					v1 = 0;
					v2 = 0;
					v3 = 0;
					v4 = 0;
				}
				if(state_right==6)                           //eiq深度学习
				{
					speed_target[0] = 0;                    //车身静止不动
					speed_target[1] = 0;
					speed_target[2] = 0;
					speed_target[3] = 0;  
					v1 = 0;
					v2 = 0;
					v3 = 0;
					v4 = 0;	
					//对eiq深度学习数据进行处理
					if(xyz!=2)//只给其赋值一次，确保准确性
					{
						//shuzi_pic[shuzi_pic_num]=eiq_right;              //因为测试所以被注释掉了
						eiq_one_flag=1;
					}	
					
					if(shuzi_pic[shuzi_pic_num]==pic_cunchu[(pic_cunchu_num-1)]&&xyz!=2)
					{
						sevor_right_flag=1;//放照片
						sevor_set();
					}										
					else
					{
						
						if(((myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)||xyz==2)&&xyz!=0)
						{
							xyz=2;	
							camera_determine_left=0;               //开关摄像头
							camera_determine_right=0;							
							uart_write_byte(UART_4, 8);              //强制清除数据						
							if(shuzi_pic_num_flag==0)//只判断一次
							{
								if(shuzi_pic_num==0)
								{
									shuzi_pic_num=1;
								}							
								else if(shuzi_pic_num==1)
								{
									shuzi_pic_num=2;
								}							
								else if(shuzi_pic_num==2)
								{
									shuzi_pic_num=3;//只用于判断读取字母过程已经完成了
								}
								shuzi_pic_num_flag=1;
						    }	

							
							if(shuzi_pic_num==3)
							{
								Zebra_flag_flag=1;
							}
						}
						else if(xyz!=2)
						{
							xyz=1;
							if(shuzi_pic_num==0)
							{
								coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
								coord_aim[1] = coord_aim_pic_y_shizi+zhongdian_move;	 
							}
							if(shuzi_pic_num==1)
							{
								coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
								coord_aim[1] = coord_aim_pic_y_shizi+zhongdian_move;	 
							}
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}
					}
				}
				
				if(Zebra_flag_flag==1&&state_right==0&&state_left==0)
				{
					Zebra_flag=2;
					Zebra_flag_flag=0;
					eiq_one_flag=0;
				}				
			}
			if(Zebra_flag==2)	
			{
				Zebra_determine();//3104行
				if(Zebra_determine_flag==2)
				{					
					if(pic_cunchu_num==0)//照片放完咯
					{
						Zebra_flag=3;//想清零后续再考虑一下
						Zebra_flag_flag=0;
						Zebra_determine_flag=0;
					}							
					if(shuzi_pic[Zebra_position]==pic_cunchu[(pic_cunchu_num-1)])
					{
						if(error_jisuan==1)
						{
							sevor_right_flag=1;//放照片						
						}
						if(error_jisuan==0)
						{
							sevor_left_flag=1;//放照片								
						}						

						sevor_set();
					}										
					else
					{
						Zebra_flag_flag=1;
					}	
	
					if(Zebra_flag_flag==1)
					{
						Zebra_determine_flag=0;
						Zebra_flag_flag=0;
					}				
				}
			}
    		if(Zebra_flag==3)	//走过斑马线停
			{
				roate_ni();								
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag = 0;
					Zebra_flag = 4;
					ssss=0;
					encoder_pic_angle=encoder_pic_angle+180;
				}
			}			
			if(Zebra_flag==4)	//走过斑马线停  到此全部结束
			{

				if(ssss<1500)
				{
					vector_y = 20;				
					speed_coord_xun();	
				}
				else
			    {
					speed_target[0] = 0;//使车辆保持静止
					speed_target[1] = 0;
					speed_target[2] = 0;
					speed_target[3] = 0; 
					v1=0;
					v2=0;
					v3=0;
					v4=0;	
				}
			}						
		}		
////////////////////////////////////////////////////////////////////////////////////////////////////////////////左环岛	
////////////////////////////////////////////////////////////////////////////////////////////////////////////////左环岛	
////////////////////////////////////////////////////////////////////////////////////////////////////////////////左环岛	
		//if(ImageFlag.image_element_rings==1)
		else if(ImageFlag.image_element_rings!=0)
		//if(0)
		{
			if(ImageFlag.image_element_rings ==1)
			{
				eiq_mode_left=1;
				eiq_mode_right=2;
				if(coord_aim_pic_x_shizi==0&&yaw_angle_flag==1)//刚进环岛时候的位置，后续清零
				{
					coord_aim_pic_x_shizi = coord_now[0];
					coord_aim_pic_y_shizi = coord_now[1];
					encoder_pic_angle_huandao=coord_now[2]; //记录初始角度
					encoder_pic_angle=encoder_pic_angle_huandao;//保持车身角度				
					continue_raoquan_flag=1;
				}
				if(huandao_wangqian==0)
				{
	
					if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
					{
						huandao_wangqian=1;
					}
					else
					{
						jianzhaopian_flag=1;
						coord_aim[0] = coord_aim_pic_x_shizi-0.8;          //回到原点
						coord_aim[1] = coord_aim_pic_y_shizi+huan_dao_qianjing_distance;	 
						speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
					}
				}
				else if(huandao_wangqian==1)
				{	
/*捡照片*/			if(left_huandao_flag==0)
					{
						//eiq_mode=1;
						
						//这样可能会和开关摄像头冲突
						if(state_left==0)
						{
							//vector_y = 0;	
							//speed_coord_xun();
							speed_target[0] = 0;//使车辆保持静止
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0; 
							v1=0;
							v2=0;
							v3=0;
							v4=0;							
							camera_determine_left=0;               //开关摄像头
							camera_determine_right=1;              //关闭右摄像头
							if(num_kong_flag==0)
							{
								num_kong=0;
								num_kong_flag=1;
							}
							
						}
						if(state_left==1)
						{
							//vector_y = 5;
							//speed_coord_xun();
							speed_target[0] = 0;//使车辆保持静止
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0; 
							v1=0;
							v2=0;
							v3=0;
							v4=0;
							
						}
						if(state_left==2)//左边有照片
						{
							jianzhaopian_flag = 1;                 //暂停环岛进程    记得清楚标志位
							stop();		                             //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							encoder_pic_angle=encoder_pic_angle_huandao;
		
						}
						
						
						if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{      
							eiq_one_flag=0;
							uart_write_byte(UART_1, 1);
							pic_speed_coord(0,0,0);			
						}
						else//判断是否有照片（环岛或十字）
						{
							if ((num_kong > 2000)&&num_kong_flag==1) 
							{ // 如果计数器超过2000次
								left_huandao_flag_flag = 2; // 设置左环岛标志位为2
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
							}
							
							if(state_left>=3)
							{
								num_kong_flag=2;
							}
						}
																		
						if(state_left==4)//确认有照片，并使用矩形识别直接捡照片， （关闭摄像头是指n=0且flag_exchange==0）
						{
							speed_coord_color(0,0);                 //矩形识别进行矫正
							sevor_left_flag=0;
						}
						if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						if(state_left==6)                           //eiq深度学习
						{
							
							//if(eiq_left!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
							sevor_set();
							if(1)//只给其赋值一次，确保准确性
							{
								yi_yiduiying();
								pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}	
							//对eiq深度学习数据进行处理
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
														 //舵机捡照片
							uart_write_byte(UART_1, 2);	             //判断是否还有照片    
						}
						if(state_left==7)                           //矩形识别发现符合要求矩形 (深度学习)
						{
							eiq_one_flag=0;                          //深度学习只进一次
							//pic_speed_coord(0,0,0);                  //保持车身正直    貌似效果不佳
							sevor_left_flag = 0;                    //设置标志位为零，进6时候再次捡照片
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;	
							sevor_left_flag=0;                   //设置标志位为零，进6时候再次捡照片	
							if(myabs(x_pic)<=7&&myabs(y_pic)<=7)
							{
								uart_write_byte(UART_1, 4);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
							
							else							
							{
								uart_write_byte(UART_1, 1);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
						}
						if(state_left==8)                           //矩形识别没发现矩形，回原点
						{
							eiq_one_flag=0;                          //深度学习只进一次
							coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+huan_dao_qianjing_distance-1;	         
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								pic_speed_coord(0,0,0);                  //保持车身正直
								//uart_write_byte(UART_1, 4);              //告诉摄像头进程已完成，清零所有标志位，并将state_left置0
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
								
								left_huandao_flag_flag=1;
								
								jianzhaopian_flag=0;
								ImageFlag.image_element_rings_flag=6;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(left_huandao_flag_flag==1&&open_close_left==1&&open_close_right==1)//表示摄像头已经开启成功
						{
							left_huandao_flag_flag=0;
							//left_huandao_flag=1;
							left_huandao_flag=2;
							
						}
						if(left_huandao_flag_flag==2&&open_close_left==1&&open_close_right==1)
						{
							ImageFlag.image_element_rings_flag=6;
							left_huandao_flag=3;
							left_huandao_flag_flag=0;
							//continue_raoquan_flag=0;
							jianzhaopian_flag=0;//可有可无
						}	
						
					}
/*匹配字母*/
					if(left_huandao_flag==1)
					{
						if(state_right==0)
						{
							vector_y = 20;
							speed_coord_xun();
							hzw_flag = 0;
							roate_tiaozhen();
							if((coord_now[2]>(encoder_pic_angle_huandao+left_huandao_0-15))&&huandao_zimu_num==-1)
							{
								//开启摄像头，靠近第一张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=1;               //关闭
								camera_determine_right=0;              //打开
								huandao_zimu_num=0;
								
							}
							if((coord_now[2]>(encoder_pic_angle_huandao+left_huandao_1-15))&&huandao_zimu_num==0)
							{
								//开启摄像头，靠近第二张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=1;               //关闭
								camera_determine_right=0;              //打开
								huandao_zimu_num=1;
							}
							if((coord_now[2]>(encoder_pic_angle_huandao+left_huandao_2-15))&&huandao_zimu_num==1)
							{
								//开启摄像头，靠近第三张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=1;               //关闭
								camera_determine_right=0;              //打开
								huandao_zimu_num=2;
							}
							if((coord_now[2]>(encoder_pic_angle_huandao+left_huandao_3-15))&&huandao_zimu_num==2)
							{
								//开启摄像头，靠近第四张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=1;               //关闭
								camera_determine_right=0;              //打开
								huandao_zimu_num=3;
							}
							if((coord_now[2]>(encoder_pic_angle_huandao+left_huandao_4-15))&&huandao_zimu_num==3)
							{
								//开启摄像头，靠近第五张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=1;               //关闭
								camera_determine_right=0;              //打开
								huandao_zimu_num=4;
							}	
						}	
		
						if(state_right==1)
						{
							vector_y = 8;
							speed_coord_xun();
						}
						if(state_right==2)
						{
							jianzhaopian_flag = 1;                 //暂停环岛进程
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							if(huandao_zimu_num==0)
							{
								encoder_pic_angle=left_huandao_0;
							}
							if(huandao_zimu_num==1)
							{
								encoder_pic_angle=left_huandao_1;
							}
							if(huandao_zimu_num==2)
							{
								encoder_pic_angle=left_huandao_2;
							}
							if(huandao_zimu_num==3)
							{
								encoder_pic_angle=left_huandao_3;
							}
							if(huandao_zimu_num==4)
							{
								encoder_pic_angle=left_huandao_4;
							}
							
						}
						if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{         
							coord_aim[0] = coord_aim_pic_x+0.3;          //回到原点
							coord_aim[1] = coord_aim_pic_y;	
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{						
								uart_write_byte(UART_4, 1);
							}
							else
							{				
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(state_right==4)//确认有字母
						{
							speed_coord_color_right(0,0);                 //矩形识别进行矫正
		
						}
						if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						if(state_right==6)
						{
		
							if(1)//只给其赋值一次，确保准确性
							{
								huandao_zimu[huandao_zimu_num]=eiq_right;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}
							if(huandao_zimu[huandao_zimu_num]==pic_cunchu[pic_cunchu_num-1])
							{
								sevor_right_flag=1;//放照片
								sevor_set();
							}
							else
							{
								coord_aim[0] = coord_aim_pic_x;          //回到原点
								coord_aim[1] = coord_aim_pic_y;	
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									//pic_speed_coord(0,0,0);                  //保持车身正直
									camera_determine_left=1;               //关闭左摄像头
									camera_determine_right=1;              //关闭右摄像头
									speed_target[0] = 0;              		   //保持车身不动
									speed_target[1] = 0;
									speed_target[2] = 0;
									speed_target[3] = 0;                    
									v1 = 0;
									v2 = 0;
									v3 = 0;
									v4 = 0;	
									hzw_flag = 1;
									jianzhaopian_flag=0;
									roate_tiaozhen_flag=0;
									//默认继续前进					state_right=0
									if(huandao_zimu_num==4)
									{
										left_huandao_flag_flag=1;
									}
									if(huandao_pic_num==0)
									{
										left_huandao_flag_flag=2;
									}
								}
								else
								{
									if(hzw_flag == 0)
									{								
										speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
									}
								}
							}
						}
						if(left_huandao_flag_flag==1&&huandao_zimu_num==4&&state_right==0)
						{
							left_huandao_flag=2;
							left_huandao_flag_flag=0;
						}
						if(left_huandao_flag_flag==2&&state_right==0)
						{
							ImageFlag.image_element_rings_flag=6;
							left_huandao_flag=3;
							left_huandao_flag_flag=0;
							//continue_raoquan_flag=0;
						}						
					}	
					
/*放照片*/			if(left_huandao_flag==2)
					{						
						left_huandao_determine();
						if(raoquan_determine_flag==1)
						{
							if(left_huandao_flag_flag==1)
							{	
								left_huandao_flag_flag=0;
								left_huandao_flag=3;
								//continue_raoquan_flag=0;
							}							
							if(raoquan_determine_flag==1)
							{						
								roate_tiaozhen();
								if(shizi_flag_flag==0)
								{
									vector_y = 15;
									speed_coord_xun();		

									if((coord_now[2])>(angle_huandao-nei_qie_angle_huandao))//+20是提前
									{
										shizi_flag_flag=1;//停车直接放照片
										stop();	
									}			
								}							
								if(shizi_flag_flag==1)//左边有字母
								{
									stop();		                           //停车		
									coord_aim_pic_x = coord_now[0];        //记录初始位置
									coord_aim_pic_y = coord_now[1];
									encoder_pic_angle=angle_huandao-nei_qie_angle_huandao;
									shizi_flag_flag=2;
								}
								if(shizi_flag_flag==2)//左边有字母
								{							
									coord_aim[0]=coord_aim_pic_x;        //记录初始位置
									if(angle_huandao_flag==0)
									{
										coord_aim[1]=coord_aim_pic_y+nei_qie_distance_B;
									}
									else
									{
										coord_aim[1]=coord_aim_pic_y+nei_qie_distance_huandao;
									}
									
									if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
									{
										shizi_flag_flag=3;
									}
									else
									{
										speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
									}								
								}							
								if(shizi_flag_flag==3)
								{
									if(huandao_zimu[angle_huandao_flag]==pic_cunchu[(pic_cunchu_num-1)])//变量共用了
									{
										sevor_right_flag=1;//放照片
										sevor_set();
									}							
									else
									{
										coord_aim[0]=coord_aim_pic_x;        //记录初始位置
										coord_aim[1]=coord_aim_pic_y;
										if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
										{
											//默认继续前进					
											raoquan_determine_flag=0;
											coord_now_flag=0;        				 	//是否减去360
											roate_tiaozhen_flag=0;
											shizi_flag_flag=0;
											if(huandao_pic_num==0)
											{
												left_huandao_flag_flag=1;
											}
										}
										else
										{
											speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
										}
									}	
									
								}
								
							}	
						}							
					}
						
/*出环岛*/			if(left_huandao_flag==3)
					{
						
						roate_tiaozhen();
						
						vector_y = chu_yuansu_sudu;
						speed_coord_xun();
						
						if((coord_now[2]<=(encoder_pic_angle_huandao+360-20))&&count_flag == 0)
						{
							num_pic=0;
							count_flag = 0;//可以与左环岛共用
						}
						if((coord_now[2]>(encoder_pic_angle_huandao+360-20))&&count_flag == 0)
						{
							num_pic=0;
							count_flag = 1;
							continue_raoquan_flag=0;
							ImageFlag.image_element_rings_flag = 8;
						}
						if(num_pic>chu_yuansu_sudu_time)//走出十字一秒，避免被字母困扰
						{
							cross=0;//走出十字地狱							
							huandao_zimu_num=-1;
							yaw_angle_flag=0;
							
							
							coord_aim_pic_x_shizi=0;//进环岛 初始位置
							coord_aim_pic_y_shizi=0;
							
							
							count_flag=0;
							jianzhaopian_flag=0;
							ImageFlag.image_element_rings=0;
							huandao_wangqian=0;						
							left_huandao_flag=0;//左转十字标志位置零
							
							num_kong_flag = 0; // 重置计数器标志位
							
							roate_tiaozhen_flag=0;
							
							
							dasdsadadasdadasda=1;
						}
					}
				}				
				
			}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右环岛	
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右环岛
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右环岛
			else if(ImageFlag.image_element_rings ==2)
			{		
				eiq_mode_left=2;
				eiq_mode_right=1;				
				if(coord_aim_pic_x_shizi==0&&yaw_angle_flag==1)//刚进环岛时候的位置，后续清零
				{
					coord_aim_pic_x_shizi = coord_now[0];
					coord_aim_pic_y_shizi = coord_now[1];
					encoder_pic_angle_huandao=coord_now[2]; //记录初始角度
					encoder_pic_angle=encoder_pic_angle_huandao;//保持车身角度				
					continue_raoquan_flag=1;
				}				
				if(huandao_wangqian==0)
				{	
					if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
					{
						huandao_wangqian=1;
					}
					else
					{
						coord_aim[0] = coord_aim_pic_x_shizi+0.8;          //回到原点
						coord_aim[1] = coord_aim_pic_y_shizi+huan_dao_qianjing_distance;	 
						speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
					}
				}
				else if(huandao_wangqian==1)
				{			
/*捡照片*/			
					if(right_huandao_flag==0)
					{
						if(state_right==0)
						{
							vector_y = 10;	
							speed_coord_xun();								
							camera_determine_left=1;               //开关摄像头
							camera_determine_right=0;              //关闭右摄像头
							if(num_kong_flag==0)
							{
								num_kong=0;
								num_kong_flag=1;
							}						
						}
						if(state_right==1)
						{
							vector_y = 10;
							speed_coord_xun();
						}
						if(state_right==2)//左边有照片
						{
							jianzhaopian_flag = 1;                 //暂停环岛进程    记得清楚标志位
							stop();		                             //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							encoder_pic_angle=encoder_pic_angle_huandao;
		
						}
						
						
						if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{         
							eiq_one_flag=0;
							uart_write_byte(UART_4, 1);
							pic_speed_coord(0,0,0);
						}
						
						else//判断是否有照片（环岛或十字）
						{
							if ((num_kong > 2000)&&num_kong_flag==1) 
							{ // 如果计数器超过2000次
								right_huandao_flag_flag = 2; // 设置左环岛标志位为2
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
							}						
							if(state_right>=3)
							{
								num_kong_flag=2;
							}
							
						}
						///////////////////////////////////////////////可能有问题，如果再进入
						
						if(state_right==4)//确认有照片，并使用矩形识别直接捡照片， （关闭摄像头是指n=0且flag_exchange==0）
						{
							speed_coord_color_right(0,0);                 //矩形识别进行矫正
							sevor_right_flag=0;
						}
						if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						if(state_right==6)                           //eiq深度学习
						{
							//对eiq深度学习数据进行处理
							
							//if(eiq_right!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
							sevor_set();
							if(1)//只给其赋值一次，确保准确性
							{
								yi_yiduiying();
								pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}	
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
														 //舵机捡照片
							uart_write_byte(UART_4, 2);	             //判断是否还有照片    
						}
						if(state_right==7)                           //矩形识别发现符合要求矩形 (深度学习)
						{
							eiq_one_flag=0;
							//pic_speed_coord(0,0,0);                  //保持车身正直    貌似效果不佳
							sevor_right_flag = 0;                    //设置标志位为零，进6时候再次捡照片
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;	
							sevor_right_flag=0;                   //设置标志位为零，进6时候再次捡照片					
							if(myabs(x_pic_right)<=7&&myabs(y_pic_right)<=7)
							{
								uart_write_byte(UART_4, 4);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
							
							else							
							{
								uart_write_byte(UART_4, 1);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
						}
						if(state_right==8)                           //矩形识别没发现矩形，回原点
						{    
							coord_aim[0] = coord_aim_pic_x_shizi;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+huan_dao_qianjing_distance-1;	 
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								pic_speed_coord(0,0,0);                //保持车身正直
								//uart_write_byte(UART_1, 4);          //告诉摄像头进程已完成，清零所有标志位，并将state_left置0
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
								right_huandao_flag_flag=1;
								jianzhaopian_flag=0;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(right_huandao_flag_flag==1&&open_close_left==1&&open_close_right==1)//表示摄像头已经开启成功
						{
							
							right_huandao_flag_flag=0;
							//right_huandao_flag=1;
							right_huandao_flag=2;
							
						}
						
						
						if(right_huandao_flag_flag==2&&open_close_left==1&&open_close_right==1)
						{
							right_huandao_flag=3;
							right_huandao_flag_flag=0;
							//continue_raoquan_flag=0;
							jianzhaopian_flag=0;//可有可无
						}	
						
					}				
/*匹配字母*/			
					if(right_huandao_flag==1)
					{
						if(state_left==0)
						{
							vector_y = 10;
							speed_coord_xun();
							hzw_flag = 0;
							roate_tiaozhen();
							if((coord_now[2]<(encoder_pic_angle_huandao+right_huandao_0+15))&&huandao_zimu_num==-1)
							{
								//开启摄像头，靠近第一张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=0;               //打开
								camera_determine_right=1;              //关闭
								huandao_zimu_num=0;
								
							}
							if((coord_now[2]<(encoder_pic_angle_huandao+right_huandao_1+15))&&huandao_zimu_num==0)
							{
								//开启摄像头，靠近第二张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=0;               //打开
								camera_determine_right=1;              //关闭
								huandao_zimu_num=1;
							}
							if((coord_now[2]<(encoder_pic_angle_huandao+right_huandao_2+15))&&huandao_zimu_num==1)
							{
								//开启摄像头，靠近第三张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=0;               //打开
								camera_determine_right=1;              //关闭
								huandao_zimu_num=2;
							}
							if((coord_now[2]<(encoder_pic_angle_huandao+right_huandao_3+15))&&huandao_zimu_num==2)
							{
								//开启摄像头，靠近第四张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=0;               //打开
								camera_determine_right=1;              //关闭
								huandao_zimu_num=3;
							}
							if((coord_now[2]<(encoder_pic_angle_huandao+right_huandao_4+15))&&huandao_zimu_num==3)
							{
								//开启摄像头，靠近第五张对应字母，匹配完关闭，如果字母与照片相符，放照片
								camera_determine_left=0;               //打开
								camera_determine_right=1;              //关闭
								huandao_zimu_num=4;
							}	
							sda=0;
						}	
						if(state_left==1)
						{
							vector_y = 8;
							speed_coord_xun();
						}
						if(state_left==2)
						{
							jianzhaopian_flag = 1;                 //暂停环岛进程
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							
							//if(sda==0)
							//{
								//sda=1;
								if(huandao_zimu_num==0)
								{
									encoder_pic_angle=right_huandao_0;
								}
								else if(huandao_zimu_num==1)
								{
									encoder_pic_angle=right_huandao_1;
								}
								else if(huandao_zimu_num==2)
								{
									encoder_pic_angle=right_huandao_2;
								}
								else if(huandao_zimu_num==3)
								{
									encoder_pic_angle=right_huandao_3;
								}
								else if(huandao_zimu_num==4)
								{
									encoder_pic_angle=right_huandao_4;
								}
							//}
						}
						if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{     
							coord_aim[0] = coord_aim_pic_x-0.3;          //回到原点
							coord_aim[1] = coord_aim_pic_y;	
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{						
								uart_write_byte(UART_1, 1);
							}
							else
							{				
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(state_left==4)//确认有字母
						{
							speed_coord_color(0,0);                 //矩形识别进行矫正
		
						}
						if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						if(state_left==6)
						{		
							//if(eiq_left!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
							if(1)//只给其赋值一次，确保准确性
							{
								huandao_zimu[huandao_zimu_num]=eiq_left;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}	
							if(huandao_zimu[huandao_zimu_num]==pic_cunchu[pic_cunchu_num-1])
							{
								sevor_left_flag=1;//放照片
								sevor_set();
							}
							else
							{
								coord_aim[0] = coord_aim_pic_x;          //回到原点
								coord_aim[1] = coord_aim_pic_y;	
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									//pic_speed_coord(0,0,0);                  //保持车身正直
									camera_determine_left=1;               //关闭左摄像头
									camera_determine_right=1;              //关闭右摄像头
									speed_target[0] = 0;              		   //保持车身不动
									speed_target[1] = 0;
									speed_target[2] = 0;
									speed_target[3] = 0;                    
									v1 = 0;
									v2 = 0;
									v3 = 0;
									v4 = 0;	
									hzw_flag = 1;
									jianzhaopian_flag=0;		
									roate_tiaozhen_flag=0;
									//默认继续前进					state_right=0
									
									if(huandao_zimu_num==4)
									{
										right_huandao_flag_flag=1;
									}
									if(huandao_pic_num==0)
									{
										right_huandao_flag_flag=2;
									}
								}
								else
								{
									if(hzw_flag == 0)
									{								
										speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
									}
									
								}
							}
						}
						if(right_huandao_flag_flag==1&&huandao_zimu_num==4&&state_left==0)
						{
							right_huandao_flag=2;
							right_huandao_flag_flag=0;
							eiq_one_flag=0;
						}	
						if(right_huandao_flag_flag==2&&state_left==0)
						{
							right_huandao_flag=3;
							right_huandao_flag_flag=0;
							//continue_raoquan_flag=0;
							eiq_one_flag=0;

						}	
						
					}	
/*放照片*/			if(right_huandao_flag==2)
					{						
						right_huandao_determine();			
						if(raoquan_determine_flag==1)
						{					
							if(right_huandao_flag_flag==1)
							{	
								right_huandao_flag_flag=0;
								right_huandao_flag=3;
								//continue_raoquan_flag=0;						
							}								
						
							roate_tiaozhen();
							if(shizi_flag_flag==0)
							{
								vector_y = 15;
								speed_coord_xun();		
								if((coord_now[2])<(angle_huandao+nei_qie_angle_huandao))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}			
							}							
							if(shizi_flag_flag==1)//左边有字母
							{
								stop();		                           //停车		
								coord_aim_pic_x = coord_now[0];        //记录初始位置
								coord_aim_pic_y = coord_now[1];
								encoder_pic_angle=angle_huandao+nei_qie_angle_huandao;
								shizi_flag_flag=2;
							}
							if(shizi_flag_flag==2)//左边有字母
							{							
								if(angle_huandao_flag==0)
								{
									coord_aim[1]=coord_aim_pic_y+nei_qie_distance_B;
								}
								else
								{
									coord_aim[1]=coord_aim_pic_y+nei_qie_distance_huandao;
								}
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									shizi_flag_flag=3;
								}
								else
								{
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
								}								
							}							
							if(shizi_flag_flag==3)
							{
								if(huandao_zimu[angle_huandao_flag]==pic_cunchu[(pic_cunchu_num-1)])//变量共用了
								{
									sevor_left_flag=1;//放照片
									sevor_set();
								}							
								else
								{
									coord_aim[0]=coord_aim_pic_x;        //记录初始位置
									coord_aim[1]=coord_aim_pic_y;
									if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
									{
										raoquan_determine_flag=0;
										coord_now_flag_right=0;        				 	//是否减去360
										roate_tiaozhen_flag=0;
										shizi_flag_flag=0;
										if(huandao_pic_num==0)
										{
											right_huandao_flag_flag=1;
										}
									}
									else
									{
										speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
									}
								}							
							}																				
					 }
						
					 
					 
					 
				 }
/*出环岛*/if(right_huandao_flag==3)
					{
						
						roate_tiaozhen();
						
						vector_y = chu_yuansu_sudu;
						speed_coord_xun();
						
						
						if((coord_now[2]>=(encoder_pic_angle_huandao-360+20))&&count_flag == 0)
						{
							num_pic=0;
							count_flag = 0;//可以与左环岛共用
						}
						if((coord_now[2]<(encoder_pic_angle_huandao-360+20))&&count_flag == 0)
						{
							num_pic=0;
							count_flag = 1;
							continue_raoquan_flag=0;
							ImageFlag.image_element_rings_flag = 8;
						}
						if(num_pic>chu_yuansu_sudu_time)//走出十字一秒，避免被字母困扰
						{
							cross=0;//走出十字地狱
							
							huandao_zimu_num=-1;
							yaw_angle_flag=0;
							coord_aim_pic_x_shizi=0;//进环岛 初始位置
							coord_aim_pic_y_shizi=0;
							count_flag=0;
							jianzhaopian_flag=0;
		
							ImageFlag.image_element_rings=0;
							huandao_wangqian=0;	
							right_huandao_flag=0;//左转十字标志位置零
							
							roate_tiaozhen_flag=0;
							
							
							
							dasdsadadasdadasda=1;
							
						}
					}	
				}				
				
			}
		}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////左十字	
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////左十字
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////左十字
		else if(cross_flag!=0)
		{
			if(cross_flag==1)
			{
				camera_determine_left=1;               //开关左摄像头
				camera_determine_right=1;              //关闭右摄像头
				vector_y = 20;	
				speed_coord_xun();
			}
			else if(cross_flag==2&&cross_flag_dir==1)
			{
				if(coord_aim_pic_x_shizi==0&&yaw_angle_flag==1)//刚进十字时候的位置，后续清零
				{
					coord_aim_pic_x_shizi = coord_now[0];
					coord_aim_pic_y_shizi = coord_now[1];
					encoder_pic_angle_shizi=coord_now[2]; //记录初始角度	
					encoder_pic_angle=encoder_pic_angle_shizi;						
				}		
				
/*捡照片*/		
				if(left_shizi_flag==0)
				{		
					eiq_mode_left=1;
					eiq_mode_right=1;					
/*捡照片*/			
					if(shizi_wanqian_flag==0)
					{
						if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
						{
							shizi_wanqian_flag=1;
						}
						else
						{
							coord_aim[0] = coord_aim_pic_x_shizi-0.4;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+shi_zi_qianjing_distance;	 
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}
					}				
					
					if(shizi_wanqian_flag==1)
					{
						if(state_left==0)
						{
							//vector_y = 0;	
							//speed_coord_xun();	
							stop();	
							camera_determine_left=0;               //开关左摄像头
							camera_determine_right=1;              //关闭右摄像头
							if(num_kong_flag==0)
							{
								num_kong=0;
								num_kong_flag=1;
							}		
						}
						if(state_left==1)
						{
							//vector_y = 0;
							//speed_coord_xun();
							stop();	
						}
						if(state_left==2)//右边有照片
						{
							//jianzhaopian_flag = 1;                 //暂停环岛进程
							stop();		                           //停车		
							//encoder_pic_angle=coord_now[2];				  
							encoder_pic_angle=encoder_pic_angle_shizi;	//回到判断到十字的位置
							//coord_now[2]=0;                     //motor.c  //不确定是否改成功,且位置不对
						}
						if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{     
							eiq_one_flag=0;							
							uart_write_byte(UART_1, 1);
							pic_speed_coord(0,0,0);
							
							
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							
							
							camera_determine_left=0;               //开关左摄像头
							camera_determine_right=1;              //关闭右摄像头
							
						}
						else//判断是否有照片（环岛或十字）
						{
							if ((num_kong > 2000)&&num_kong_flag==1) 
							{ // 如果计数器超过2000次
								left_shizi_flag_flag = 2; // 设置左环岛标志位为2
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
							}
							if(state_left>=3)
							{
								num_kong_flag=2;
							}
						}
						if(state_left==4)//确认有照片，并使用矩形识别直接捡照片，关闭左摄像头	 （关闭摄像头是指n=0且flag_exchange==0）
						{
							speed_coord_color(0,0);                 //矩形识别进行矫正
							sevor_left_flag=0;
						}
						if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						if(state_left==6)                           //eiq深度学习
						{
							//if(state_left!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
							sevor_set();
							if(1)//只给其赋值一次，确保准确性
							{
								yi_yiduiying();
								pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}	
							//对eiq深度学习数据进行处理
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
														 //舵机捡照片
							uart_write_byte(UART_1, 2);	             //判断是否还有照片    
						}
						if(state_left==7)                           //矩形识别发现符合要求矩形 (深度学习)
						{
							eiq_one_flag=0;	
							//pic_speed_coord(0,0,0);                  //保持车身正直    貌似效果不佳
							sevor_left_flag = 0;                    //设置标志位为零，进6时候再次捡照片
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;	
							sevor_left_flag=0;                   //设置标志位为零，进6时候再次捡照片					
							if(myabs(x_pic)<=7&&myabs(y_pic)<=7)
							{
								uart_write_byte(UART_1, 4);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
							
							else							
							{
								uart_write_byte(UART_1, 1);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
						}
						if(state_left==8)                           //矩形识别没发现矩形，回原点
						{
							eiq_one_flag=0;	
							coord_aim[0] = coord_aim_pic_x_shizi+0.15;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+shi_zi_qianjing_distance-0.4;	         
							if(myabs_double(coord_aim[0]-coord_now[0])<0.050&&myabs_double(coord_aim[1]-coord_now[1])<0.050)
							{
								pic_speed_coord(0,0,0);                  //保持车身正直
								//uart_write_byte(UART_1, 4);              //告诉摄像头进程已完成，清零所有标志位，并将state_left置0
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
								left_shizi_flag_flag=1;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(left_shizi_flag_flag==1&&open_close_left==1&&open_close_right==1)//表示摄像头已经开启成功
						{
							eiq_one_flag=0;	
							left_shizi_flag_flag=0;
							//left_shizi_flag=1;
							left_shizi_flag=2;
						}
						if(left_shizi_flag_flag==2&&open_close_left==1&&open_close_right==1)
						{
							eiq_one_flag=0;	
							left_shizi_flag=3;
							left_shizi_flag_flag=0;
						}
					}
					
				}
/*匹配字母*/		
				if(left_shizi_flag==1)
				{			
					eiq_mode_left=2;
					eiq_mode_right=2;	
					if(state_right==0)
					{
						vector_y = 12;
						speed_coord_xun();
						hzw_flag=0;
						if((coord_now[2]>(encoder_pic_angle_shizi+left_zhizi_0-30))&&shizi_zimu_num==-1)
						{
							//开启摄像头，靠近第一张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							shizi_zimu_num=0;
						}
						if((coord_now[2]>(encoder_pic_angle_shizi+left_zhizi_1-30))&&shizi_zimu_num == 0)
						{
							//开启摄像头，靠近第二张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							shizi_zimu_num=1;
						}
						if((coord_now[2]>(encoder_pic_angle_shizi+left_zhizi_2-30))&&shizi_zimu_num == 1)
						{
							//开启摄像头，靠近第三张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							shizi_zimu_num=2;
						}
						if((coord_now[2]>(encoder_pic_angle_shizi+left_zhizi_3-30))&&shizi_zimu_num == 2)
						{
							//开启摄像头，靠近第四张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							shizi_zimu_num=3;
						}
						if((coord_now[2]>(encoder_pic_angle_shizi+left_zhizi_4-30))&&shizi_zimu_num == 3)
						{
							//开启摄像头，靠近第五张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							shizi_zimu_num=4;
						}					
					}	
					if(state_right==1)
					{
						vector_y = 5;
						speed_coord_xun();
					}
					if(state_right==2)
					{
						//jianzhaopian_flag = 1;                 //暂停环岛进程
						stop();		                           //停车		
						coord_aim_pic_x = coord_now[0];        //记录初始位置
						coord_aim_pic_y = coord_now[1];
						//encoder_pic_angle=coord_now[2]; 
						if(shizi_zimu_num==0)
						{
							encoder_pic_angle=left_zhizi_0;
						}
						if(shizi_zimu_num==1)
						{
							encoder_pic_angle=left_zhizi_1;
						}
						if(shizi_zimu_num==2)
						{
							encoder_pic_angle=left_zhizi_2;
						}
						if(shizi_zimu_num==3)
						{
							encoder_pic_angle=left_zhizi_3;
						}
						if(shizi_zimu_num==4)
						{
							encoder_pic_angle=left_zhizi_4;
						}				
						
					}
					if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
					{  
						coord_aim[0] = coord_aim_pic_x+0.3;          //回到原点
						coord_aim[1] = coord_aim_pic_y;	
						if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
						{						
							uart_write_byte(UART_4, 1);
						}
						else
						{				
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}						
					}
					if(state_right==4)//确认有字母
					{

						speed_coord_color_right(0,0);                 //矩形识别进行矫正
					}
					if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
					{
						speed_target[0] = 0;
						speed_target[1] = 0;
						speed_target[2] = 0;
						speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
						v1 = 0;
						v2 = 0;
						v3 = 0;
						v4 = 0;
					}
					if(state_right==6)
					{
						//if(state_right!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
						if(1)//只给其赋值一次，确保准确性
						{
							shizi_zimu[shizi_zimu_num]=eiq_right;              //因为测试所以被注释掉了
							eiq_one_flag=1;
						}	
						if(shizi_zimu[shizi_zimu_num]==pic_cunchu[pic_cunchu_num-1])
						{
							sevor_right_flag=1;//放照片
							sevor_set();
						}
						else
						{
							coord_aim[0] = coord_aim_pic_x;          //回到原点
							coord_aim[1] = coord_aim_pic_y;	
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								//pic_speed_coord(0,0,0);                  //保持车身正直
								speed_target[0] = 0;              		   //保持车身不动
								speed_target[1] = 0;
								speed_target[2] = 0;
								speed_target[3] = 0;                    
								v1 = 0;
								v2 = 0;
								v3 = 0;
								v4 = 0;		
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //关闭右摄像头
								hzw_flag = 1;
								//默认继续前进					state_right=0
								if(shizi_zimu_num==4)
								{
									left_shizi_flag_flag=1;
								}
								if(shi_zi_pic_num==0)
								{
									left_shizi_flag_flag=2;
								}
							}
							else
							{
								if(hzw_flag == 0)
								{								
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
								}
							}
						}
					}
					if(left_shizi_flag_flag==1&&shizi_zimu_num==4&&state_right==0)
					{
						eiq_one_flag=0;
						left_shizi_flag=2;
						left_shizi_flag_flag=0;
					}
					if(left_shizi_flag_flag==2&&state_right==0)
					{
						eiq_one_flag=0;
						left_shizi_flag=3;
						left_shizi_flag_flag=0;
					}					
				}
				
/*放照片*/		
				if(left_shizi_flag==2)
				{
					left_roate_determine();//左十字确定是否掉头
					if(left_shizi_flag_flag==1)
					{	
						left_shizi_flag_flag=0;
						left_shizi_flag=3;
					}
					
					if(roate_shun_ni_flag==1&&left_roate_determine_flag==1)//反向开车
					{						
						roate_tiaozhen();
						if(shizi_flag_flag==0)
						{
							vector_y = 15;
							speed_coord_xun();		
							xyz=0;//给捡照片权限
							if(angle_shizi_left==0||angle_shizi_left==4)
							{
								if((coord_now[2]+180)<(angle_shizi+nei_qie_angle_shizi_H_E))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}
							else
							{
								if((coord_now[2]+180)<(angle_shizi+nei_qie_angle_shizi))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}								
							}
			
						}							
						if(shizi_flag_flag==1)//左边有字母
						{
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							if(angle_shizi_left==0||angle_shizi_left==4)
							{
								encoder_pic_angle=angle_shizi-180+nei_qie_angle_shizi_H_E;
							}
							else
							{
								encoder_pic_angle=angle_shizi-180+nei_qie_angle_shizi;								
							}
							
							shizi_flag_flag=2;
						}
						
						if(shizi_flag_flag==2)//左边有字母
						{							
							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y+nei_qie_distance_shizi;
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								shizi_flag_flag=3;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
							}
						}

						if(shizi_flag_flag==3)
						{
							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y;
							if(shizi_zimu[angle_shizi_left]==pic_cunchu[(pic_cunchu_num-1)]&&xyz==0)//变量共用了
							{
								sevor_left_flag=1;//放照片
								sevor_set();
							}
							
							else
							{
								xyz=1;
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									pic_speed_coord(0,0,0);                  //保持车身正直
									left_roate_determine_flag=0;
									roate_tiaozhen_flag=0;
									shizi_flag_flag=0;
									if(shi_zi_pic_num==0)
									{
										left_shizi_flag_flag=1;
									}
								}
								else
								{
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
								}
							}
						}
					}
					
					if(roate_shun_ni_flag==0&&left_roate_determine_flag==1)
					{
						roate_tiaozhen();
						if(shizi_flag_flag==0)
						{
							vector_y = 15;
							speed_coord_xun();	
							xyz=0;//给捡照片权限	
							if(angle_shizi_left==0||angle_shizi_left==4)
							{
								if(coord_now[2]>(angle_shizi-nei_qie_angle_shizi_H_E))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}
							else
							{
								if(coord_now[2]>(angle_shizi-nei_qie_angle_shizi))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}								
							}
						}
						if(shizi_flag_flag==1)//左边有字母
						{
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							if(angle_shizi_left==0||angle_shizi_left==4)
							{
								encoder_pic_angle=angle_shizi-nei_qie_angle_shizi_H_E;
							}
							else
							{
								encoder_pic_angle=angle_shizi-nei_qie_angle_shizi;								
							}
							shizi_flag_flag=2;
						}
						
						if(shizi_flag_flag==2)//左边有字母
						{
							
							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y+nei_qie_distance_shizi;
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								shizi_flag_flag=3;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
							}
						}

						if(shizi_flag_flag==3)
						{		

							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y;							
							if(shizi_zimu[angle_shizi_left]==pic_cunchu[(pic_cunchu_num-1)]&&xyz==0)//变量共用了
							{
								sevor_right_flag=1;//放照片
								sevor_set();
							}							
							else
							{
								xyz=1;

								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									pic_speed_coord(0,0,0);                  //保持车身正直
									left_roate_determine_flag=0;
									roate_tiaozhen_flag=0;
									shizi_flag_flag=0;
									if(shi_zi_pic_num==0)
									{
										left_shizi_flag_flag=1;
										
									}
									
								}
								else
								{
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
								}
							}						
						}
						
					}								
				}
/*出十字*/		
				if(left_shizi_flag==3)//出十字过程
				{
						if(roate_shun_ni_flag==0)
						{	
							roate_tiaozhen();
							vector_y = chu_yuansu_sudu;
							speed_coord_xun();
							if((coord_now[2]<=(encoder_pic_angle_shizi+270-30))&&count_flag == 0)
							{
								num_pic=0;
								count_flag = 0;
							}
							if((coord_now[2]>(encoder_pic_angle_shizi+270-30))&&count_flag == 0)
							{
								num_pic=0;
								count_flag = 1;
								
							}
							if(num_pic>chu_yuansu_sudu_time)//走出十字一秒，避免被字母困扰
							{
								cross=0;//走出十字地狱
								shizi_dir_queding_flag++;
								shizi_zimu_num=-1;
								yaw_angle_flag=0;
								coord_aim_pic_x_shizi=0;//进环岛 初始位置
								coord_aim_pic_y_shizi=0;
								count_flag=0;
								cross_flag=0;
								left_shizi_flag=0;//左转十字标志位置零
								
								shizi_wanqian_flag=0;
								
								roate_tiaozhen_flag=0;
								
								num_kong_flag=0;//清楚没右照片标志位
								
							}
						}
						else
						{
							roate_ni();
							if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
							{
								roate_determine_angle_flag=0;
								roate_shun_ni_flag=0;
							}
							//掉转走出十字
						}
				}
			}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右十字
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右十字
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////右十字
			else if(cross_flag==2&&cross_flag_dir==2)
			{
				if(coord_aim_pic_x_shizi==0&&yaw_angle_flag==1)//刚进十字时候的位置，后续清零
				{
					coord_aim_pic_x_shizi = coord_now[0];
					coord_aim_pic_y_shizi = coord_now[1];
					encoder_pic_angle_shizi=coord_now[2]; //记录初始角度
					encoder_pic_angle=encoder_pic_angle_shizi;	
					chakanshuzuneirong_flag	=pic_cunchu_num;	

					
				}
				
				if(right_shizi_flag==0)
				{
					eiq_mode_left=1;
					eiq_mode_right=1;
/*捡照片*/			if(shizi_wanqian_flag==0)
					{
						if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
						{
							shizi_wanqian_flag=1;
						}
						else
						{
							coord_aim[0] = coord_aim_pic_x_shizi+0.4;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+shi_zi_qianjing_distance;	 
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}
					}
					if(shizi_wanqian_flag==1)
					{		
							
						if(state_right==0)
						{
							
							camera_determine_left=1;               //关闭左摄像头
							camera_determine_right=0;              //打开右摄像头
							//vector_y = 0;
							//speed_coord_xun();
							stop();	
							if(num_kong_flag==0)
							{
								num_kong=0;
								num_kong_flag=1;
							}	

						}
						if(state_right==1)
						{
							///vector_y = 0;
							//speed_coord_xun();
							stop();	
						}
						if(state_right==2)//右边有照片
						{
							//jianzhaopian_flag = 1;                 //暂停环岛进程
							stop();		                           //停车		

							//encoder_pic_angle=coord_now[2];				  
							encoder_pic_angle=encoder_pic_angle_shizi;	//回到判断到十字的位置
							//coord_now[2]=0;                     //motor.c  //不确定是否改成功,且位置不对
						}
						///////////////////////////////////////////////////////后期考虑将前面删除	
						if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
						{    
							eiq_one_flag=0;								
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							//camera_determine_left=1;               //关闭左摄像头
							//camera_determine_right=0;              //打开右摄像头
							uart_write_byte(UART_4, 1);
							pic_speed_coord(0,0,0);
						}
						else//判断是否有照片（环岛或十字）
						{
							if ((num_kong > 2000)&&num_kong_flag==1) 
							{ // 如果计数器超过2000次
								left_shizi_flag_flag = 2; // 设置左环岛标志位为2
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
							}
							
							if(state_right>=3)
							{
								num_kong_flag=2;
							}
						}
						if(state_right==4)//确认有照片，并使用矩形识别直接捡照片，关闭左摄像头	 （关闭摄像头是指n=0且flag_exchange==0）
						{
							speed_coord_color_right(0,0);                 //矩形识别进行矫正
							sevor_right_flag=0;
						}
						if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
						{
							speed_target[0] = 0;
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						}
						
						if(state_right==6)                           //eiq深度学习
						{
							//if(eiq_one_flag == 0)//只给其赋值一次，确保准确性
							sevor_set();	
							if(1)//只给其赋值一次，确保准确性
							{
								yi_yiduiying();
								pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
								eiq_one_flag=1;
							}	
							//对eiq深度学习数据进行处理
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;
						 //舵机捡照片
							uart_write_byte(UART_4, 2);	             //判断是否还有照片    
						}
						
						if(state_right==7)                           //矩形识别发现符合要求矩形 (深度学习)
						{
							eiq_one_flag=0;	
							//pic_speed_coord(0,0,0);                  //保持车身正直    貌似效果不佳
							sevor_right_flag = 0;                    //设置标志位为零，进6时候再次捡照片
							speed_target[0] = 0;                    //车身静止不动
							speed_target[1] = 0;
							speed_target[2] = 0;
							speed_target[3] = 0;  
							v1 = 0;
							v2 = 0;
							v3 = 0;
							v4 = 0;					
							if(myabs(x_pic_right)<=7&&myabs(y_pic_right)<=7)
							{
								uart_write_byte(UART_4, 4);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
							
							else							
							{
								uart_write_byte(UART_4, 1);            //可注释，openart识别矩形时以及将flag_exchange置4							
							}
						}
						if(state_right==8)                           //矩形识别没发现矩形，回原点
						{
							eiq_one_flag=0;	
							coord_aim[0] = coord_aim_pic_x_shizi-0.15;          //回到原点
							coord_aim[1] = coord_aim_pic_y_shizi+shi_zi_qianjing_distance-0.4;	         
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								pic_speed_coord(0,0,0);                  //保持车身正直
								//uart_write_byte(UART_1, 4);              //告诉摄像头进程已完成，清零所有标志位，并将state_left置0
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //打开右摄像头
								left_shizi_flag_flag=1;                //这个变量可以共用（左右）
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
							}
						}
						if(left_shizi_flag_flag==1&&open_close_left==1&&open_close_right==1)//表示摄像头已经开启成功
						{
							left_shizi_flag_flag=0;
							right_shizi_flag=2;
						}
						if(left_shizi_flag_flag==2&&open_close_left==1&&open_close_right==1)
						{
							right_shizi_flag=3;
							left_shizi_flag_flag=0;
						}						
					}
				}
/*匹配字母*/		if(right_shizi_flag==1)
				{
					eiq_mode_left=2;
					eiq_mode_right=2;
					if(state_left==0)
					{
						vector_y = 8;
						speed_coord_xun();
						hzw_flag=0;
						roate_tiaozhen();
						if((coord_now[2]<(encoder_pic_angle_shizi+right_zhizi_0+15))&&shizi_zimu_num==-1)
						{
							//开启摄像头，靠近第一张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=0;               //打开左摄像头
							camera_determine_right=1;              //关闭右摄像头
							shizi_zimu_num=0;
						}
						if((coord_now[2]<(encoder_pic_angle_shizi+right_zhizi_1+15))&&shizi_zimu_num == 0)
						{
							//开启摄像头，靠近第二张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=0;               //打开左摄像头
							camera_determine_right=1;              //关闭右摄像头
							shizi_zimu_num=1;
						}
						if((coord_now[2]<(encoder_pic_angle_shizi+right_zhizi_2+15))&&shizi_zimu_num == 1)
						{
							//开启摄像头，靠近第三张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=0;               //打开左摄像头
							camera_determine_right=1;              //关闭右摄像头
							shizi_zimu_num=2;
						}
						if((coord_now[2]<(encoder_pic_angle_shizi+right_zhizi_3+15))&&shizi_zimu_num == 2)
						{
							//开启摄像头，靠近第四张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=0;               //打开左摄像头
							camera_determine_right=1;              //关闭右摄像头
							shizi_zimu_num=3;
						}
						if((coord_now[2]<(encoder_pic_angle_shizi+right_zhizi_4+15))&&shizi_zimu_num == 3)
						{
							//开启摄像头，靠近第五张对应字母，匹配完关闭，如果字母与照片相符，放照片
							camera_determine_left=0;               //打开左摄像头
							camera_determine_right=1;              //关闭右摄像头
							shizi_zimu_num=4;
						}					
					}	
					if(state_left==1)
					{
						vector_y = 5;
						speed_coord_xun();
					}
					if(state_left==2)
					{
						//jianzhaopian_flag = 1;                 //暂停环岛进程
						stop();		                           //停车		
						coord_aim_pic_x = coord_now[0];        //记录初始位置
						coord_aim_pic_y = coord_now[1];
						//encoder_pic_angle=coord_now[2]; 
						if(shizi_zimu_num==0)
						{
							encoder_pic_angle=right_zhizi_0;
						}
						if(shizi_zimu_num==1)
						{
							encoder_pic_angle=right_zhizi_1;
						}
						if(shizi_zimu_num==2)
						{
							encoder_pic_angle=right_zhizi_2;
						}
						if(shizi_zimu_num==3)
						{
							encoder_pic_angle=right_zhizi_3;
						}
						if(shizi_zimu_num==4)
						{
							encoder_pic_angle=right_zhizi_4;
						}				
					}
					if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
					{     					
						coord_aim[0] = coord_aim_pic_x-0.3;          //回到原点
						coord_aim[1] = coord_aim_pic_y;	
						if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
						{						
							uart_write_byte(UART_1, 1);
						}
						else
						{				
							speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
						}
					}
					if(state_left==4)//确认有字母
					{
						speed_coord_color(0,0);                 //矩形识别进行矫正
					}
					if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
					{
						speed_target[0] = 0;
						speed_target[1] = 0;
						speed_target[2] = 0;
						speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
						v1 = 0;
						v2 = 0;
						v3 = 0;
						v4 = 0;
					}
					if(state_left==6)
					{
						if(1)//只给其赋值一次，确保准确性
						{
							shizi_zimu[shizi_zimu_num]=eiq_left;              //因为测试所以被注释掉了
							eiq_one_flag=1;
						}							
						if(shizi_zimu[shizi_zimu_num]==pic_cunchu[pic_cunchu_num-1])
						{
							sevor_left_flag=1;//放照片
							sevor_set();
						}
						else
						{
							
							coord_aim[0] = coord_aim_pic_x;          //回到原点
							coord_aim[1] = coord_aim_pic_y;	
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								//pic_speed_coord(0,0,0);                  //保持车身正直
								speed_target[0] = 0;              		   //保持车身不动
								speed_target[1] = 0;
								speed_target[2] = 0;
								speed_target[3] = 0;                    
								v1 = 0;
								v2 = 0;
								v3 = 0;
								v4 = 0;		
								roate_tiaozhen_flag=0;
								camera_determine_left=1;               //关闭左摄像头
								camera_determine_right=1;              //关闭右摄像头
								hzw_flag = 1;
								//默认继续前进					state_right=0
								if(shizi_zimu_num==4)
								{
									left_shizi_flag_flag=1;
								}
								if(shi_zi_pic_num==0)
								{
									left_shizi_flag_flag=2;
								}
							}
							else
							{
								if(hzw_flag == 0)
								{								
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
								}
							}
						}
					}
					if(left_shizi_flag_flag==1&&shizi_zimu_num==4&&state_left==0)
					{		
						right_shizi_flag=2;
						left_shizi_flag_flag=0;//可以与左十字共用
					}	
					
					if(left_shizi_flag_flag==2&&state_left==0)
					{	
						right_shizi_flag=3;
						left_shizi_flag_flag=0;
					}	
				}
/*放照片*/
				if(right_shizi_flag==2)
				{	
					right_roate_determine();//左十字确定是否掉头	
					if(left_shizi_flag_flag==1)
					{	
						left_shizi_flag_flag=0;
						right_shizi_flag=3;
					}
					
					if(roate_shun_ni_flag==1&&left_roate_determine_flag==1)//反向开车
					{
						
						roate_tiaozhen();
						if(shizi_flag_flag==0)
						{
							vector_y = 15;
							speed_coord_xun();		
							xyz=0;//给捡照片权限
							if(angle_shizi_right==0||angle_shizi_right==4)
							{
								if((coord_now[2]+180)>(angle_shizi-nei_qie_angle_shizi_H_E))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}
							else	
							{
								if((coord_now[2]+180)>(angle_shizi-nei_qie_angle_shizi))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}								
						}
						
						if(shizi_flag_flag==1)//左边有字母
						{
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							if(angle_shizi_right==0||angle_shizi_right==4)
							{
								encoder_pic_angle=angle_shizi-180-nei_qie_angle_shizi_H_E;
							}
							else	
							{
								encoder_pic_angle=angle_shizi-180-nei_qie_angle_shizi;
							}	
							shizi_flag_flag=2;
						}
						if(shizi_flag_flag==2)//左边有字母
						{
							
							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y+nei_qie_distance_shizi;
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								shizi_flag_flag=3;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
							}
						}

						if(shizi_flag_flag==3)
						{
							if(shizi_zimu[angle_shizi_right]==pic_cunchu[(pic_cunchu_num-1)]&&xyz==0)//变量共用了
							{
								sevor_right_flag=1;//放照片
								sevor_set();
							}
							
							else
							{
								xyz=1;
								coord_aim[0]=coord_aim_pic_x;        //记录初始位置
								coord_aim[1]=coord_aim_pic_y;
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									pic_speed_coord(0,0,0);                  //保持车身正直
									left_roate_determine_flag=0;
									roate_tiaozhen_flag=0;
									shizi_flag_flag=0;
									if(shi_zi_pic_num==0)
									{
										left_shizi_flag_flag=1;
									}
								}
								else
								{
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
								}
							}
						}
					}	
					
					if(roate_shun_ni_flag==0&&left_roate_determine_flag==1)//正向开车
					{
						roate_tiaozhen();
						if(shizi_flag_flag==0)
						{
							vector_y = 15;
							speed_coord_xun();	
							xyz=0;//给捡照片权限		
							if(angle_shizi_right==0||angle_shizi_right==4)
							{
								if(coord_now[2]<(angle_shizi+nei_qie_angle_shizi_H_E))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}
							else	
							{
								if(coord_now[2]<(angle_shizi+nei_qie_angle_shizi))//+20是提前
								{
									shizi_flag_flag=1;//停车直接放照片
									stop();	
								}	
							}							
		
						}
						if(shizi_flag_flag==1)//左边有字母
						{
							stop();		                           //停车		
							coord_aim_pic_x = coord_now[0];        //记录初始位置
							coord_aim_pic_y = coord_now[1];
							if(angle_shizi_right==0||angle_shizi_right==4)
							{
								encoder_pic_angle=angle_shizi+nei_qie_angle_shizi_H_E;
							}
							else	
							{
								encoder_pic_angle=angle_shizi+nei_qie_angle_shizi;
							}	
							
							shizi_flag_flag=2;
						}
                        if(shizi_flag_flag==2)//左边有字母
						{
							
							coord_aim[0]=coord_aim_pic_x;        //记录初始位置
							coord_aim[1]=coord_aim_pic_y+nei_qie_distance_shizi;
							if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
							{
								shizi_flag_flag=3;
							}
							else
							{
								speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
							}
						}

						if(shizi_flag_flag==3)
						{
							if(shizi_zimu[angle_shizi_right]==pic_cunchu[(pic_cunchu_num-1)]&&xyz==0)//变量共用了
							{
								sevor_left_flag=1;//放照片
								sevor_set();
							}
							
							else
							{
								xyz=1;
								coord_aim[0]=coord_aim_pic_x;        //记录初始位置
								coord_aim[1]=coord_aim_pic_y;
								if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
								{
									pic_speed_coord(0,0,0);                  //保持车身正直
									left_roate_determine_flag=0;
									roate_tiaozhen_flag=0;
									shizi_flag_flag=0;
									if(shi_zi_pic_num==0)
									{
										left_shizi_flag_flag=1;
									}
								}
								else
								{
									speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
	
								}
							}
						}
						
					
					}	

					
				}
				
				
/*出十字*/		if(right_shizi_flag==3)//出十字过程
				{
					if(roate_shun_ni_flag==0)
					{
						
						roate_tiaozhen();
						
						vector_y = chu_yuansu_sudu;
						speed_coord_xun();
						if((coord_now[2]>=(encoder_pic_angle_shizi-270+30))&&count_flag == 0)
						{
							num_pic=0;
							count_flag = 0;//可以与左环岛共用
						}
						if((coord_now[2]<(encoder_pic_angle_shizi-270+30))&&count_flag == 0)
						{
							num_pic=0;

							count_flag = 1;
						}
						if(num_pic>chu_yuansu_sudu_time)//走出十字一秒，避免被字母困扰
						{
							cross=0;//走出十字地狱
							shizi_dir_queding_flag++;
							shizi_zimu_num=-1;
							yaw_angle_flag=0;
							coord_aim_pic_x_shizi=0;//进环岛 初始位置
							coord_aim_pic_y_shizi=0;
							count_flag=0;
							cross_flag=0;           //十字标志位
							right_shizi_flag=0;//左转十字标志位置零
							
							num_kong_flag=0;//清楚没右照片标志位
							
							shizi_wanqian_flag=0;
							
							roate_tiaozhen_flag=0;
							
							
						}
					}
					else
					{
						roate_ni();
						if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
						{
							roate_determine_angle_flag=0;
							roate_shun_ni_flag=0;
						}
						//掉转走出十字
					}
				}
			}
		}
/////////////////////////////////////////////////////////////////////////////////////////////散落照片		
/////////////////////////////////////////////////////////////////////////////////////////////散落照片		
/////////////////////////////////////////////////////////////////////////////////////////////散落照片		
		else//是直道捡散落照片
		{
			eiq_mode_left=0;
			eiq_mode_right=0;
			if(state_left==0&&state_right==0)
			{
				
				vector_y = 20;
				sanluo_picing=0;				
				speed_coord_xun();				
			}
			if(state_left==1||state_right==1)
			{
				vector_y = 5;
				sanluo_picing=1;
				speed_coord_xun();
			}				
/*左散落*/	if(state_left==2)//左边有照片
			{
				
				roate_tiaozhen();
				
				
				sanluo_picing=1;				
				stop();		                           //停车		
				coord_aim_pic_x = coord_now[0];        //记录初始位置
				coord_aim_pic_y = coord_now[1];
				encoder_pic_angle=coord_now[2]; 	
			}
			if(state_left==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
			{      
				eiq_one_flag=0;
				camera_determine_left=0;               //开关摄像头
				camera_determine_right=1;
				coord_aim_pic_x = coord_now[0];        //记录初始位置
				coord_aim_pic_y = coord_now[1];				
				uart_write_byte(UART_1, 1);
				sevor_left_flag = 0;   //捡照片
			}
			if(state_left==4)                           //保持车身静止，方便进行eiq深度学习并捡照片
			{
				speed_coord_color(0,0);                 //矩形识别进行矫正
				
				//sevor_right_flag =2;	//捡照片	
			}		
			if(state_left==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
			{
				speed_target[0] = 0;
				speed_target[1] = 0;
				speed_target[2] = 0;
				speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	  可能要放中断里
				v1 = 0;
				v2 = 0;
				v3 = 0;
				v4 = 0;
			}
			if(state_left==6)//条件突然不满足了                           //eiq深度学习
			{
				//if(state_left!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
				sevor_set();
				if(1)//只给其赋值一次，确保准确性
				{
					yi_yiduiying();
					pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
					eiq_one_flag=1;
				}	
				//对eiq深度学习数据进行处理
				speed_target[0] = 0;                    //车身静止不动
				speed_target[1] = 0;
				speed_target[2] = 0;
				speed_target[3] = 0;  
				v1 = 0;
				v2 = 0;
				v3 = 0;
				v4 = 0;
											 //舵机捡照片
				
				coord_aim[0] = coord_aim_pic_x;          //
				coord_aim[1] = coord_aim_pic_y;	         //回到原点
			
				if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
				{
					pic_speed_coord(0,0,0);                  //保持车身正直
					roate_tiaozhen_flag=0;
					
					camera_determine_left=0;               //开关摄像头
					camera_determine_right=0;
					
					
					uart_write_byte(UART_1, 8);            //强制清楚数据
					
					sanluo_picing=0;
				}
				else
				{
					speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
				}  
			}								
/*右散落*/	if(state_right==2)//右边有照片
			{
				roate_tiaozhen();
				sanluo_picing=1;	
				stop();		                           //停车		
				coord_aim_pic_x = coord_now[0];        //记录初始位置
				coord_aim_pic_y = coord_now[1];
				encoder_pic_angle=coord_now[2]; 	
			}
			if(state_right==3)                           //确认有照片，并使用矩形识别直接捡照片，关闭右边摄像头    （关闭摄像头是指n=0且flag_exchange==0）
			{      
				eiq_one_flag=0;				
				camera_determine_left=1;               //开关摄像头
				camera_determine_right=0;
				uart_write_byte(UART_4, 1);
				sevor_right_flag =0;	//捡照片
				coord_aim_pic_x = coord_now[0];        //记录初始位置
				coord_aim_pic_y = coord_now[1];
			}
			if(state_right==4)                           //保持车身静止，方便进行eiq深度学习并捡照片
			{				
				speed_coord_color_right(0,0);                 //矩形识别进行矫正
				//sevor_left_flag = 2;   //捡照片
					
			}		
			if(state_right==5)                           //保持车身静止，方便进行eiq深度学习并捡照片
			{
				speed_target[0] = 0;
				speed_target[1] = 0;
				speed_target[2] = 0;
				speed_target[3] = 0;                    //由于车身移动完毕会抖动一下，试一试	
				v1 = 0;
				v2 = 0;
				v3 = 0;
				v4 = 0;
			}
			if(state_right==6)                           //eiq深度学习
			{
				//if(state_right!=0&&eiq_one_flag == 0)//只给其赋值一次，确保准确性
				sevor_set();
				if(1)//只给其赋值一次，确保准确性
				{
					yi_yiduiying();
					pic_cunchu[pic_cunchu_num-1]=eiq_chuli;              //因为测试所以被注释掉了
					eiq_one_flag=1;
				}	
				//对eiq深
				//对eiq深度学习数据进行处理
				speed_target[0] = 0;                    //车身静止不动
				speed_target[1] = 0;
				speed_target[2] = 0;
				speed_target[3] = 0;  
				v1 = 0;
				v2 = 0;
				v3 = 0;
				v4 = 0;
											 //舵机捡照片
  				if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
				{
					roate_tiaozhen_flag=0;
					pic_speed_coord(0,0,0);                  //保持车身正直
					
					
					camera_determine_left=0;               //开关摄像头
					camera_determine_right=0;
					
					
					uart_write_byte(UART_4, 8);              //强制清除数据
					
					sanluo_picing=0;
				}
				else
				{
					coord_aim[0] = coord_aim_pic_x;          //
					coord_aim[1] = coord_aim_pic_y;	         //回到原点
					speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
				}
			}
		}
		
///还有一种情况，是停车放照片的时候
		
		
		
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////弯道判断（为了提前识别环岛）
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////弯道判断（为了提前识别环岛）
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////弯道判断（为了提前识别环岛）
		
		
		
		//感觉会和散落照片冲突
		if(sanluo_picing==0&&cross_flag==0&&ImageFlag.image_element_rings==0&&ImageFlag.Zebra_Flag == 0)
		{
			if(ImageStatus.Miss_Left_lines>10&&ImageStatus.Miss_Right_lines<5&&Miss_Left_lines_flag==0)
			{
				ImageStatus_num=0;
				Miss_Left_lines_flag=1;
			}
			if(ImageStatus.Miss_Right_lines>10&&ImageStatus.Miss_Left_lines<5&&Miss_Right_lines_flag==0)
			{
				ImageStatus_num=0;
				Miss_Right_lines_flag=1;
			}	
			
			
			if(ImageStatus_num<500)
			{
				if(Miss_Left_lines_flag==1)
				{
					camera_determine_left=1;
					camera_determine_right=0;
				}
				
				if(Miss_Right_lines_flag==1)
				{
					camera_determine_left=0;
					camera_determine_right=1;
				}		
				
			}
			else
			{
				
				
				if(Miss_Left_lines_flag==0&&Miss_Right_lines_flag==0)
				{
					camera_determine_left=0;
					camera_determine_right=0;
				}
				
				
				Miss_Left_lines_flag=0;
				Miss_Right_lines_flag=0;                   
			}
		}





////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////车库放照片
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////车库放照片		
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////车库放照片		
		/*planA：深度学习数字，才能够后往前放并且记录当前数字对应的位置，然后一直放直到放完*/
		/*planB：先识别到斑马线再回退固定数值的距离，然后一边矫正位置一边修正那个固定数值，然后等到深度学习完之后放照片，之后就根据固定数值进行前进后退放照片*/
		/*
		if(深度学习结果是数字)
		{
			cheku_flag = 1;
		}
		if(cheku_flag == 1)
		{
			angle_banmaxian = coord_now[2];
		}
		*/
		camera_determine(camera_determine_left,camera_determine_right);//我的想法是不调整好摄像头，车不动			
		eiq_mode_determine(eiq_mode_left , eiq_mode_right);			
}
/////////////
/*左十字确认*/
/////////////
void left_roate_determine()
{
	if(left_roate_determine_flag==0)
	{
		if(shizi_zimu[0]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_0;
			angle_shizi_left=0;
		}
		
		else if(shizi_zimu[1]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_1;
			angle_shizi_left=1;
		}
		
		else if(shizi_zimu[2]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_2;
			angle_shizi_left=2;
		}
		
		else if(shizi_zimu[3]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_3;
			angle_shizi_left=3;
		}		
		else if(shizi_zimu[4]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_4;
			angle_shizi_left=4;
		}
		else//后续确认是否需要更改，应为较不准
		{
			angle_shizi=encoder_pic_angle_shizi+left_zhizi_3;
			angle_shizi_left=3;
			pic_cunchu[pic_cunchu_num-1]=shizi_zimu[3];
		}


		
		if(roate_shun_ni_flag==0)
		{
			
			static int h=0;
			if((coord_now[2]>angle_shizi)&&h==0)
			{
				h=1;
			}
			if((coord_now[2]<angle_shizi)&&h==0)
			{
				h=2;
			}
			if(h==1&&camera_determine_flag==1)//如果此时车头向前				
			{
				//调转车头，循迹置想要度数，开启摄像头捡照片		
				roate_shun();
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag=0;//旋转初始角度标志位
					roate_shun_ni_flag=1;
					left_roate_determine_flag=1;
					h=0;
				}
			}
			else if(h==2&&camera_determine_flag==1)//如果此时车头向后
			{
				//循迹置想要度数，开启摄像头捡照片
				//roate_shun();
				left_roate_determine_flag=1;
				roate_shun_ni_flag=0;
				h=0;
			}	
		}
		if(roate_shun_ni_flag==1)
		{
			static int k;
			if(((coord_now[2]+180)>angle_shizi)&&k==0)
			{
				k=1;
			}
			if(((coord_now[2]+180)<angle_shizi)&&k==0)
			{
				k=2;
			}
			if(k==1&&camera_determine_flag==1)//如果此时车头向后
			{
				//循迹置想要度数，开启摄像头捡照片		
				//roate_ni();
				left_roate_determine_flag=1;
				roate_shun_ni_flag=1;
				k=0;
			}
			if(k==2&&camera_determine_flag==1)//如果此时车头向前
			{
				//调转车头，循迹置想要度数，开启摄像头捡照片
				roate_ni();
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag=0;//旋转初始角度标志位
					roate_shun_ni_flag=0;
					left_roate_determine_flag=1;
					k=0;
				}
				
			}
		}
	}
}	
/////////////
/*右十字确认*/
/////////////
void right_roate_determine()
{
	if(left_roate_determine_flag==0)//这个变量可以共用
	{
		if(shizi_zimu[0]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_0;
			angle_shizi_right=0;
		}
		
		else if(shizi_zimu[1]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_1;
			angle_shizi_right=1;
		}
		
		else if(shizi_zimu[2]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_2;
			angle_shizi_right=2;
		}
		
		else if(shizi_zimu[3]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_3;
			angle_shizi_right=3;
		}
		
		else if(shizi_zimu[4]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_4;
			angle_shizi_right=4;
		}
		else
		{
			angle_shizi=encoder_pic_angle_shizi+right_zhizi_3;
			angle_shizi_right=3;
			pic_cunchu[pic_cunchu_num-1]=shizi_zimu[3];
		}

		
		if(roate_shun_ni_flag==0)
		{
			
			static int h=0;
			if((coord_now[2]<angle_shizi)&&h==0)
			{
				h=1;
			}
			if((coord_now[2]>angle_shizi)&&h==0)
			{
				h=2;
			}
			if(h==1&&camera_determine_flag==1)//如果此时车头向前				
			{
				//调转车头，循迹置想要度数，开启摄像头捡照片		
				roate_shun();
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag=0;//旋转初始角度标志位
					roate_shun_ni_flag=1;
					left_roate_determine_flag=1;
					h=0;
				}
			}
			else if(h==2&&camera_determine_flag==1)//如果此时车头向后
			{
				//循迹置想要度数，开启摄像头捡照片
				//roate_shun();
				left_roate_determine_flag=1;
				roate_shun_ni_flag=0;
				h=0;
			}	
		}
		if(roate_shun_ni_flag==1)
		{
			static int k;
			if(((coord_now[2]+180)<angle_shizi)&&k==0)
			{
				k=1;
			}
			if(((coord_now[2]+180)>angle_shizi)&&k==0)
			{
				k=2;
			}
			if(k==1&&camera_determine_flag==1)//如果此时车头向后
			{
				//循迹置想要度数，开启摄像头捡照片		
				//roate_ni();
				left_roate_determine_flag=1;
				roate_shun_ni_flag=1;
				k=0;
			}
			else if(k==2&&camera_determine_flag==1)//如果此时车头向前
			{
				//调转车头，循迹置想要度数，开启摄像头捡照片
				roate_ni();
				if(myabs_double(roate_determine_angle-coord_now[2])<2.000)	
				{
					roate_determine_angle_flag=0;//旋转初始角度标志位
					roate_shun_ni_flag=0;
					left_roate_determine_flag=1;
					k=0;
				}
			}
		}
	}
}		
/////////////
/*左环岛确认*/
/////////////
void left_huandao_determine()//左环岛是否绕圈
{
	if(raoquan_determine_flag==0)//这个变量可以共用
	{
		

		if(huandao_zimu[0]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_0;
			angle_huandao_flag=0;
		}
		
		else if(huandao_zimu[1]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_1;
			angle_huandao_flag=1;
		}
		
		else if(huandao_zimu[2]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_2;
			angle_huandao_flag=2;
		}
		
		else if(huandao_zimu[3]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_3;
			angle_huandao_flag=3;
		}
		
		else if(huandao_zimu[4]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_4;
			angle_huandao_flag=4;
		}	
        else
		{
			angle_huandao=encoder_pic_angle_huandao+left_huandao_3;
			angle_huandao_flag=3;
			pic_cunchu[pic_cunchu_num-1]=huandao_zimu[3];
		}

		
		if(coord_now[2]<angle_huandao)
		{
			raoquan_determine_flag=1;
		}
		
		if(coord_now[2]>=angle_huandao)
		{
			coord_now_flag=1;
		}
		if(coord_now_flag==2)
		{
			raoquan_determine_flag=1;
		}
	}		
}
/////////////
/*右环岛确认*/
/////////////
void right_huandao_determine()//右环岛是否绕圈
{
	if(raoquan_determine_flag==0)//这个变量可以共用
	{
		

		if(huandao_zimu[0]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_0;
			angle_huandao_flag=0;
		}
		
		else if(huandao_zimu[1]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_1;
			angle_huandao_flag=1;
		}
		
		else if(huandao_zimu[2]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_2;
			angle_huandao_flag=2;
		}
		
		else if(huandao_zimu[3]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_3;
			angle_huandao_flag=3;
		}
		
		else if(huandao_zimu[4]==pic_cunchu[pic_cunchu_num-1])
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_4;
			angle_huandao_flag=4;
		}	
        else
		{
			angle_huandao=encoder_pic_angle_huandao+right_huandao_3;
			angle_huandao_flag=3;
			pic_cunchu[pic_cunchu_num-1]=huandao_zimu[3];
		}

		
		if(coord_now[2]>angle_huandao)
		{
			raoquan_determine_flag=1;
		}
		
		if(coord_now[2]<=angle_huandao)
		{
			coord_now_flag_right=1;
		}
		if(coord_now_flag_right==2)
		{
			raoquan_determine_flag=1;
		}
	}		
}

void Zebra_determine()
{
//在这里决定好下一个位置，并移动到下一个位置	
	
		if(shuzi_pic[0]==pic_cunchu[pic_cunchu_num-1]&&Zebra_determine_flag==0)
		{
			Zebra_position=0;
			Zebra_determine_flag=1;
		}		
		else if(shuzi_pic[1]==pic_cunchu[pic_cunchu_num-1]&&Zebra_determine_flag==0)
		{
			Zebra_position=1;
			Zebra_determine_flag=1;
		}		
		else if(shuzi_pic[2]==pic_cunchu[pic_cunchu_num-1]&&Zebra_determine_flag==0)
		{
			Zebra_position=2;
			Zebra_determine_flag=1;
		}	
		
		
		if(Zebra_determine_flag==1)
		{
			
			if(Zebra_position==0)
			{
				coord_aim[0]=coord_aim_pic_x_shizi_zebra_1;        //记录初始位置
				coord_aim[1]=coord_aim_pic_y_shizi_zebra_1;
				if(coord_aim_pic_x_shizi_zebra_1>coord_aim_pic_x_shizi)
				{
					error_jisuan=1;
				}
				else
				{
					error_jisuan=0;
				}
			
			}
			if(Zebra_position==1)
			{
				coord_aim[0]=coord_aim_pic_x_shizi_zebra_2;    //记录初始位置
				coord_aim[1]=coord_aim_pic_y_shizi_zebra_2;
				if(coord_aim_pic_x_shizi_zebra_2>coord_aim_pic_x_shizi)
				{
					error_jisuan=1;
				}
				else
				{
					error_jisuan=0;
				}
			}
			if(Zebra_position==2)
			{
				coord_aim[0]=coord_aim_pic_x_shizi_zebra_3;    //记录初始位置
				coord_aim[1]=coord_aim_pic_y_shizi_zebra_3;
				if(coord_aim_pic_x_shizi_zebra_3>coord_aim_pic_x_shizi)
				{
					error_jisuan=1;
				}
				else
				{
					error_jisuan=0;
				}
			}
			
			if(myabs_double(coord_aim[0]-coord_now[0])<0.1&&myabs_double(coord_aim[1]-coord_now[1])<0.1)
			{
				pic_speed_coord(0,0,0);                  //保持车身正直
				Zebra_determine_flag=2;

			}
			else
			{

				
				speed_coord(coord_now,coord_aim);	     //惯性导航回到原来位置
			}

			
		}
}
////////////
/*捡放照片*/
////////////
void sevor_set()
{
    if ((sevor_left_flag == 0) || (sevor_left_flag == 1)||(sevor_right_flag == 0) || (sevor_right_flag == 1))
    {
		speed_target[0] = 0;//使车辆保持静止
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		v1=0;
		v2=0;
		v3=0;
		v4=0;
        //捡照片（左）
        if(sevor_left_flag==0) 
		{		
			speed_pid1[0] = 0;
			speed_pid2[0] = 0;
			speed_pid3[0] = 0;
			speed_pid4[0] = 0;
			speed_pid1[2] = 20;
			speed_pid2[2] = 20;
			speed_pid3[2] = 20;
			speed_pid4[2] = 20;		
			//delay_zhongduan(500);//中断延迟500ms，等待eiq被接受			
			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm1, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3, step_size, move_delay, servos);		
			delay_zhongduan(50);
			gpio_set_level(D4, 1);
			moveToPosition(current_pwm, target_pwm, step_size,  move_delay, servos);
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2, step_size, move_delay, servos);
			delay_zhongduan(50);		
			moveToPosition(current_pwm, middle_pwm1, step_size, move_delay, servos);		
			delay_zhongduan(50);
			gpio_set_level(D4, 0);
//			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);
//			delay_zhongduan(50);
			
			//照片存储
			if(cross_flag==2)
			{
				shi_zi_pic_num++;
			}
			if(ImageFlag.image_element_rings!=0)
			{
				huandao_pic_num++;
			}
			pic_cunchu_num++;
		}
        if(sevor_right_flag==0) 
		{		
			//delay_zhongduan(500);//中断延迟500ms，等待eiq被接受
			speed_pid1[0] = 0;
			speed_pid2[0] = 0;
			speed_pid3[0] = 0;
			speed_pid4[0] = 0;
			speed_pid1[2] = 20;
			speed_pid2[2] = 20;
			speed_pid3[2] = 20;
			speed_pid4[2] = 20;				
			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm1_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			gpio_set_level(D4, 1);
			moveToPosition(current_pwm, target_pwm_right, step_size,  move_delay, servos);
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2_right, step_size, move_delay, servos);
			delay_zhongduan(50);		
			moveToPosition(current_pwm, middle_pwm1_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			gpio_set_level(D4, 0);
//			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);
//			delay_zhongduan(50);
			
			//照片存储
			if(cross_flag==2)
			{
				shi_zi_pic_num++;
			}
			if(ImageFlag.image_element_rings!=0)
			{
				huandao_pic_num++;
			}
			pic_cunchu_num++;
		}
        if(sevor_left_flag==1)	
		{			
		//放照片	（左）	
			speed_pid1[0] = 0;
			speed_pid2[0] = 0;
			speed_pid3[0] = 0;
			speed_pid4[0] = 0;
			speed_pid1[2] = 20;
			speed_pid2[2] = 20;
			speed_pid3[2] = 20;
			speed_pid4[2] = 20;	
			moveToPosition(current_pwm, middle_pwm1, step_size, move_delay, servos);		
			delay_zhongduan(50);
			gpio_set_level(D4, 1);			
			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm1, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, target_pwm, step_size,  move_delay, servos);
			delay_zhongduan(50);	
			gpio_set_level(D4, 0);
			moveToPosition(current_pwm, middle_pwm3, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2, step_size, move_delay, servos);
			delay_zhongduan(50);	
			moveToPosition(current_pwm, middle_pwm1, step_size, move_delay, servos);
			delay_zhongduan(50);			
//			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
//			delay_zhongduan(50);			
			//if(cross!=0)
			if(cross_flag==2)
			{
				shi_zi_pic_num--;
			}
			if(ImageFlag.image_element_rings!=0)
			{
				huandao_pic_num--;
			}
			pic_cunchu_num--;
		}
        if(sevor_right_flag==1)	
		{			
			//放照片	（右）	
			speed_pid1[0] = 0;
			speed_pid2[0] = 0;
			speed_pid3[0] = 0;
			speed_pid4[0] = 0;
			speed_pid1[2] = 20;
			speed_pid2[2] = 20;
			speed_pid3[2] = 20;
			speed_pid4[2] = 20;	
			moveToPosition(current_pwm, middle_pwm1_right, step_size, move_delay, servos);		
			delay_zhongduan(50);			
			gpio_set_level(D4, 1);		
			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm1_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm3_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, target_pwm_right, step_size,  move_delay, servos);
			delay_zhongduan(50);	
			gpio_set_level(D4, 0);
			moveToPosition(current_pwm, middle_pwm3_right, step_size, move_delay, servos);		
			delay_zhongduan(50);
			moveToPosition(current_pwm, middle_pwm2_right, step_size, move_delay, servos);
			delay_zhongduan(50);	
			moveToPosition(current_pwm, middle_pwm1_right, step_size, move_delay, servos);
			delay_zhongduan(50);			
//			moveToPosition(current_pwm, initial_pwm, step_size, move_delay, servos);		
//			delay_zhongduan(50);		
			//if(cross!=0)
			if(cross_flag==2)
			{
				shi_zi_pic_num--;
			}
			if(ImageFlag.image_element_rings!=0)
			{
				huandao_pic_num--;
			}
			pic_cunchu_num--;
		}
		speed_pid1[0] = 20;
		speed_pid2[0] = 20;
		speed_pid3[0] = 20;
		speed_pid4[0] = 20;
		speed_pid1[2] = 0;
		speed_pid2[2] = 0;
		speed_pid3[2] = 0;
		speed_pid4[2] = 0;	
		sevor_right_flag = 2;		
		sevor_left_flag = 2;	
    }        
}   
void pit_handler (void)    //计时
{
	num++;
	num_pic++;	
	ImageStatus_num++;
	num_kong++;
	num_pic_zhongduan++;
	num_qipao++;
	ssss++;
}
void delay_zhongduan(int time)  // 延时函数
{
    num_pic_zhongduan = 0;  // 重置延时计数
    while (num_pic_zhongduan < time)  // 等待 num_pic 达到指定值
    {
        // 在这里可以加入低功耗模式的指令，具体取决于你的硬件平台
//pic_speed_coord(0,0,0);
		speed_target[0] = 0;
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		//Image_Process();		//由于车身移动完毕会抖动一下，试一试
		//speed_coord(coord_now,coord_aim);
		
//		v1 = 0;
//		v2 = 0;
//		v3 = 0;
//		v4 = 0;
    }
}
//舵机pwm值调节
void moveToPosition(int current_pwm[4], int target_pwm[4], int step_size, int move_delay, int servos[4]) 
{
    bool moving = true;
    while (moving) {

        moving = false;
        for (int i = 0; i < 4; ++i) {
            if (current_pwm[i] != target_pwm[i]) {
                if (current_pwm[i] < target_pwm[i]) {
                    current_pwm[i] += step_size;
                    if (current_pwm[i] > target_pwm[i]) {
                        current_pwm[i] = target_pwm[i];
                    }
                } else {
                    current_pwm[i] -= step_size;
                    if (current_pwm[i] < target_pwm[i]) {
                        current_pwm[i] = target_pwm[i];
                    }
                }
                pwm_set_duty(servos[i], (int)current_pwm[i]);
                moving = true;
            }
        }
        delay_zhongduan(move_delay);
    }
}
	
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//分两套，左边一套，右边一套
void camera_determine(int i,int j)		//需改进，不需要一直发送，可能影响帧率 （暂且不管）     法一:如果关掉就一直发1，没关掉就一直发0
{
	camera_determine_flag=1;
	//开左开右
	if((i==0&&j==0)&&(open_close_left!=0||open_close_right!=0))
	{
		if(open_close_left!=0)
		{
			uart_write_byte(UART_1, 8);	
		}
		if(open_close_right!=0)
		{
			uart_write_byte(UART_4, 8);	
		}		
		
		speed_target[0] = 0;//使车辆保持静止
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		v1=0;
		v2=0;
		v3=0;
		v4=0;
		camera_determine_flag=0;
	}
	//开左关右
	if((i==0&&j==1)&&(open_close_left!=0||open_close_right!=1))
	{
		
		if(open_close_left!=0)
		{
			uart_write_byte(UART_1, 8);	
		}
		if(open_close_right!=1)
		{
			uart_write_byte(UART_4, 9);	
		}

		speed_target[0] = 0;//使车辆保持静止
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		v1=0;
		v2=0;
		v3=0;
		v4=0;
		camera_determine_flag=0;
	}
	//开右关左
	if((i==1&&j==0)&&(open_close_left!=1||open_close_right!=0))
	{
		if(open_close_left!=1)
		{
			uart_write_byte(UART_1, 9);	
		}
		if(open_close_right!=0)
		{
			uart_write_byte(UART_4, 8);	
		}
		speed_target[0] = 0;//使车辆保持静止
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		v1=0;
		v2=0;
		v3=0;
		v4=0;
		camera_determine_flag=0;
	}
	//关左关右
	if((i==1&&j==1)&&(open_close_left!=1||open_close_right!=1))
	{
		if(open_close_left!=1)
		{
			uart_write_byte(UART_1, 9);	
		}
		if(open_close_right!=1)
		{
			uart_write_byte(UART_4, 9);	
		}
		speed_target[0] = 0;//使车辆保持静止
		speed_target[1] = 0;
		speed_target[2] = 0;
		speed_target[3] = 0; 
		v1=0;
		v2=0;
		v3=0;
		v4=0;
		camera_determine_flag=0;
	}
}

void eiq_mode_determine(int i,int j)//优先级最高// 模式  0 小视野  1 大视野   2 环岛放照片    发送数据   5 散落 6 环岛 7 数字 
{
	
	// 00 散落照片 
	// 11 识别到斑马线 放照片  左十字 右十字（过程1）
	// 22 左十字 右十字（过程2）
	// 12 左环岛（一直）   
	// 21 右环岛（一直）

	while(i!=(eiq_mode_left_flag-5)||j!=(eiq_mode_right_flag-5))
	{
		
		if((i==0&&j==0)&&(eiq_mode_left_flag!=5||eiq_mode_right_flag!=5))
		{
			if(eiq_mode_left_flag!=5)
			{
				uart_write_byte(UART_1, 5);	
			}
			if(eiq_mode_right_flag!=5)
			{
				uart_write_byte(UART_4, 5);	
			}		
			speed_target[0] = 0;//使车辆保持静止
			speed_target[1] = 0;
			speed_target[2] = 0;
			speed_target[3] = 0; 
			v1=0;
			v2=0;
			v3=0;
			v4=0;		
		}
	
		
		if((i==1&&j==1)&&(eiq_mode_left_flag!=6||eiq_mode_right_flag!=6))
		{
			if(eiq_mode_left_flag!=6)
			{
				uart_write_byte(UART_1, 6);	
			}
			if(eiq_mode_right_flag!=6)
			{
				uart_write_byte(UART_4, 6);	
			}		
			speed_target[0] = 0;//使车辆保持静止
			speed_target[1] = 0;
			speed_target[2] = 0;
			speed_target[3] = 0; 
			v1=0;
			v2=0;
			v3=0;
			v4=0;		
		}
	
		
		if((i==2&&j==2)&&(eiq_mode_left_flag!=7||eiq_mode_right_flag!=7))
		{
			if(eiq_mode_left_flag!=7)
			{
				uart_write_byte(UART_1, 7);	
			}
			if(eiq_mode_right_flag!=7)
			{
				uart_write_byte(UART_4, 7);	
			}		
			speed_target[0] = 0;//使车辆保持静止
			speed_target[1] = 0;
			speed_target[2] = 0;
			speed_target[3] = 0; 
			v1=0;
			v2=0;
			v3=0;
			v4=0;		
		}
	
		
		if((i==1&&j==2)&&(eiq_mode_left_flag!=6||eiq_mode_right_flag!=7))//左环岛
		{
			if(eiq_mode_left_flag!=6)
			{
				uart_write_byte(UART_1, 6);	
			}
			if(eiq_mode_right_flag!=7)
			{
				uart_write_byte(UART_4, 7);	
			}		
			speed_target[0] = 0;//使车辆保持静止
			speed_target[1] = 0;
			speed_target[2] = 0;
			speed_target[3] = 0; 
			v1=0;
			v2=0;
			v3=0;
			v4=0;		
		}
	
		
		if((i==2&&j==1)&&(eiq_mode_left_flag!=7||eiq_mode_right_flag!=6))//右环岛
		{
			if(eiq_mode_left_flag!=7)
			{
				uart_write_byte(UART_1, 7);	
			}
			if(eiq_mode_right_flag!=6)
			{
				uart_write_byte(UART_4, 6);	
			}		
			speed_target[0] = 0;//使车辆保持静止
			speed_target[1] = 0;
			speed_target[2] = 0;
			speed_target[3] = 0; 
			v1=0;
			v2=0;
			v3=0;
			v4=0;				
		}
	}
}

void yi_yiduiying()
{
	if(eiq_left==7)
	{
		eiq_chuli=4;
	}
	if(eiq_left==3)
	{
		eiq_chuli=5;
	}
	if(eiq_left==4)
	{
		eiq_chuli=6;
	}
	if(eiq_left==8)
	{
		eiq_chuli=7;
	}
	if(eiq_left==6)
	{
		eiq_chuli=8;
	}
	if(eiq_left==15)
	{
		eiq_chuli=9;
	}
	if(eiq_left==11)
	{
		eiq_chuli=10;
	}
	if(eiq_left==5)
	{
		eiq_chuli=11;
	}
	if(eiq_left==2)
	{
		eiq_chuli=12;
	}
	if(eiq_left==13)
	{
		eiq_chuli=13;
	}
	if(eiq_left==12)
	{
		eiq_chuli=14;
	}
	if(eiq_left==14)
	{
		eiq_chuli=15;
	}
	if(eiq_left==1)
	{
		eiq_chuli=16;
	}
	if(eiq_left==10)
	{
		eiq_chuli=17;
	}
	if(eiq_left==9)
	 {
		eiq_chuli=18;
	}

	
	if(eiq_right==7)
	{
		eiq_chuli=4;
	}
	if(eiq_right==3)
	{
		eiq_chuli=5;
	}
	if(eiq_right==4)
	{
		eiq_chuli=6;
	}
	if(eiq_right==8)
	{
		eiq_chuli=7;
	}
	if(eiq_right==6)
	{
		eiq_chuli=8;
	}
	if(eiq_right==15)
	{
		eiq_chuli=9;
	}
	if(eiq_right==11)
	{
		eiq_chuli=10;
	}
	if(eiq_right==5)
	{
		eiq_chuli=11;
	}
	if(eiq_right==2)
	{
		eiq_chuli=12;
	}
	if(eiq_right==13)
	{
		eiq_chuli=13;
	}
	if(eiq_right==12)
	{
		eiq_chuli=14;
	}
	if(eiq_right==14)
	{
		eiq_chuli=15;
	}
	if(eiq_right==1)
	{
		eiq_chuli=16;
	}
	if(eiq_right==10)
	{
		eiq_chuli=17;
	}
	if(eiq_right==9)
	{
		eiq_chuli=18;
	}
}
