Diff: System/Process/Process.cpp

Revision:

14:93776ca449a4

Parent:

13:b6e02d6261d7

--- a/System/Process/Process.cpp	Mon Oct 08 15:51:15 2018 +0000
+++ b/System/Process/Process.cpp	Thu Oct 25 09:00:19 2018 +0000
@@ -78,7 +78,7 @@
 	else return abs(pwmVal);
 }
 
-//************メカナム********************
+//****************************************************************************************************
 
 //************カラーセンサ********************
 
@@ -97,20 +97,12 @@
 void ColorDetection();
 void getcolor();
 
-//************カラーセンサ********************
+//***************************************************************************************************************
 
 //************ライントレース変数*******************
-int PointA[3] = {400, 700, 1000};//赤，緑，青
-int PointB[3] = {400, 700, 1000};//赤，緑，青
+int PointA[3] = {350, 600, 800};//赤，緑，青
+int PointB[3] = {350, 600, 800};//赤，緑，青
 int PointC[3] = {1000, 1700, 2400};//赤，緑，青
-	
-int startP = 35;
-int downP = 5;
-
-int Asasult = 0;
-int Bsasult = 0;
-int Csasult = 0;
-int Dsasult = 0;
 
 bool compA = false;
 bool compB = false;
@@ -125,27 +117,30 @@
 Ticker Color_T;
 
 void ColorDetection();
-void Color_changeflag();
-
-//************ライントレース変数*******************
+void Color_changeflagA();
+void Color_changeflagB();
+//*****************************************************************************************************
 
 //************ROタコン******************
 QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
 QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
 Ticker get_rpm;
-PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0);  
-PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0); 
-double rpmX;
-double rpmY;
-double disX;
-double disY;
+PID Rt_X = PID(0.03, -255, 255, 0.15, 0, 0);  
+PID Rt_Y = PID(0.03, -255, 255, 0.17, 0, 0); 
+double rpmX = 0, rpmXB =0;
+double rpmY = 0, rpmYB =0;
+double disX =0,disXB =0;
+double disY =0,disYB =0;
 int palseX;
 int palseY;
-int RtpwmX;
-int RtpwmY; 
-double goalX = 900.000;
-double goalY = 700.000; 
+int RtpwmX =0,RtpwmXB =0;
+int RtpwmY =0,RtpwmYB =0; 
+double goalAX = 1000.000;
+double goalAY = 850.000;
+double goalBX = -430.000;
+double goalBY = 1000.000; 
 void filip();
+void filipB();
 
 //************ROタコン****************** 
 
@@ -165,6 +160,7 @@
 PwmOut buzzer(BUZZER_PIN);
 void BuzzerTimer_func();
 Ticker BuzzerTimer;
+Ticker LostTimer;
 bool Emsflag = false;
 //************Buzzer******************
 
@@ -174,6 +170,8 @@
 TapeLedData sendLedData;
 TapeLED_Mode ledMode = Normal;
 Ticker tapeLedTimer;
+Ticker LostLedTimer;
+//void LostLed_func();
 //************TapaLed*****************
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
@@ -298,13 +296,24 @@
 	SystemProcessInitialize();
 
 	while(1)
-	{ /*
+	{ 
+		/*
 		getcolor();
 		pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
 		pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
 		pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
 		pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
+		
+		palseX = RtX.getPulses();
+   	    palseY = RtY.getPulses();
+		
+		pc.printf("X:%d",palseX);
+		pc.printf("Y:%d",palseY);
 		*/
+		//wheel.getPulses()...どちらの方向にどれだけ回ったか
+		//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+		//軸が何回転したか
+		//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -315,6 +324,11 @@
 			CONTROLLER::Controller::DataReset();
 			AllActuatorReset();
 			lock = true;
+			//BuzzerTimer.attach(BuzzerTimer_func, 0.5);
+			//LostLedTimer.attach(LostLed_func, 0.5);
+			//sendLedData.code = (uint32_t)Yellow;
+			//buzzer = 0.5;
+			//LostTimer.attach(BuzzerTimer_func, 1.2);
 		}
 		else
 		#endif
@@ -376,10 +390,10 @@
 {	
 	tapeLED.code = (uint32_t)Green;
 	if(RedSW){
-		current = 1;
+		current = 6;
 	}
 	if(BlueSW){
-		current = 2;
+		current = 5;
 	}
 }
 #endif
@@ -388,6 +402,7 @@
 static void Process1() //手動
 {
     tapeLED.code = (uint32_t)Orange;
+    
     motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
     motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
     motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]); 
@@ -444,49 +459,55 @@
 	//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
 	//軸が何回転したか
 	//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
+
+    //filip();
+    //pc.printf("AKApwmX:%d , AKApwmY:%d\r\n",RtpwmX,RtpwmY);
+    
+    //filipB();
+    //pc.printf("BLUEpwmX:%d , BLUEpwmY:%d \r\n",RtpwmXB,RtpwmYB);
 }
 #endif
 
-#if USE_PROCESS_NUM>2
+#if USE_PROCESS_NUM>2 //Blue Zone
 static void Process2() //trace
 {	
-	tapeLED.code = (uint32_t)Yellow;
-	static bool color_flag = false;
-	
+	/*//tapeLED.code = (uint32_t)Blue;
 	static bool traceon = false;//fase1
 	static bool yokofla = false;//fase2
 	static bool boxslip = false;//fase3
 	
-	//static bool syu = false;
+	static bool Rt_flagX = false;
+	static bool Rt_flagY = false;
+
 	
 	ColorDetection();
-	Color_changeflag();
+	Color_changeflagA();
     
-	if(controller->Button.B && !color_flag)
+	/*if(controller->Button.B && !color_nopush)
 	{
 		traceon = true;
-		color_flag = true;
+		color_nopush = true;
 	}
-	else if(!controller->Button.B)color_flag = false;
+	traceon = true;
+	
+	//else if(!controller->Button.B)color_nopush = false;
 	
 	if(traceon)
 	{
-		Color_changeflag();
-		if(!invationA && !compA && !invationB && !compB)
+		Color_changeflagA();
+		if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
 		{
-		motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = BACK;
-		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
 		
-		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FR].pwm = 42;
 		motor[TIRE_FL].pwm = startP;
 		motor[TIRE_BR].pwm = startP;
 		motor[TIRE_BL].pwm = startP;
-		
-		Color_changeflag();
 		}
-	    else if(invationC && compC && !invationB && !compB)
+	    else if(invationB || compB || invationA || compA || invationC || compC)
 	    {
 	    for(int i = 0; i<1000; i++){
 	    motor[TIRE_FR].dir = BRAKE;
@@ -509,8 +530,345 @@
 	if(yokofla && !traceon)
 	{
 		//pointcalculation();
-	Color_changeflag();
-	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+	Color_changeflagA();
+	   if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
+	{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	
+		
+		boxslip = true;
+		yokofla = false;
+	}
+	else{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = 47;
+  }
+}
+  
+   if(boxslip)
+   {
+   	    for(int i = 0; i<500; i++){
+   	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		}
+		
+		RtX.reset();
+		RtY.reset();
+		
+		Rt_flagX = true;
+		boxslip = false;
+   }
+  
+    filipB();
+	if(Rt_flagX && !boxslip)
+	{
+	  filipB();
+		if(disX > -goalAX + 5){
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+	}
+	else if(disX < -goalAX + 5){
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		Rt_flagY = true;
+		Rt_flagX = false;
+	}
+  }
+	
+	
+if(Rt_flagY && !Rt_flagX){
+	filipB();
+	 if(disY < goalAY - 5){
+		motor[TIRE_FR].dir = SetStatus(-RtpwmY);
+		motor[TIRE_FL].dir = SetStatus(RtpwmY);
+		motor[TIRE_BR].dir = SetStatus(-RtpwmY);
+		motor[TIRE_BL].dir = SetStatus(RtpwmY);
+		motor[TIRE_FR].pwm = SetPWM(RtpwmY);
+		motor[TIRE_FL].pwm = SetPWM(RtpwmY);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmY);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
+	}
+	else if(disY > goalAY - 5)
+	{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	}
+  }*/
+}
+#endif
+
+#if USE_PROCESS_NUM>3
+static void Process3() //Red Zone
+{
+	/*tapeLED.code = (uint32_t)Red;
+	
+	//static bool color_nopush = false;
+	
+	static bool traceon = false;//fase1
+	static bool yokofla = false;//fase2
+	static bool boxslip = false;//fase3
+	
+	static bool Rt_flagX = false;
+	static bool Rt_flagY = false;
+	
+	//static bool syu = false;
+	
+	ColorDetection();
+	Color_changeflagA();
+    
+	/*if(controller->Button.B && !color_nopush)
+	{
+		traceon = true;
+		color_nopush = true;
+	}
+	//else if(!controller->Button.B)color_nopush = false;
+	traceon = true;
+	
+	if(traceon)
+	{
+		Color_changeflagA();
+		if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+	    else if(invationA || compA || invationB || compB || invationC || compC)
+	    {
+	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		yokofla = true;
+		traceon = false;
+		}
+	}
+	
+	
+	if(yokofla && !traceon)
+	{
+		//pointcalculation();
+	Color_changeflagA();
+	   if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+	{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		boxslip = true;
+		yokofla = false;
+	}
+	else{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+  }
+}
+  
+  
+  /********************************
+   if(boxslip && !yokofla)
+   {
+   	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		
+		RtX.reset();
+		RtY.reset();
+		
+		Rt_flagX = true;
+		boxslip = false;
+   }
+  
+	if(Rt_flagX && !boxslip)
+	{
+	  filip();
+		if(disX < goalAX - 5){
+		
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+	}
+	else if(disX > goalAX - 5){
+		
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		Rt_flagY = true;
+		Rt_flagX = false;
+	}
+  }
+	
+	
+if(Rt_flagY && !Rt_flagX){
+	filip();
+	 if(disY < goalAY - 5){
+		motor[TIRE_FR].dir = SetStatus(-RtpwmY);
+		motor[TIRE_FL].dir = SetStatus(RtpwmY);
+		motor[TIRE_BR].dir = SetStatus(-RtpwmY);
+		motor[TIRE_BL].dir = SetStatus(RtpwmY);
+		motor[TIRE_FR].pwm = SetPWM(RtpwmY);
+		motor[TIRE_FL].pwm = SetPWM(RtpwmY);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmY);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmY);
+	}
+	else if(disY > goalAY - 5)
+	{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	}
+  }*/
+}
+#endif
+
+#if USE_PROCESS_NUM>4
+static void Process4() 
+{	
+	
+}
+#endif
+
+#if USE_PROCESS_NUM>5
+static void Process5()//traceB
+{
+	static bool traceonB = true;//fase1
+	static bool yokoflaB = false;//fase2
+	static bool boxslipB = false;//fase3
+	
+	static bool Rt_flagXB = false;
+	static bool Rt_flagYB = false;
+	
+	ColorDetection();
+	Color_changeflagB();
+	
+	if(traceonB && !Rt_flagXB && !Rt_flagYB && !boxslipB)
+	{
+		Color_changeflagB();
+		if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
+		{
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		
+		motor[TIRE_FR].pwm = 48;
+		motor[TIRE_FL].pwm = 43;
+		motor[TIRE_BR].pwm = 43;
+		motor[TIRE_BL].pwm = 43;
+		}
+	    else if(invationA || compA || invationB || compB || invationC || compC)
+	    {
+	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		yokoflaB = true;
+		traceonB = false;
+		}
+	}
+	
+	
+	if(yokoflaB && !traceonB)
+	{
+	Color_changeflagB();
+	   if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
 	{
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
@@ -522,172 +880,113 @@
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
 		
-		wait(2);
-		
-		boxslip = true;
-		yokofla = false;
+		boxslipB = true;
+		yokoflaB = false;
 	}
-	else if(compA && compB && compC)
+	else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
 	{
-	    motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = BACK;
 		motor[TIRE_BR].dir = FOR;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		
-		Color_changeflag();
+		motor[TIRE_FR].pwm = 55;
+		motor[TIRE_FL].pwm = 55;
+		motor[TIRE_BR].pwm = 55;
+		motor[TIRE_BL].pwm = 55;
 	}
-	else if(invationA && invationB && invationC)
-	{
-		motor[TIRE_FR].dir = FREE;
-		motor[TIRE_FL].dir = BACK;
-		motor[TIRE_BR].dir = FOR;
-		motor[TIRE_BL].dir = FREE;
-		
-		//motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		//motor[TIRE_BL].pwm = startP;
-	}
-	else if(!invationA && !invationB && !invationC)
+	else if(invationA || invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
 	{
 		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = FREE;
 		motor[TIRE_BR].dir = FREE;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = startP;
-		//motor[TIRE_FL].pwm = startP;
-		//motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
+		motor[TIRE_FR].pwm = 55;
+		//motor[TIRE_FL].pwm = 30;
+	    //motor[TIRE_BR].pwm = 30;
+		motor[TIRE_BL].pwm = 55;
+	}
+	else if(!invationA || !invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
+	{
+		motor[TIRE_FR].dir = FREE;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FREE;
+		
+		//motor[TIRE_FR].pwm = 30;
+		motor[TIRE_FL].pwm = 55;
+		motor[TIRE_BR].pwm = 55;
+		//motor[TIRE_BL].pwm = 30;
 	}
-	else if(!invationA && compC && invationC)//C固定A下
-		{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
+	else
+	{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = 50;
+		motor[TIRE_FL].pwm = 50;
+		motor[TIRE_BR].pwm = 50;
+		motor[TIRE_BL].pwm = 50;
+	}
+}
+  
+  
+  /*********************************/
+   if(boxslipB && !yokoflaB && !traceonB)
+   {
+   	    Color_changeflagB();
+		if((compA && compB) || compC){
+		
+		RtX.reset();
+		RtY.reset();
+		
+		Rt_flagXB = true;
+		boxslipB = false;
+		}
+		else if(invationA || invationB){
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
 		motor[TIRE_BR].dir = BACK;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 100;
-		motor[TIRE_BR].pwm = 55;
-		motor[TIRE_BL].pwm = startP;
-		
-		Color_changeflag();
+		motor[TIRE_FR].pwm = 25;
+		motor[TIRE_FL].pwm = 25;
+		motor[TIRE_BR].pwm = 25;
+		motor[TIRE_BL].pwm = 25;
 		}
-	else if(compA && compB && !invationC)//AB固定C下
-		{
-		motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = 55;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 100;
-		
-		Color_changeflag();
-		}
-		else if(compA && compB && !compC && invationC)//AB固定C上
-		{
+		else if(!invationA || !invationB){
 		motor[TIRE_FR].dir = BACK;
 		motor[TIRE_FL].dir = BACK;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = 55;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 100;
-		
-		Color_changeflag();
-		}
-		else if(!compA && invationA && compC)//C固定A上
-		{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = FOR;
 		motor[TIRE_BL].dir = FOR;
 		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 100;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = 55;
-		
-		Color_changeflag();
+		motor[TIRE_FR].pwm = 25;
+		motor[TIRE_FL].pwm = 25;
+		motor[TIRE_BR].pwm = 25;
+		motor[TIRE_BL].pwm = 25;
 		}
-  }
-  
-   if(boxslip)
-   {
-   	    motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
    }
-}
-#endif
-
-#if USE_PROCESS_NUM>3
-static void Process3() 
-{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>4
-static void Process4() 
-{	
-}
-#endif
-
-#if USE_PROCESS_NUM>5
-static void Process5() //ロタコンXY 
-{
-	tapeLED.code = (uint32_t)White;
-	
-	static bool nopushed = false; 
-	static bool Rt_flagX = false;
-	static bool Rt_flagY = false;
-	
-	if(controller->Button.A && !nopushed){
-		Rt_flagX = true;
-		nopushed = true;
-		
-		RtX.reset();
-		RtY.reset();
-	}else if(!controller->Button.A)nopushed = false;
-
-    filip();
-
-	if(Rt_flagX)
+   
+   
+  
+	filipB();
+	if(Rt_flagXB && !boxslipB)
 	{
-	  filip();
-		if(disX < goalX - 5){
-		filip();
-		
-		motor[TIRE_FR].dir = SetStatus(-RtpwmX);
-		motor[TIRE_FL].dir = SetStatus(-RtpwmX);
-		motor[TIRE_BR].dir = SetStatus(RtpwmX);
-		motor[TIRE_BL].dir = SetStatus(RtpwmX);
+	  filipB();
+		if(disXB > goalBX + 5){
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
 		motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
 		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
 		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
 		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
 	}
-	else if(disX > goalX - 5){
-		
-		for(int i = 0; i<200; i++){
+	else if(disXB < goalBX + 5){
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
@@ -697,18 +996,18 @@
 		motor[TIRE_FL].pwm = 255;
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
-		}
+		
+		wait(1);
 		
-		Rt_flagY = true;
-		Rt_flagX = false;
+		Rt_flagYB = true;
+		Rt_flagXB = false;
 	}
   }
 	
 	
-if(Rt_flagY && !Rt_flagX){
-	filip();
-	 if(disY < goalY - 5){
-		filip();
+if(Rt_flagYB && !Rt_flagXB){
+	filipB();
+	 if(disYB < goalBY - 5){
 		motor[TIRE_FR].dir = SetStatus(-RtpwmY);
 		motor[TIRE_FL].dir = SetStatus(RtpwmY);
 		motor[TIRE_BR].dir = SetStatus(-RtpwmY);
@@ -716,16 +1015,15 @@
 		motor[TIRE_FR].pwm = SetPWM(RtpwmY);
 		motor[TIRE_FL].pwm = SetPWM(RtpwmY);
 		motor[TIRE_BR].pwm = SetPWM(RtpwmY);
-		motor[TIRE_BL].pwm = SetPWM(RtpwmY);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
 	}
-	else if(disY > goalY - 5)
+	else if(disYB > goalBY - 5)
 	{
-		filip();
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
 		motor[TIRE_BL].dir = BRAKE;
-		motor[TIRE_FR].pwm = 255*0.85;
+		motor[TIRE_FR].pwm = 255;
 		motor[TIRE_FL].pwm = 255;
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
@@ -734,52 +1032,36 @@
 }
 #endif
 
-#if USE_PROCESS_NUM>6
+#if USE_PROCESS_NUM>6//traceA
 static void Process6() 
-{
-	tapeLED.code = (uint32_t)Yellow;
-	static bool color_flag = false;
-	
-	static bool traceon = false;//fase1
+{	
+	static bool traceon = true;//fase1
 	static bool yokofla = false;//fase2
 	static bool boxslip = false;//fase3
 	
-	static bool nopushed = false; 
 	static bool Rt_flagX = false;
 	static bool Rt_flagY = false;
 	
-	//static bool syu = false;
+	ColorDetection();
+	Color_changeflagA();
 	
-	ColorDetection();
-	Color_changeflag();
-    
-	if(controller->Button.B && !color_flag)
+	if(traceon && !Rt_flagX && !Rt_flagY && !boxslip )
 	{
-		traceon = true;
-		color_flag = true;
-	}
-	else if(!controller->Button.B)color_flag = false;
-	
-	if(traceon)
-	{
-		Color_changeflag();
-		if(!invationA && !compA && !invationB && !compB)
+		Color_changeflagA();
+		if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
 		{
 		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = FOR;
 		motor[TIRE_BR].dir = BACK;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		
-		Color_changeflag();
+		motor[TIRE_FR].pwm = 42;
+		motor[TIRE_FL].pwm = 42;
+		motor[TIRE_BR].pwm = 42;
+		motor[TIRE_BL].pwm = 42;
 		}
-	    else if(invationC && compC && !invationB && !compB)
+	    else if(invationA || compA || invationB || compB || invationC || compC)
 	    {
-	    for(int i = 0; i<1000; i++){
 	    motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
@@ -789,7 +1071,6 @@
 		motor[TIRE_FL].pwm = 255;
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
-		}
 		
 		yokofla = true;
 		traceon = false;
@@ -799,9 +1080,8 @@
 	
 	if(yokofla && !traceon)
 	{
-		//pointcalculation();
-	Color_changeflag();
-	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+	Color_changeflagA();
+	   if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
 	{
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
@@ -813,26 +1093,22 @@
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
 		
-		wait(2);
-		
 		boxslip = true;
 		yokofla = false;
 	}
-	else if(compA && compB && compC)
+	else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
 	{
-	    motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = BACK;
 		motor[TIRE_BR].dir = FOR;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		
-		Color_changeflag();
+		motor[TIRE_FR].pwm = 55;
+		motor[TIRE_FL].pwm = 55;
+		motor[TIRE_BR].pwm = 55;
+		motor[TIRE_BL].pwm = 55;
 	}
-	else if(invationA && invationB && invationC)
+	else if((invationA || invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
 	{
 		motor[TIRE_FR].dir = FREE;
 		motor[TIRE_FL].dir = BACK;
@@ -840,85 +1116,93 @@
 		motor[TIRE_BL].dir = FREE;
 		
 		//motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_FL].pwm = 55;
+		motor[TIRE_BR].pwm = 55;
 		//motor[TIRE_BL].pwm = startP;
 	}
-	else if(!invationA && !invationB && !invationC)
+	else if((!invationA || !invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
 	{
 		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = FREE;
 		motor[TIRE_BR].dir = FREE;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FR].pwm = 55;
 		//motor[TIRE_FL].pwm = startP;
-		//motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
+	    //motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = 55;
+	}
+	else
+	{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = 50;
+		motor[TIRE_FL].pwm = 50;
+		motor[TIRE_BR].pwm = 50;
+		motor[TIRE_BL].pwm = 50;
 	}
-	else if(!invationA && compC && invationC)//C固定A下
-		{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
+}
+  
+  
+  /*********************************/
+   if(boxslip && !yokofla && !traceon)
+   {
+   	    Color_changeflagA();
+		if((compA && compB) || compC){
+		
+		RtX.reset();
+		RtY.reset();
+		
+		Rt_flagX = true;
+		boxslip = false;
+		}
+		else if(invationA || invationB && !compC && !(compA && compB)){
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		
+		motor[TIRE_FR].pwm = 25;
+		motor[TIRE_FL].pwm = 25;
+		motor[TIRE_BR].pwm = 25;
+		motor[TIRE_BL].pwm = 25;
+		}
+		else if(!invationA || !invationB && !compC && !(compA && compB)){
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
 		motor[TIRE_BR].dir = BACK;
 		motor[TIRE_BL].dir = BACK;
 		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 100;
-		motor[TIRE_BR].pwm = 55;
-		motor[TIRE_BL].pwm = startP;
+		motor[TIRE_FR].pwm = 25;
+		motor[TIRE_FL].pwm = 25;
+		motor[TIRE_BR].pwm = 25;
+		motor[TIRE_BL].pwm = 25;
+		}
+   }
+   
+   
+    filip();
+	if(Rt_flagX && !boxslip)
+	{
+	  filip();
+		if(disX < goalAX - 5){
 		
-		Color_changeflag();
-		}
-	else if(compA && compB && !invationC)//AB固定C下
-		{
 		motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = 55;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 100;
-		
-		Color_changeflag();
-		}
-		else if(compA && compB && !compC && invationC)//AB固定C上
-		{
-		motor[TIRE_FR].dir = BACK;
-		motor[TIRE_FL].dir = BACK;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = 55;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 100;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+	}
+	else if(disX > goalAX - 5){
 		
-		Color_changeflag();
-		}
-		else if(!compA && invationA && compC)//C固定A上
-		{
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = FOR;
-		motor[TIRE_BL].dir = FOR;
-		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 100;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = 55;
-		
-		Color_changeflag();
-		}
-  }
-  
-   if(boxslip)
-   {
-   	    for(int i = 0; i<500; i++)
-   	    motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
 		motor[TIRE_BL].dir = BRAKE;
 		
@@ -927,52 +1211,7 @@
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
 		
-		Rt_flagX = true;
-		nopushed = true;
-		
-		RtX.reset();
-		RtY.reset();
-   }
-  
-	
-	/*if(controller->Button.A && !nopushed){
-		Rt_flagX = true;
-		nopushed = true;
-		
-		RtX.reset();
-		RtY.reset();
-	}else if(!controller->Button.A)nopushed = false;
-	*/
-    filip();
-
-	if(Rt_flagX)
-	{
-	  filip();
-		if(disX < goalX - 5){
-		filip();
-		
-		motor[TIRE_FR].dir = SetStatus(-RtpwmX);
-		motor[TIRE_FL].dir = SetStatus(-RtpwmX);
-		motor[TIRE_BR].dir = SetStatus(RtpwmX);
-		motor[TIRE_BL].dir = SetStatus(RtpwmX);
-		motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
-		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
-	}
-	else if(disX > goalX - 5){
-		
-		for(int i = 0; i<500; i++){
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		motor[TIRE_FR].pwm = 255;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
-		}
+		wait(1);
 		
 		Rt_flagY = true;
 		Rt_flagX = false;
@@ -982,8 +1221,7 @@
 	
 if(Rt_flagY && !Rt_flagX){
 	filip();
-	 if(disY < goalY - 5){
-		filip();
+	 if(disY < goalAY - 5){
 		motor[TIRE_FR].dir = SetStatus(-RtpwmY);
 		motor[TIRE_FL].dir = SetStatus(RtpwmY);
 		motor[TIRE_BR].dir = SetStatus(-RtpwmY);
@@ -993,9 +1231,8 @@
 		motor[TIRE_BR].pwm = SetPWM(RtpwmY);
 		motor[TIRE_BL].pwm = SetPWM(RtpwmY);
 	}
-	else if(disY > goalY - 5)
+	else if(disY > goalAY - 5)
 	{
-		filip();
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
@@ -1006,7 +1243,6 @@
 		motor[TIRE_BL].pwm = 255;
 	}
   }
-   
 }
 #endif
 
@@ -1139,7 +1375,6 @@
 #if USE_PROCESS_NUM>8 //kakudo
 static void Process8()
 {
-	
 }
 #endif
 
@@ -1180,8 +1415,22 @@
    	disX = 48*3.141*rpmX;
 	disY = 48*3.141*rpmY;
 	
-	RtpwmX = (int)Rt_X.SetPV(disX , goalX);
-	RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
+	RtpwmX = (int)Rt_X.SetPV(disX , goalAX);
+	RtpwmY = (int)Rt_Y.SetPV(disY , goalAY);
+}
+
+void filipB(){
+   	palseX = RtX.getPulses();
+   	palseY = RtY.getPulses();
+   	
+   	rpmXB = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
+   	rpmYB = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
+   	
+   	disXB = 48*3.141*rpmXB;
+	disYB = 48*3.141*rpmYB;
+	
+	RtpwmXB = (int)Rt_X.SetPV(disXB , goalBX);
+	RtpwmYB = (int)Rt_Y.SetPV(disYB , goalBY);
 }
 
 unsigned long ColorIn(int index)
@@ -1241,29 +1490,26 @@
     Color_D[2] = ColorIn(3);
 }
 
-void Color_changeflag(){
+void Color_changeflagA(){
     ColorDetection();
     
     if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
 	{
-	invationA ^= 1;//start false,over true
 	compA = true;//on true,noon false
 	}	
-	else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶
+	else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
 	
 	if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
 	{
-	invationB ^= 1;//start false,over true
 	compB = true;//on true,noon false
 	}	
-	else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶
+	else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
 	
 	if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
 	{
-	invationC ^= 1;//start false,over true
 	compC = true;//on true,noon false
 	}	
-	else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶
+	else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
 	/*
 	if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
 	{
@@ -1272,6 +1518,68 @@
 	}	
 	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
     */
+    
+    if(compA && !compB)
+    {
+    	invationA = false;
+    }
+    else if(!compA && compB)
+    {
+    	invationA = true;
+    }
+    
+    if(!invationA && !compA && !compB){
+    invationB = false;
+    }
+    else if(invationA && !compA && !compB){
+    invationB = true;
+    }
+}
+
+void Color_changeflagB(){
+    ColorDetection();
+    
+    if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
+	{
+	compA = true;//on true,noon false
+	}	
+	else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
+	
+	if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
+	{
+	compB = true;//on true,noon false
+	}	
+	else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
+	
+	if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
+	{
+	compC = true;//on true,noon false
+	}	
+	else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
+	/*
+	if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
+	{
+	invationD ^= 1;//start false,over true
+	compD = true;//on true,noon false
+	}	
+	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
+    */
+    
+    if(compB && !compA)
+    {
+    	invationB = false;
+    }
+    else if(!compB && compA)
+    {
+    	invationB = true;
+    }
+    
+    if(!invationB && !compA && !compB){
+    invationA = false;
+    }
+    else if(invationB && !compA && !compB){
+    invationA = true;
+    }
 }
 
 void getcolor(){
@@ -1319,4 +1627,8 @@
 void TapeLedEms_func() {
 	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
 }
+
+void LostLed_func(){
+	sendLedData.code = sendLedData.code == (uint32_t)Yellow ? (uint32_t)Black : (uint32_t)Yellow;
+}
 #pragma endregion