Diff: System/Process/Process.cpp

Revision:

5:3ae504b88679

Parent:

3:e10d8736fd22

diff -r e10d8736fd22 -r 3ae504b88679 System/Process/Process.cpp
--- a/System/Process/Process.cpp	Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/Process.cpp	Mon Oct 01 09:07:27 2018 +0000
@@ -12,11 +12,14 @@
 #include "../../LED/LED.h"
 #include "../../Safty/Safty.h"
 #include "../Using.h"
+#include "../../Communication/PID/PID.h"
 
 
 using namespace SWITCH;
 using namespace COLORSENSOR;
 using namespace ACCELERATIONSENSOR;
+using namespace PID_SPACE;
+using namespace ROTARYENCODER;
 
 static CONTROLLER::ControllerData *controller;
 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -55,13 +58,20 @@
     return result;
 }
 
-#define TILE_FR 0 //足回り前右
-#define TILE_FL 1 //足回り前左
-#define TILE_BR 2 //足回り後右
-#define TILE_BL 3 //足回り後左
+#define TIRE_FR 0 //足回り前右
+#define TIRE_FL 1 //足回り前左
+#define TIRE_BR 2 //足回り後右
+#define TIRE_BL 3 //足回り後左
 
-#define Anguladjust_R 4 //角度調節右
-#define Anguladjust_L 5 //角度調節左
+#define Angle_R 4 //角度調節右
+#define Angle_L 5 //角度調節左
+
+#define Lim_AR 3 //角度調節右
+#define Lim_AL 4 //角度調節左
+#define Lim_R  0 //センター右
+#define Lim_L  1 //センター左
+
+//************メカナム********************
 
 const int mecanum[15][15]=
 {
@@ -96,44 +106,71 @@
 	else return abs(pwmVal);
 }
 
+//************メカナム********************
+
+//************カラーセンサ変数********************
 int Color_A[3]; //[赤,緑,青]
 int Color_B[3];
 int Color_C[3];
 int Color_D[3];         
 int intergration = 50;
-
-//************ライントレース変数*******************
-	int Point[3] = {234, 466, 590};//赤，緑，青
-	
-	int startP = 150;
-	int downP = 70;
-	
-	bool compA = false;
-	bool compB = false;
-	bool compC = false;
-	bool compD = false;
-	
-	bool invationA = false;
-	bool invationB = false;
-	bool invationC = false;
-	bool invationD = false;
-//************ライントレース変数*******************
-
-int averageR_0;
-int averageG_0;
-int averageB_0;
-int averageR_1;
-int averageG_1;
-int averageB_1;
-int averageR_2;
-int averageG_2;
-int averageB_2;
-int averageR_3;
-int averageG_3;
-int averageB_3;
+/*int averageR_A;
+int averageG_A;
+int averageB_A;
+int averageR_B;
+int averageG_B;
+int averageB_B;
+int averageR_C;
+int averageG_C;
+int averageB_C;
+int averageR_D;
+int averageG_D;
+int averageB_D;*/
 
 void ColorDetection();
 
+/*DigitalIn www(RT11_PIN);
+DigitalIn mmm(RT12_PIN);
+
+int pp = 0;
+int bb = 0;
+
+*/
+
+
+//************カラーセンサ変数********************
+
+//************ライントレース変数*******************
+int Point[3] = {234, 466, 590};//赤，緑，青
+int colorSN;
+int startP = 35;
+int downP = 5;
+
+bool Goal_flag = false;
+PID goal = PID(0.03,-255,255,0,0,0);	
+void GoalArrival();
+//************ライントレース変数*******************
+
+//************ジャイロ*******************
+//void AngleDetection();
+//void AngleControl();
+float AngleY;
+PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
+bool Angle_flag = false;
+float rotateY;
+int AngletargetX = 50;
+int AngletargetY = -50;
+int Angle_I;
+//************ジャイロ*******************
+
+//************ロタコン*******************
+int MemoRt;
+int Rt0;
+int Rt1;
+int Rt_A;
+int Rt_B;
+int PresentRt;
+//************ロタコン*******************
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
@@ -174,7 +211,6 @@
 	#pragma region USER-DEFINED_VARIABLE_INIT
 	
 	/*Replace here with the initialization code of your variables.*/
-
 	#pragma endregion USER-DEFINED_VARIABLE_INIT
 
 	lock = true;
@@ -256,22 +292,8 @@
 	SystemProcessInitialize();
 
 	while(1)
-	{        	
-	   	float x = 0, y= 0, z = 0;
-	   	
-	    pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
+	{
 	    
-	    x = acc[0]*1000;
-	    y = acc[1]*1000;
-	    z = acc[2]*1000;
-	    
-	    pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
-	    
-	    float rotateX = (x - 306)/2.22 - 90;
-	    float rotateY = (y - 305)/2.21 - 90;
-	    pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
-		wait_ms(50);
-
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -311,37 +333,87 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-	ColorDetection();
+	
+	if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].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;
+		AngleY += rotateY;
+	}
+	AngleY = AngleY /20;
+	int gyropwm = gyro.SetPV(AngleY,Angle_I);
+	
+	if(controller->Button.A){
+		Angle_flag = true;
+	}/*
+	if(controller->Button.Y){
+		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(Angle_I - 2 < AngleY && AngleY < Angle_I + 2){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		Angle_flag = false;
+		}
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
-    motor[0].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]) * 0.8;
-    motor[1].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]) * 0.8;
-    motor[2].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]) * 0.8; 
-    motor[3].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]) * 0.8;
+    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]); 
+    motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
          
-    motor[0].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
-   	motor[1].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
-    motor[2].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
-    motor[3].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
+    motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
+   	motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
+    motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
+    motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
                   
     if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
-    motor[0].pwm = motor[0].pwm * 1.3;
-    motor[1].pwm = motor[1].pwm * 1.3;
-    
+    motor[TIRE_FR].pwm = motor[0].pwm * 1.3;
+    motor[TIRE_FL].pwm = motor[1].pwm * 1.3;
     }
 }
 #endif
 
-bool buttoncomp = false;
 #if USE_PROCESS_NUM>2
 static void Process2()
 {
-	/*ColorDetection();
+	static bool color_flag = false;
+	static bool traceon = false;
+	
+	static bool yokofla = 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
@@ -349,21 +421,147 @@
 	}	
 	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
 	
-	if(controller->Button.A && buttoncomp = false)
+	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
 	{
-		motor[0].dir = dir;
-		motor[0].pwm = startP;
+	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(controller->Button.B && !color_flag)
+	{
+		traceon ^= 1;
+		color_flag = true;
+	}
+	else if(!controller->Button.B)
+	{
+		color_flag = false;
 	}
 	
-	if(invationA)
+	if(traceon)
 	{
-	   motor[0].PWM = startP
+		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;
+		
+			motor[0].pwm = 255;
+			motor[1].pwm = 255;
+			motor[2].pwm = 255;
+			motor[3].pwm = 255;
+		
+			wait(5);
+			yokofla = true;
+		}
+		else if(invationA && !compA && invationB && !compB && yokofla){
+		    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 - 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 && yokofla){
+			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(3) && LimitSw::IsPressed(4) && invationA && !compA && !invationB && !compB && yokofla && traceon){
+			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;
+		}
+		/*////
+		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;
+		}*//////
+		else 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 = 100;
+			motor[TIRE_FL].pwm = 100;
+			motor[TIRE_BR].pwm = 100;
+			motor[TIRE_BL].pwm = 100;
+		}else if(!LimitSw::IsPressed(3) && LimitSw::IsPressed(4)){
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_FR].pwm = 20;
+			motor[TIRE_BR].pwm = 20;
+		}else if(LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_FL].pwm = 20;
+			motor[TIRE_BL].pwm = 20;
+		}
+		 else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){
+			Goal_flag = true;
+			GoalArrival();			
+		}else{
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+		}
+	}
 }
 #endif
 
@@ -371,86 +569,146 @@
 static void Process3()
 {
 	if(controller->Button.R){
-	    motor[4].dir = FOR;
-	    motor[5].dir = BACK;
-	    motor[4].pwm = 150;
-	    motor[5].pwm = 150;
+	    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[4].dir = BACK;
-	    motor[5].dir = FOR;
-	    motor[4].pwm = 150;
-	    motor[5].pwm = 150;
+	    motor[Angle_R].dir = BACK;
+	    motor[Angle_L].dir = FOR;
+	    motor[Angle_R].pwm = 150;
+	    motor[Angle_L].pwm = 150;
 	}else{
-	    motor[4].dir = BRAKE;
-	    motor[5].dir = BRAKE;
-	}  
-	if(LimitSw::IsPressed(0)){
-	    motor[4].dir = BRAKE;
-	    motor[5].dir = BRAKE;
-	}else if(LimitSw::IsPressed(1)){
-	    motor[4].dir = BRAKE;
-	    motor[5].dir = BRAKE;
-	} 
+		motor[Angle_R].dir = BRAKE;
+	    motor[Angle_L].dir = BRAKE;
+	}
+
+	if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].dir == FOR){
+	    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>4
 static void Process4()
 {
-	//ColorDetection();
-	
-    for(int i=0;i<=10;i++)
+ /*   for(int i=0;i<10;i++)
     {
     	ColorDetection();
     	
-    	averageR_0 += Color_A[0];
-    	averageG_0 += Color_A[1];
-    	averageB_0 += Color_A[2];
-    	averageR_1 += Color_B[0];
-    	averageG_1 += Color_B[1];
-    	averageB_1 += Color_B[2];
-    	averageR_2 += Color_C[0];
-    	averageG_2 += Color_C[1];
-    	averageB_2 += Color_C[2];
-    	averageR_3 += Color_D[0];
-    	averageG_3 += Color_D[1];
-    	averageB_3 += Color_D[2];
+    	averageR_A += Color_A[0];
+    	averageG_A += Color_A[1];
+    	averageB_A += Color_A[2];
+    	averageR_B += Color_B[0];
+    	averageG_B += Color_B[1];
+    	averageB_B += Color_B[2];
+    	averageR_C += Color_C[0];
+    	averageG_C += Color_C[1];
+    	averageB_C += Color_C[2];
+    	averageR_D += Color_D[0];
+    	averageG_D += Color_D[1];
+    	averageB_D += Color_D[2];
     }
-    pc.printf("AR_0:%d, AG_0:%d ,AB_0:%d \r\n",averageR_0 / 10 ,averageG_0 / 10, averageB_0 / 10);
-    pc.printf("AR_1:%d, AG_1:%d ,AB_1:%d \r\n",averageR_1 / 10 ,averageG_1 / 10, averageB_1 / 10);
-    pc.printf("AR_2:%d, AG_2:%d ,AB_2:%d \r\n",averageR_2 / 10 ,averageG_2 / 10, averageB_2 / 10);
-    pc.printf("AR_3:%d, AG_3:%d ,AB_3:%d \r\n",averageR_3 / 10 ,averageG_3 / 10, averageB_3 / 10);
+    pc.printf("AR_A:%d, AG_A:%d ,AB_A:%d \r\n",averageR_A / 10 ,averageG_A / 10, averageB_A / 10);
+    pc.printf("AR_B:%d, AG_B:%d ,AB_B:%d \r\n",averageR_B / 10 ,averageG_B / 10, averageB_B / 10);
+    pc.printf("AR_C:%d, AG_C:%d ,AB_C:%d \r\n",averageR_C / 10 ,averageG_C / 10, averageB_C / 10);
+    pc.printf("AR_D:%d, AG_D:%d ,AB_D:%d \r\n",averageR_D / 10 ,averageG_D / 10, averageB_D / 10);
 	
-	averageR_0 = 0;
-    averageG_0 = 0;
-	averageB_0 = 0;
-	averageR_1 = 0;
-    averageG_1 = 0;
-	averageB_1 = 0;
-	averageR_2 = 0;
-    averageG_2 = 0;
-	averageB_2 = 0;
-	averageR_3 = 0;
-    averageG_3 = 0;
-	averageB_3 = 0;
+	averageR_A = 0;
+    averageG_A = 0;
+	averageB_A = 0;
+	averageR_B = 0;
+    averageG_B = 0;
+	averageB_B = 0;
+	averageR_C = 0;
+    averageG_C = 0;
+	averageB_C = 0;
+	averageR_D = 0;
+    averageG_D = 0;
+	averageB_D = 0;*/
 }
 #endif
 
 #if USE_PROCESS_NUM>5
 static void Process5()
-{
-	pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
-	//int rotateX = (acc[0].read()-)/ -90;
-	//int rotateY = (acc[1].read()-)/ -90;
-	//pc.printf("X:%d ,Y:%d", rotateX, rotateY);
-	wait_ms(50);
+{	
+	if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].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;
+		AngleY += rotateY;
+	}
+	AngleY = AngleY /20;
+	int gyropwm = gyro.SetPV(AngleY,AngletargetY);
+	
+	if(controller->Button.X){
+		Angle_flag = true;
+	}/*
+	if(controller->Button.Y){
+		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(AngletargetY - 2 < AngleY && AngleY < AngletargetY + 2){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		Angle_flag = 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;
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>6
 static void Process6()
-{
-
+{		
+ //void Int::Initialize(BoardRtInt[0].fall(int2));
 }
 #endif
 
@@ -515,17 +773,11 @@
     wait_us(3);
     Color_A[2] = ColorIn(0); //緑
         
-    //pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d",Color_A[0],Color_A[1],Color_A[2]);
-    //pc.printf("\r\n");
-        
     Color_B[0] = ColorIn(1);
     wait_us(3);
     Color_B[1] = ColorIn(1);
     wait_us(3);
     Color_B[2] = ColorIn(1);
-        
-    //pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d",Color_B[0],Color_B[1],Color_B[2]);
-    //pc.printf("\r\n");
          
     Color_C[0] = ColorIn(2);
     wait_us(3);
@@ -533,16 +785,94 @@
     wait_us(3);
     Color_C[2] = ColorIn(2);
         
-    /*pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d",Color_C[0],Color_C[1],Color_C[2]);
-    pc.printf("\r\n");*/
-         
     Color_D[0] = ColorIn(3);
     wait_us(3);
     Color_D[1] = ColorIn(3);
     wait_us(3);
     Color_D[2] = ColorIn(3);
-        
-    /*pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d",Color_D[0],Color_D[1],Color_D[2]);
-    pc.printf("\r\n");*/
 }
+/*void AngleDetection(){
+	float x = 0, y= 0, z = 0;
+	    
+    //x = acc[0]*1000;
+    y = acc[1]*1000;
+	//z = acc[2]*1000;
+	//pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
+	//float rotateX = (x - 306)/2.22 - 90;
+	float rotateY = (y - 305)/2.21 - 90;
+	//float rotateZ = (z - 300)/1.18 - 90;
+	//pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
+}*/
+/*void AngleControl(){	
+	int Angletarget = -50;
+	float AnglevalueY = rotateY;  
+	int gyropwm = gyro.SetPV(AnglevalueY , Angletarget);
+			
+	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 < AnglevalueY && AnglevalueY < 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;
+	}	 
+	//pc.printf("PWM:%d \r\n" , gyropwm);
+}*/
+void GoalArrival(){	
+	int Goaltarget = 0;
+	float Goalvalue = 0;  
+	int goalpwm = goal.SetPV(Goalvalue,Goaltarget);
+	
+	if (Goal_flag){
+		motor[TIRE_FR].dir = SetStatus(goalpwm);
+		motor[TIRE_FL].dir = SetStatus(-goalpwm);
+		motor[TIRE_BR].dir = SetStatus(goalpwm);
+		motor[TIRE_BL].dir = SetStatus(-goalpwm);
+		motor[TIRE_FR].pwm = SetPWM(goalpwm);
+		motor[TIRE_FL].pwm = SetPWM(goalpwm);
+		motor[TIRE_BR].pwm = SetPWM(goalpwm);
+		motor[TIRE_BL].pwm = SetPWM(goalpwm);
+		if(Goaltarget > Goalvalue - 10 && Goaltarget < Goalvalue+ 10){
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		Goal_flag = false;
+		}
+	}else{
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+	}	 
+}
+/*
+void RotaryD(uint8_t, uint8_t){
+	uint8_t D;
+	MemoRt = Rt1;
+	Rt0 = Rt0 << 1;
+	Rt0 = Rt0 & 3;
+	Rt1 = Rt1 & 3;
+	D = (Rt0 + Rt1) & 3;
+	//return (D);
+}
+void GetRt(){
+	uint8_t DJ;
+	double rad = 0;
+	PresentRt = 0x00;
+	if(LimitSw::IsPressed(Rt_A))
+		PresentRt = 0x02;
+	if(LimitSw::IsPressed(Rt_B))
+		PresentRt = 0x01;
+	if(MemoRt != PresentRt)
+		DJ = RotaryD(MemoRt,PresentRt);
+	if(D>=2)rad = 360.0 /50.0;
+	else rad = -(360.0/50.0);
+	Rt = Rt + rad /360.0*20;
+}*/
+
 #pragma endregion