Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_neww

aaaaaaaaa

Dependencies:   QEI mbed

Fork of MainBoard2018_Auto_Master_A_new by Akihiro Nakabayashi

Diff: System/Process/Process.cpp

Revision:
12:c09b3e08a316
Parent:
11:028a150943b5
Child:
13:b6e02d6261d7
--- a/System/Process/Process.cpp	Sat Oct 06 08:30:58 2018 +0000
+++ b/System/Process/Process.cpp	Sun Oct 07 09:08:18 2018 +0000
@@ -42,22 +42,6 @@
 
 Serial pc(USBTX, USBRX);
 
-#define TIRE_FR 0 //足回り前右
-#define TIRE_FL 1 //足回り前左
-#define TIRE_BR 2 //足回り後右
-#define TIRE_BL 3 //足回り後左
-
-#define Angle_R 4 //角度調節右
-#define Angle_L 5 //角度調節左
-
-#define Lim_AR 3 //角度調節右
-#define Lim_AL 4 //角度調節左
-#define Lim_R  0 //センター右
-#define Lim_L  1 //センター左
-#define EMS_0  LimitSw::IsPressed(8) 
-#define EMS_1  LimitSw::IsPressed(9)
-#define LS     LimitSw::IsPressed(7) //赤ゾーン用スイッチ
-#define BS     LimitSw::IsPressed(6) //青ゾーン用スイッチ
 //************メカナム********************
 
 const int mecanum[15][15]=
@@ -103,24 +87,14 @@
 int Color_D[3];         
 int intergration = 50;
 
-unsigned long ColorIn(int index)
-{
-	int result = 0;
-	bool rtn = false;
-    for(int i=0; i<12; i++)
-    {
-        CK[index] = 1;
-        rtn = DOUT[index];
-        CK[index] = 0;
-        if(rtn)
-        {
-           result|=(1 << i);
-        }
-    }
-    return result;
-}
+int Avecolor_A[3];
+int Avecolor_B[3];
+int Avecolor_C[3];
+int Avecolor_D[3];
+
+void ColorIn();
 void ColorDetection();
-
+void getcolor();
 //************カラーセンサ********************
 
 //************ライントレース変数*******************
@@ -139,7 +113,6 @@
 Ticker Color_T;
 //************ライントレース変数*******************
 
-
 //************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);
@@ -157,12 +130,10 @@
 double goalX = 1200.000;
 double goalY = 900.000; 
 void filip();
-//PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
- 
+
 //************ROタコン****************** 
 
 //************ジャイロ*******************
-
 bool Angle_flagI = false;
 int Angle;
 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
@@ -170,7 +141,7 @@
 //初期値 -5
 int AngletargetX = 4; 
 int AngletargetY = -12;
-int Angle_I = -5;
+int AngletargetI = -5;
 //************ジャイロ*******************
 
 //************Buzzer******************
@@ -312,6 +283,12 @@
 
 	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
@@ -379,65 +356,16 @@
 #pragma region PROCESS
 #ifdef USE_SUBPROCESS
 #if USE_PROCESS_NUM>0
-static void Process0()
+static void Process0() 
 {	
-	static bool Xnopush = false;
-	static bool Angle_flagX = false;
-
-	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 gyropwmX = gyro.SetPV(Angle,AngletargetX);
-	
-	if(controller->Button.X && !Xnopush){
-		Angle_flagX = true;
-		Xnopush = true;
-	}else if(!controller->Button.X)Xnopush = false;	
-	
-	
-	if (Angle_flagX){
-		motor[Angle_R].dir = SetStatus(-gyropwmX);
-		motor[Angle_L].dir = SetStatus(gyropwmX);
-		motor[Angle_R].pwm = SetPWM(gyropwmX);
-		motor[Angle_L].pwm = SetPWM(gyropwmX);
-		if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-		Angle_flagX = false;
-		}
-	}
-	
-	else{
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-	}
+	tapeLED.code = (uint32_t)Green;
 }
 #endif
 
 #if USE_PROCESS_NUM>1
-static void Process1()
+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]); 
@@ -453,17 +381,408 @@
     motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
     }
     
+    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[Angle_R].pwm = 255;
+	    motor[Angle_L].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;
+	    motor[Angle_R].pwm = 255;
+	    motor[Angle_L].pwm = 255;
+	}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;
+	    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;
+	pc.printf("Y:%d \r\n",Angle);*/
     
-	   //wheel.getPulses()...どちらの方向にどれだけ回ったか
-	   //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
-	   //軸が何回転したか
-	   //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
+	//wheel.getPulses()...どちらの方向にどれだけ回ったか
+	//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+	//軸が何回転したか
+	//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
 }
 #endif
 
 #if USE_PROCESS_NUM>2
-static void Process2()
+static void Process2() //自動角度調節
+{	
+	tapeLED.code = (uint32_t)Hotpink;
+	static bool Xnopush = false;
+	static bool Ynopush = false;
+	static bool Inopush = false;
+	
+	static bool Angle_flagX = false;
+    static bool Angle_flagY = false;
+    static bool ANgle_flagI = false;
+    
+	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_R].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 gyropwmX = gyro.SetPV(Angle,AngletargetX);
+	int gyropwmY = gyro.SetPV(Angle,AngletargetY);
+	int gyropwmI = gyro.SetPV(Angle,AngletargetI);
+	
+	if(controller->Button.X && !Xnopush){
+		Angle_flagX = true;
+		Xnopush = true;
+	}else if(!controller->Button.X)Xnopush = false;	
+	
+	if(controller->Button.Y && !Ynopush){
+		Angle_flagY = true;
+		Ynopush = true;
+	}else if(!controller->Button.Y)Ynopush = false;
+	
+	if(controller->Button.A && !Inopush){
+		Angle_flagI = true;
+		Inopush = true;
+	}else if(!controller->Button.A)Inopush = false;
+	
+	if (Angle_flagX){
+		motor[Angle_R].dir = SetStatus(gyropwmX);
+		motor[Angle_L].dir = SetStatus(-gyropwmX);
+		motor[Angle_R].pwm = SetPWM(gyropwmX);
+		motor[Angle_L].pwm = SetPWM(gyropwmX);
+		if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		motor[Angle_R].pwm = 255;
+	    motor[Angle_L].pwm = 255;
+		Angle_flagX = false;
+		}
+	}
+	
+	if (Angle_flagY){
+		motor[Angle_R].dir = SetStatus(-gyropwmY);
+		motor[Angle_L].dir = SetStatus(gyropwmY);
+		motor[Angle_R].pwm = SetPWM(gyropwmY);
+		motor[Angle_L].pwm = SetPWM(gyropwmY);
+		if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		motor[Angle_R].pwm = 255;
+	    motor[Angle_L].pwm = 255;
+		Angle_flagY = false;
+		}
+	}
+	
+	if (Angle_flagI){
+		if(Angle < 0)
+		{
+			motor[Angle_R].dir = SetStatus(-gyropwmI);
+			motor[Angle_L].dir = SetStatus(gyropwmI);
+			motor[Angle_R].pwm = SetPWM(gyropwmI);
+			motor[Angle_L].pwm = SetPWM(gyropwmI);
+
+			if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
+			motor[Angle_R].dir = BRAKE;
+		 	motor[Angle_L].dir = BRAKE;
+			motor[Angle_R].pwm = 255;
+	    	motor[Angle_L].pwm = 255;
+			Angle_flagI = false;
+			}
+		}
+		else if(Angle > 0)
+		{   
+		   	motor[Angle_R].dir = FOR;
+			motor[Angle_L].dir = BACK;
+			motor[Angle_R].pwm = 150;
+			motor[Angle_L].pwm = 150;
+		   	
+		   	if(Angle < 0){
+				motor[Angle_R].dir = SetStatus(gyropwmI);
+				motor[Angle_L].dir = SetStatus(-gyropwmI);
+				motor[Angle_R].pwm = SetPWM(gyropwmI);
+				motor[Angle_L].pwm = SetPWM(gyropwmI);
+
+				if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
+				motor[Angle_R].dir = BRAKE;
+		 	 	motor[Angle_L].dir = BRAKE;
+				motor[Angle_R].pwm = 255;
+	    		motor[Angle_L].pwm = 255;
+				Angle_flagI = false;
+				}
+			}
+		}	
+	}
+	else{
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		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()
+{
+	
+}
+#endif
+
+#if USE_PROCESS_NUM>8
+static void Process8()
+{
+
+}
+#endif
+
+#if USE_PROCESS_NUM>9
+	static void Process9()
+	{
 	static bool color_flag = false;
 	
 	static bool traceon = false;//fase1
@@ -688,466 +1007,6 @@
 		}*///////
 }
 #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[Angle_R].pwm = 255;
-	    motor[Angle_L].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;
-	    motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-	}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;
-	    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;
-	pc.printf("Y:%d \r\n",Angle);
-}
-#endif
-
-#if USE_PROCESS_NUM>4
-static void Process4()
-{
-	static bool Xnopush = false;
-	static bool Ynopush = false;
-	
-	static bool Angle_flagX = false;
-    static bool Angle_flagY = false;
-    
-	
-	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_R].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 gyropwmX = gyro.SetPV(Angle,AngletargetX);
-	int gyropwmY = gyro.SetPV(Angle,AngletargetY);
-	
-	if(controller->Button.X && !Xnopush){
-		Angle_flagX = true;
-		Xnopush = true;
-	}else if(!controller->Button.X)Xnopush = false;	
-	
-	if(controller->Button.Y && !Ynopush){
-		Angle_flagY = true;
-		Ynopush = true;
-	}else if(!controller->Button.Y)Ynopush = false;
-	
-	if (Angle_flagX){
-		motor[Angle_R].dir = SetStatus(gyropwmX);
-		motor[Angle_L].dir = SetStatus(-gyropwmX);
-		motor[Angle_R].pwm = SetPWM(gyropwmX);
-		motor[Angle_L].pwm = SetPWM(gyropwmX);
-		if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-		Angle_flagX = false;
-		}
-	}
-	
-	if (Angle_flagY){
-		motor[Angle_R].dir = SetStatus(-gyropwmY);
-		motor[Angle_L].dir = SetStatus(gyropwmY);
-		motor[Angle_R].pwm = SetPWM(gyropwmY);
-		motor[Angle_L].pwm = SetPWM(gyropwmY);
-		if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-		Angle_flagY = false;
-		}
-	}
-	/*float y = 0;
-    y = acc[1]*1000;
-	float rotateY = (y - 305)/2.21 - 90;	
-	int gyropwm = gyro.SetPV(rotateY , Angletarget);
-	
-	if(controller->Button.X){
-		Angle_flag = true;
-	}
-	if (Angle_flag){
-		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(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		Angle_flag = false;
-		}
-	}*/
-	else{
-		motor[Angle_R].dir = BRAKE;
-		motor[Angle_L].dir = BRAKE;
-		motor[Angle_R].pwm = 255;
-	    motor[Angle_L].pwm = 255;
-	}
-	//pc.printf("%d \r\n",gyropwmY);
-}
-#endif
-
-#if USE_PROCESS_NUM>5
-static void Process5()
-{
-	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>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
-
-#if USE_PROCESS_NUM>7
-static void Process7()
-{
-	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);
-		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>8
-static void Process8()
-{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>9
-static void Process9()
-{
-
-}
-#endif
 #endif
 #pragma endregion PROCESS
 
@@ -1196,6 +1055,22 @@
 	RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
 }
 
+unsigned long ColorIn(int index)
+{
+	int result = 0;
+	bool rtn = false;
+    for(int i=0; i<12; i++)
+    {
+        CK[index] = 1;
+        rtn = DOUT[index];
+        CK[index] = 0;
+        if(rtn)
+        {
+           result|=(1 << i);
+        }
+    }
+    return result;
+}
 
 void ColorDetection(){
 	GATE = 0;
@@ -1236,6 +1111,41 @@
     wait_us(3);
     Color_D[2] = ColorIn(3);
 }
+
+void getcolor(){
+	for(int i=0;i<20;i++){
+		ColorDetection();
+		
+		Avecolor_A[0] += Color_A[0];
+		Avecolor_A[1] += Color_A[1];
+		Avecolor_A[2] += Color_A[2];
+		Avecolor_B[0] += Color_B[0];
+		Avecolor_B[1] += Color_B[1];
+		Avecolor_B[2] += Color_B[2];
+		Avecolor_C[0] += Color_C[0];
+		Avecolor_C[1] += Color_C[1];
+		Avecolor_C[2] += Color_C[2];
+		Avecolor_D[0] += Color_D[0];
+		Avecolor_D[1] += Color_D[1];
+		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;
+}
+
+
+
 void BuzzerTimer_func() {
 	if(buzzer == 0.5){
 		buzzer = 0;