Diff: System/Process/Process.cpp

Revision:

10:ce0421ec431d

Parent:

9:f93fc79a49ea

Child:

13:ddd605700e8a

--- a/System/Process/Process.cpp	Thu Oct 04 12:14:25 2018 +0000
+++ b/System/Process/Process.cpp	Sat Oct 06 17:05:33 2018 +0000
@@ -108,21 +108,19 @@
 	else return abs(pwmVal);
 }
 
-//************メカナム********************
+//********************************
 
-//************カラーセンサ********************
+
+//カラーセンサ////
 
 int Color_A[3]; //[赤,緑,青]
 int Color_B[3];
 int Color_C[3];
 int Color_D[3];         
 int intergration = 50;
-
-void ColorDetection();
+////////////////
 
-//************カラーセンサ********************
-
-//************ライントレース変数*******************
+//**********ライントレース変数*********
 int Point[3] = {234, 466, 590};//赤，緑，青
 	
 int startP = 35;
@@ -133,13 +131,25 @@
 int Csasult = 0;
 int Dsasult = 0;
 
-void pointcalculation();
+bool compA = false;
+bool compB = false;
+bool compC = false;
+bool compD = false;
+	
+bool invationA = false;
+bool invationB = false;
+bool invationC = false;
+bool invationD = false;
 
 Ticker Color_T;
-//************ライントレース変数*******************
+
+void ColorDetection();
+void Color_changeflag();
+void pointcalculation();
+//*************************************
 
 
-//ROタコン
+//*************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;
@@ -160,8 +170,7 @@
 
 void filip();
 //PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
- 
-//ROタコン 
+//********************************ROタコン 
 
 //************ジャイロ*******************
 
@@ -173,14 +182,14 @@
 int AngletargetX = 20;
 int AngletargetY = -20;
 int Angle_I = -5;
-//************ジャイロ*******************
+//*******************************
 
 //************Buzzer******************
 DigitalOut buzzer(BUZZER_PIN);
 void BuzzerTimer_func();
 Ticker BuzzerTimer;
 bool Emsflag = false;
-//************Buzzer******************
+//******************************
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
@@ -219,7 +228,8 @@
 void SystemProcessInitialize()
 {
 	#pragma region USER-DEFINED_VARIABLE_INIT
-	get_rpm.attach_us(&filip,100);
+	get_rpm.attach_us(&filip,1000);
+	//Color_T.attach_us(&ColorDetection,1000);
 	
 	/*Replace here with the initialization code of your variables.*/
 
@@ -415,47 +425,18 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-	if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
-	    motor[Angle_R].dir = BRAKE;
-	    motor[Angle_L].dir = BRAKE;
-	    motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-	}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){
-	    motor[Angle_R].dir = BRAKE;
-	    motor[Angle_L].dir = BRAKE;
-	    motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-	}	
-	for(int i = 0;i<20;i++){
-		float y = 0;
-		y = acc[1]*1000;
-		float rotateY = (y - 305)/2.21 - 90;
-		Angle += rotateY;
-	}
-	Angle = Angle /20;
-	int gyropwm = gyro.SetPV(Angle,Angle_I);
-	
-	if(controller->Button.A){
-		Angle_flagI = true;
-	}
-	if (Angle_flagI){
-		motor[Angle_R].dir = SetStatus(gyropwm);
-		motor[Angle_L].dir = SetStatus(-gyropwm);
-		motor[Angle_R].pwm = SetPWM(gyropwm);
-		motor[Angle_L].pwm = SetPWM(gyropwm);
-		if(Angle_I - 2 < Angle && Angle < Angle_I + 2){
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		Angle_flagI = false;
-		}
-	}
+	ColorDetection();
+	pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]);
+	pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]);
+	pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]);
+	pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]);
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
-       
+    
 	   
     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]);
@@ -485,53 +466,253 @@
 {
 	static bool color_flag = false;
 	
+	static bool traceon = false;//fase0
+	static bool Phase1_PA = false;//fase1
+	static bool Phase1_PB = false;
+	static bool Phase1_PAB = false;
+	static bool Phase2 = false;//fase2
+	
+	ColorDetection();
+	Color_changeflag();
+	//pointcalculation();
+	/***********************
+	ColorDetection();//値
+	
+    Color_changeflag();//通り越しフラグ処理
+    invationA...通り越し
+    compA...線上
+    
+    pointcalculation();//値判定
+    Asasult...値が小さい方が白ラインに近い
+    *///********************
+	
+	//start////////////////////////
+	if(controller->Button.B && !color_flag)
+	{
+		traceon ^= 1;
+		color_flag = true;
+	}
+	else if(!controller->Button.B)color_flag = false;
+	///////////////////////////////
+	
+	if(traceon)
+	{
+		Color_changeflag();
+		if(!invationA && !invationB && !invationC && !invationD)
+		{
+		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(compA && !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;
+		
+		wait(1);
+		Phase1_PA = true;
+		traceon = false;
+		}
+		else if(!invationA && !compA && 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;
+		
+		wait(1);
+		Phase1_PB = true;
+		traceon = false;
+		}
+		else if(compA && 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;
+		
+		wait(1);
+		Phase1_PAB = true;
+		traceon = false;
+		}
+		else
+		{
+		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;
+		}
+	}
+	
+	if(Phase1_PA || Phase1_PB || Phase1_PAB && !traceon)
+	{
+		Color_changeflag();
+		if(LimitSw::IsPressed(0) || LimitSw::IsPressed(1))
+		{
+	    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);
+		Phase2 = true;
+		Phase1_PA = false;
+		Phase1_PB = false;
+		Phase1_PAB = false;
+		}
+		else if(compA && !invationC && !compC)//A固定
+		{
+		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(!invationA && !compA && compC)//C固定
+		{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		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();
+		}
+		else if(compA && !compC && invationC)
+		{
+		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)
+		{
+		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();
+		}
+		else if(compA && compC)
+		{
+		Color_changeflag();
+		
+		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();
+		}
+		else
+		{
+		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;
+		}
+	}
+	
+	if(Phase2){
+	    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;
+	}
+	/*
+	if(!Phase1 && Phase2)
+	{
+		//rotakon
+	}
+	*/
+}
+#endif
+
+#if USE_PROCESS_NUM>3
+static void Process3()
+{
+	static bool color_flag = false;
+	
 	static bool traceon = false;//fase1
 	static bool yokofla = false;//fase2
 	static bool boxslip = false;//fase3
 	
-	static bool compA = false;
-	static bool compB = false;
-	static bool compC = false;
-	static bool compD = false;
-	
-	static bool invationA = false;
-	static bool invationB = false;
-	static bool invationC = false;
-	static bool invationD = false;
+	//static bool syu = false;
 	
 	ColorDetection();
-	//
-	if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
-	{
-	invationA ^= 1;//start false,over true
-	compA = true;//on true,noon false
-	}	
-	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
-	
-	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
-	{
-	invationB ^= 1;//start false,over true
-	compB = true;//on true,noon false
-	}	
-	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
-	/*
-	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
-	{
-	invationC ^= 1;//start false,over true
-	compC = true;//on true,noon false
-	}	
-	else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[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;//茶
-    */
-
-	//
-	
+	Color_changeflag();
+    
 	if(controller->Button.B && !color_flag)
 	{
 		traceon ^= 1;
@@ -539,7 +720,7 @@
 	}
 	else if(!controller->Button.B)color_flag = false;
 	
-	if(traceon && !yokofla && !boxslip)
+	if(traceon)
 	{
 		if(!invationA && !compA && !invationB && !compB)
 		{
@@ -555,23 +736,33 @@
 		}
 	    else if(invationA && compA && !invationB && !compB)
 	    {
-	    motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = BACK;
-		motor[TIRE_BL].dir = BACK;
+	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
 		
-		motor[TIRE_FR].pwm = startP - downP;
-		motor[TIRE_FL].pwm = startP - downP;
-		motor[TIRE_BR].pwm = startP - downP;
-		motor[TIRE_BL].pwm = startP - downP;
-	    }
-		else if(invationA && !compA && !invationB && !compB)
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+		
+		wait(2);
+		
+		yokofla = true;
+		traceon = false;
+		}
+		else if(!invationA && !compA && !invationB && !compB)
 	    {
 	    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(2);
 		
 		yokofla = true;
@@ -584,60 +775,33 @@
 		motor[TIRE_BL].dir = BRAKE;
 		}
 	}
-		
-	if(!traceon && yokofla && !boxslip)
-	 {
-		if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
-		{
+	
+	pointcalculation();
+	
+	if(yokofla && !traceon)
+	{
+		//pointcalculation();
+	Color_changeflag();
+	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;
+		
 		wait(2);
 		
 		boxslip = true;
 		yokofla = false;
-		}
-		else if(invationA && !compA && invationB)
-		{
-		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_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		}
-		else if(!invationA && !compB && !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 = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		}
-		else if(invationA && compA && !invationB && !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 = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		}
-		else if(compB && invationB)
-		{
-		motor[TIRE_FR].dir = FOR;
+	}
+	else if(compA && compB)
+	{
+	    motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = BACK;
 		motor[TIRE_BR].dir = FOR;
 		motor[TIRE_BL].dir = BACK;
@@ -646,27 +810,53 @@
 		motor[TIRE_FL].pwm = startP;
 		motor[TIRE_BR].pwm = startP;
 		motor[TIRE_BL].pwm = startP;
-		}
-		else
-		{
-		motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = BACK;
-		motor[TIRE_BR].dir = FOR;
-		motor[TIRE_BL].dir = BACK;
+		
+		Color_changeflag();
+	}
+	else if(compA && !compB)
+	{
+	    motor[TIRE_FR].dir = FREE;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = FREE;
+		
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		
+		Color_changeflag();
+	}
+	else if(!compA && compB)
+	{
+	    motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = FREE;
+		motor[TIRE_BR].dir = FREE;
+		motor[TIRE_BL].dir = FOR;
 		
 		motor[TIRE_FR].pwm = startP;
 		motor[TIRE_FL].pwm = startP;
 		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;	
-		}
+		motor[TIRE_BL].pwm = startP;
+		
+		Color_changeflag();
 	}
+	else if(compB && compC && (!compA || !compD))
+	{
+	    motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
 		
-	if(!traceon && !yokofla && boxslip)
-	 {
-	 	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
-	 	{
-	 	motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		
+		Color_changeflag();
+	}
+	else if(compA && compD && (!compB || !compC))
+	{
+	    motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
 		motor[TIRE_BR].dir = BACK;
 		motor[TIRE_BL].dir = BACK;
 		
@@ -674,73 +864,36 @@
 		motor[TIRE_FL].pwm = startP;
 		motor[TIRE_BR].pwm = startP;
 		motor[TIRE_BL].pwm = startP;
-	 	}
-	 	else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
-	 	{
-	 	motor[TIRE_FR].dir = BRAKE;
+		
+		Color_changeflag();
+	}
+	else
+	{
+		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
 		motor[TIRE_BR].dir = BRAKE;
 		motor[TIRE_BL].dir = BRAKE;
-		}
-	 }
-		/*////
-		motor[0].dir = BACK;
-		motor[1].dir = BACK;
-		motor[2].dir = FOR;
-		motor[3].dir = FOR;
-		
-		motor[0].pwm = startP;
-		motor[1].pwm = startP;
-		motor[2].pwm = startP;
-		motor[3].pwm = startP;
-		else if()
-		{
-		motor[0].dir = BRAKE;
-		motor[1].dir = BRAKE;
-		motor[2].dir = BRAKE;
-		motor[3].dir = BRAKE;
 		
-		motor[0].pwm = 255;
-		motor[1].pwm = 255;
-		motor[2].pwm = 255;
-		motor[3].pwm = 255;
-		}*///////
-}
-#endif
-
-#if USE_PROCESS_NUM>3
-static void Process3()
-{
-	if(controller->Button.R){
-	    motor[Angle_R].dir = FOR;
-	    motor[Angle_L].dir = BACK;
-	    motor[Angle_R].pwm = 150;
-	    motor[Angle_L].pwm = 150;
-	}else if(controller->Button.L){
-	    motor[Angle_R].dir = BACK;
-	    motor[Angle_L].dir = FOR;
-	    motor[Angle_R].pwm = 150;
-	    motor[Angle_L].pwm = 150;
-	}else{
-		motor[Angle_R].dir = BRAKE;
-	    motor[Angle_L].dir = BRAKE;
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
 	}
+  }
+  
+   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;
+   }
 	
-	if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
-	    motor[Angle_R].dir = BRAKE;
-	    motor[Angle_L].dir = BRAKE;
-	}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
-	    motor[Angle_R].dir = BRAKE;
-	    motor[Angle_L].dir = BRAKE;
-	}
-	for(int i = 0;i<20;i++){
-		float y = 0;
-		y = acc[1]*1000;
-		float rotateY = (y - 305)/2.21 - 90;
-		Angle += rotateY;
-	}
-	Angle = Angle /20;
-	pc.printf("Y:%f \r\n",Angle);
 }
 #endif
 
@@ -897,183 +1050,7 @@
 #if USE_PROCESS_NUM>6
 static void Process6()
 {
-	/*static bool color_flag = false;
 	
-	static bool traceon = false;//fase1
-	static bool yokofla = false;//fase2
-	static bool boxslip = false;//fase3
-	
-	static bool syu = false;
-	
-	static bool compA = false;
-	static bool compB = false;
-	static bool compC = false;
-	static bool compD = false;
-	
-	static bool invationA = false;
-	static bool invationB = false;
-	static bool invationC = false;
-	static bool invationD = false;
-	
-	
-	ColorDetection();
-    
-	//
-	if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
-	{
-	invationA ^= 1;//start false,over true
-	compA = true;//on true,noon false
-	}	
-	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
-	
-	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
-	{
-	invationB ^= 1;//start false,over true
-	compB = true;//on true,noon false
-	}	
-	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
-	/*
-	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
-	{
-	invationC ^= 1;//start false,over true
-	compC = true;//on true,noon false
-	}	
-	else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[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(controller->Button.B && !color_flag)
-	{
-		traceon ^= 1;
-		color_flag = true;
-	}
-	else if(!controller->Button.B)color_flag = false;
-	
-	if(traceon && !yokofla && !boxslip && !syu)
-	{
-		if(!invationA && !compA && !invationB && !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 = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		}
-	    else if(invationA && compA && !invationB && !compB)
-	    {
-	    motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		wait(2);
-		
-		syu = true;
-		yokofla = false;
-		traceon = false;
-		}
-		else{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		}
-	}
-	
-	pointcalculation();
-	
-	if(syu && !traceon && !yokofla && !boxslip)
-	{
-		if(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)
-		{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		wait(2);
-		
-		yokofla = true;
-		traceon = false;
-		syu = false;
-		}
-		else if(Asasult < Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
-		{
-		motor[TIRE_FR].dir = FOR;
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = FOR;
-		motor[TIRE_BL].dir = FOR;
-		
-		motor[TIRE_FR].pwm = startP;
-		motor[TIRE_FL].pwm = startP;
-		motor[TIRE_BR].pwm = startP;
-		motor[TIRE_BL].pwm = startP;
-		}
-		else if(Asasult > Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
-		{
-	    motor[TIRE_FR].dir = BACK;
-		motor[TIRE_FL].dir = BACK;
-		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
-		{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		}
-	}
-	
-	if(!syu && !traceon && yokofla && !boxslip)
-	{
-	    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;
-	}
-	else 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;
-		
-		wait(2);
-		
-		boxslip = true;
-		yokofla = false;
-	}
-	else
-	{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-	}
-	*/
 }
 #endif
 
@@ -1129,19 +1106,6 @@
 }
 
 #pragma region USER-DEFINED-FUNCTIONS
-void pointcalculation(){
-	ColorDetection();
-	/*if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
-	{
-	invationA ^= 1;//start false,over true
-	compA = true;//on true,noon false
-	}	
-	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶*/
-	for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];}
-	for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];}
-	for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];}
-	for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];}
-}
 
 void filip(){
    	palseX = RtX.getPulses();
@@ -1197,6 +1161,53 @@
     wait_us(3);
     Color_D[2] = ColorIn(3);
 }
+
+void Color_changeflag(){
+    ColorDetection();
+    
+    if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+	{
+	invationA ^= 1;//start false,over true
+	compA = true;//on true,noon false
+	}	
+	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
+	
+	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
+	{
+	invationB ^= 1;//start false,over true
+	compB = true;//on true,noon false
+	}	
+	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
+	
+	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
+	{
+	invationC ^= 1;//start false,over true
+	compC = true;//on true,noon false
+	}	
+	else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[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;//茶
+}
+
+void pointcalculation(){
+	ColorDetection();
+	/*if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+	{
+	invationA ^= 1;//start false,over true
+	compA = true;//on true,noon false
+	}	
+	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶*/
+	for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];}
+	for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];}
+	for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];}
+	for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];}
+}
+
 void BuzzerTimer_func() {
 	buzzer = !buzzer;
 }