Diff: System/Process/Process.cpp

Revision:

13:b6e02d6261d7

Parent:

12:c09b3e08a316

Child:

14:93776ca449a4

Child:

15:dfcec98f5aa9

--- a/System/Process/Process.cpp	Sun Oct 07 09:08:18 2018 +0000
+++ b/System/Process/Process.cpp	Mon Oct 08 15:51:15 2018 +0000
@@ -1,3 +1,4 @@
+
 #include "mbed.h"
 #include "Process.h"
 #include "QEI.h"
@@ -95,10 +96,13 @@
 void ColorIn();
 void ColorDetection();
 void getcolor();
+
 //************カラーセンサ********************
 
 //************ライントレース変数*******************
-int Point[3] = {234, 466, 590};//赤，緑，青
+int PointA[3] = {400, 700, 1000};//赤，緑，青
+int PointB[3] = {400, 700, 1000};//赤，緑，青
+int PointC[3] = {1000, 1700, 2400};//赤，緑，青
 	
 int startP = 35;
 int downP = 5;
@@ -108,16 +112,28 @@
 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();
+
 //************ライントレース変数*******************
 
 //************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.1, 0, 0);  
+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;
@@ -127,8 +143,8 @@
 int palseY;
 int RtpwmX;
 int RtpwmY; 
-double goalX = 1200.000;
-double goalY = 900.000; 
+double goalX = 900.000;
+double goalY = 700.000; 
 void filip();
 
 //************ROタコン****************** 
@@ -282,13 +298,13 @@
 	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]);
-		
+		*/
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -359,6 +375,12 @@
 static void Process0() 
 {	
 	tapeLED.code = (uint32_t)Green;
+	if(RedSW){
+		current = 1;
+	}
+	if(BlueSW){
+		current = 2;
+	}
 }
 #endif
 
@@ -426,8 +448,571 @@
 #endif
 
 #if USE_PROCESS_NUM>2
-static void Process2() //自動角度調節
+static void Process2() //trace
+{	
+	tapeLED.code = (uint32_t)Yellow;
+	static bool color_flag = false;
+	
+	static bool traceon = false;//fase1
+	static bool yokofla = false;//fase2
+	static bool boxslip = false;//fase3
+	
+	//static bool syu = false;
+	
+	ColorDetection();
+	Color_changeflag();
+    
+	if(controller->Button.B && !color_flag)
+	{
+		traceon = true;
+		color_flag = true;
+	}
+	else if(!controller->Button.B)color_flag = false;
+	
+	if(traceon)
+	{
+		Color_changeflag();
+		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;
+		
+		Color_changeflag();
+		}
+	    else if(invationC && compC && !invationB && !compB)
+	    {
+	    for(int i = 0; i<1000; 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;
+		}
+		
+		yokofla = true;
+		traceon = false;
+		}
+	}
+	
+	
+	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(compA && compB && compC)
+	{
+	    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 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)
+	{
+		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;
+	}
+	else if(!invationA && compC && invationC)//C固定A下
+		{
+		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 && 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;
+		
+		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)
+   {
+   	    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)
+	{
+	  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<200; 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;
+		}
+		
+		Rt_flagY = true;
+		Rt_flagX = false;
+	}
+  }
+	
+	
+if(Rt_flagY && !Rt_flagX){
+	filip();
+	 if(disY < goalY - 5){
+		filip();
+		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 > goalY - 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_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	}
+  }
+}
+#endif
+
+#if USE_PROCESS_NUM>6
+static void Process6() 
+{
+	tapeLED.code = (uint32_t)Yellow;
+	static bool color_flag = false;
+	
+	static bool traceon = false;//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_changeflag();
+    
+	if(controller->Button.B && !color_flag)
+	{
+		traceon = true;
+		color_flag = true;
+	}
+	else if(!controller->Button.B)color_flag = false;
+	
+	if(traceon)
+	{
+		Color_changeflag();
+		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;
+		
+		Color_changeflag();
+		}
+	    else if(invationC && compC && !invationB && !compB)
+	    {
+	    for(int i = 0; i<1000; 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;
+		}
+		
+		yokofla = true;
+		traceon = false;
+		}
+	}
+	
+	
+	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(compA && compB && compC)
+	{
+	    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 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)
+	{
+		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;
+	}
+	else if(!invationA && compC && invationC)//C固定A下
+		{
+		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 && 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;
+		
+		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;
+		
+		motor[TIRE_FR].pwm = 255;
+		motor[TIRE_FL].pwm = 255;
+		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;
+		}
+		
+		Rt_flagY = true;
+		Rt_flagX = false;
+	}
+  }
+	
+	
+if(Rt_flagY && !Rt_flagX){
+	filip();
+	 if(disY < goalY - 5){
+		filip();
+		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 > goalY - 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;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	}
+  }
+   
+}
+#endif
+
+#if USE_PROCESS_NUM>7
+static void Process7()
+{
 	tapeLED.code = (uint32_t)Hotpink;
 	static bool Xnopush = false;
 	static bool Ynopush = false;
@@ -548,463 +1133,20 @@
 		motor[Angle_R].pwm = 255;
 	    motor[Angle_L].pwm = 255;
 	}
-
-}
-#endif
-
-#if USE_PROCESS_NUM>3
-static void Process3() // ロタコンX
-{
-	tapeLED.code = (uint32_t)Blue;
-	static bool nopushed = false; 
-	static bool Rt_flagX = false;
-	
-	if(controller->Button.A && !nopushed){
-		Rt_flagX = true;
-		nopushed = true;
-		}else if(!controller->Button.A)nopushed = false;
-
-	
-	if (Rt_flagX && SetPWM(RtpwmX) > 0){
-		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.72;
-		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
-	}
-	else if(Rt_flagX && SetPWM(RtpwmX) < 0)
-	{
-		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;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
-		
-	}
-	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;
-	}
-	//pc.printf("X:%d \r\n",RtpwmX);
 }
 #endif
 
-#if USE_PROCESS_NUM>4
-static void Process4() //ロタコンY
-{	
-	tapeLED.code = (uint32_t)Violet;
-	static bool nopushed = false; 
-	static bool Rt_flagY = false;
-	  /* wait(0.1);
-	   //RtX.getPulses();//...どちらの方向にどれだけ回ったか
-	   //RtY.getPulses();
-	   pc.printf("Pulses:%07d \r\n",RtX.getPulses());
-	   pc.printf("Pulses:%07d \r\n",RtY.getPulses());
-	   //軸が何回転したか
-	   pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
-	   */
-	
-	
-	if(controller->Button.B && !nopushed){
-		Rt_flagY = true;
-		nopushed = true;
-		}else if(!controller->Button.B)nopushed = false;
-
-	
-	if (Rt_flagY && SetPWM(RtpwmY) > 0){
-		filip();
-		
-		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(Rt_flagY && SetPWM(RtpwmY) < 0)
-	{
-		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;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
-		
-	}
-	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;
-	}
-
-	//pc.printf("RtpwmX:%f \r\n", RtpwmX);
-	
-	
-	//pc.printf("PWM:%d \r\n", RtpwmY);
-	//pc.printf("回転数:%f \r\n" ,rpmY);
-	//pc.printf("距離:%f \r\n", disY);
-}
-#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;
-	}else if(!controller->Button.A)nopushed = false;
-
-	if (Rt_flagX && SetPWM(RtpwmX) > 0){
-		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.72;
-		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
-		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
-	}
-	else if(Rt_flagX && SetPWM(RtpwmX) < 0){
-		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;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
-	}
-	
-	if(SetPWM(RtpwmX)== 0 && Rt_flagX){
-		wait(2);
-		
-		Rt_flagX = false;
-		Rt_flagY = true;
-	}
-	
-	if (Rt_flagY && SetPWM(RtpwmY) > 0){
-		filip();
-		
-		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(Rt_flagY && SetPWM(RtpwmY) < 0)
-	{
-		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;
-		motor[TIRE_FL].pwm = 255;
-		motor[TIRE_BR].pwm = 255;
-		motor[TIRE_BL].pwm = 255;
-		
-	}
-	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;
-	}
-}
-#endif
-
-#if USE_PROCESS_NUM>6
-static void Process6()
-{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>7
-static void Process7()
+#if USE_PROCESS_NUM>8 //kakudo
+static void Process8()
 {
 	
 }
 #endif
 
-#if USE_PROCESS_NUM>8
-static void Process8()
+#if USE_PROCESS_NUM>9
+static void Process9()
 {
-
-}
-#endif
-
-#if USE_PROCESS_NUM>9
-	static void Process9()
-	{
-	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;
-	
-	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)
-	{
-		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 = FOR;
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BR].dir = BACK;
-		motor[TIRE_BL].dir = BACK;
-		
-		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].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		
-		wait(2);
-		
-		yokofla = true;
-		traceon = false;
-		}
-		else{
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		}
-	}
-		
-	if(!traceon && yokofla && !boxslip)
-	 {
-		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 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;
-		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
-		{
-		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(!traceon && !yokofla && boxslip)
-	 {
-	 	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
-	 	{
-	 	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(!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[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
 #endif
@@ -1027,19 +1169,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();
@@ -1112,8 +1241,41 @@
     Color_D[2] = ColorIn(3);
 }
 
+void Color_changeflag(){
+    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;//茶
+	
+	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;//茶
+	
+	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;//茶
+	/*
+	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 getcolor(){
-	for(int i=0;i<20;i++){
+	for(int i=0;i<10;i++){
 		ColorDetection();
 		
 		Avecolor_A[0] += Color_A[0];
@@ -1130,22 +1292,21 @@
 		Avecolor_D[2] += Color_D[2];
 }
 
-Avecolor_A[0] = Avecolor_A[0]/20;
-Avecolor_A[1] = Avecolor_A[1]/20;
-Avecolor_A[2] = Avecolor_A[2]/20;
-Avecolor_B[0] = Avecolor_B[0]/20;
-Avecolor_B[1] = Avecolor_B[1]/20;
-Avecolor_B[2] = Avecolor_B[2]/20;
-Avecolor_C[0] = Avecolor_C[0]/20;
-Avecolor_C[1] = Avecolor_C[1]/20;
-Avecolor_C[2] = Avecolor_C[2]/20;
-Avecolor_D[0] = Avecolor_D[0]/20;
-Avecolor_D[1] = Avecolor_D[1]/20;
-Avecolor_D[2] = Avecolor_D[2]/20;
+Avecolor_A[0] = Avecolor_A[0]/10;
+Avecolor_A[1] = Avecolor_A[1]/10;
+Avecolor_A[2] = Avecolor_A[2]/10;
+Avecolor_B[0] = Avecolor_B[0]/10;
+Avecolor_B[1] = Avecolor_B[1]/10;
+Avecolor_B[2] = Avecolor_B[2]/10;
+Avecolor_C[0] = Avecolor_C[0]/10;
+Avecolor_C[1] = Avecolor_C[1]/10;
+Avecolor_C[2] = Avecolor_C[2]/10;
+Avecolor_D[0] = Avecolor_D[0]/10;
+Avecolor_D[1] = Avecolor_D[1]/10;
+Avecolor_D[2] = Avecolor_D[2]/10;
+
 }
 
-
-
 void BuzzerTimer_func() {
 	if(buzzer == 0.5){
 		buzzer = 0;