Diff: System/Process/Process.cpp

Revision:

25:b3a9f34b201d

Parent:

24:370616a56815

Child:

26:4c0ce2f05688

diff -r 370616a56815 -r b3a9f34b201d System/Process/Process.cpp
--- a/System/Process/Process.cpp	Wed Sep 18 02:03:56 2019 +0000
+++ b/System/Process/Process.cpp	Fri Sep 27 07:51:32 2019 +0000
@@ -1,4 +1,3 @@
-
 #include "mbed.h"
 #include "Process.h"
 
@@ -141,14 +140,22 @@
 float pw = 0;
 int lineFase = 0;
 bool lineCheck = false;
+bool lineFlag  = false;
+bool adjAnable = false;
+int adj = 0;
+
+int count = 0;
+int lineCount = 0;
+int targetCount = 0;
+
+bool startFlag = true;
+
 int linePWM;
 int adj_F;
 int adj_B;
 
 int mode = 0;
 
-int lineCount = 0;
-
 // ************* Line ************** //
 
 const int omni[15][15] =
@@ -224,7 +231,12 @@
 {
 	#pragma region USER-DEFINED_VARIABLE_INIT
 	/*Replace here with the initialization code of your variables.*/	
-	rotaconPIDtimer.attach(tirePID,0.1);
+	//rotaconPIDtimer.attach(tirePID,0.1);
+	
+	//DigitalOut  Air_16(LS_16);
+	//DigitalOut  Air_17(LS_17);
+	//DigitalOut  Air_18(LS_18);
+	//DigitalOut  Air_19(LS_19);
 	
 	#pragma endregion USER-DEFINED_VARIABLE_INIT
 
@@ -315,13 +327,24 @@
 		}
 		
 		pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
-		
-		//float a = ultrasonic[0].ReadDis();
-		//pc.printf("%f\n\r",a);
-		
-		//int ppap = encoder[0].getPulses();
-		//pc.printf("%d\n\r",ppap);
-		
+
+
+		if(LimitSw::IsPressed(START_SW) && startFlag == true) {
+			startFlag == false;
+			lock = false;
+			lineFase = 0;
+			lineCount = 0;
+			lineCheck = false;
+			count = 0;
+			/*
+			if(LimitSw::IsPressed(ISREDBLUE_SW) {
+				current = 4;
+			} else {
+				current = 5;
+			}
+			*/
+			current = 4;
+		}
 		
 		buzzer.period(1.0/800);
 		
@@ -330,7 +353,7 @@
 		#endif
 
 		#ifdef USE_ERRORCHECK
-		if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
+		if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
 		{
 			CONTROLLER::Controller::DataReset();
 			AllActuatorReset();
@@ -406,10 +429,10 @@
     	motor[LIFT_RB].dir = BACK;
     	motor[LIFT_RB].pwm = 180;
     } else {
-    	motor[LIFT_LB].dir = FREE;
+    	motor[LIFT_LB].dir = BRAKE;
     	motor[LIFT_LB].pwm = 255;
     	motor[LIFT_RB].dir = BACK;
-    	motor[LIFT_RB].pwm = 10;
+    	motor[LIFT_RB].pwm = 20;
     }
     
     
@@ -418,39 +441,49 @@
     	motor[LIFT_U].pwm = 180;
     } else if(controller->Button.Y) {
     	motor[LIFT_U].dir = BACK;
-    	motor[LIFT_U].pwm = 180;
+    	motor[LIFT_U].pwm = 230;
     } else {
     	motor[LIFT_U].dir = BRAKE;
-    	motor[LIFT_U].pwm = 180;
+    	motor[LIFT_U].pwm = 255;
     }
 
 	if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
-	    motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     );
+	    motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     );
 		motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         );
 		motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  );
-		motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      );
+		motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      );
 	   
-		motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
+		motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
 		motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2)    ;
 		motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
-		motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2)   ;	
+		motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2)   ;	
 	} else {
-		motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
+		motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
 		motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
 		motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
-		motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
+		motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
 	   
 		motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
 		motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
 		motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
 		motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
 	}
+	
+	if(controller->Button.ZR) {
+		solenoid.solenoid4 = SOLENOID_ON;
+	} else {
+		solenoid.solenoid4 = SOLENOID_OFF;
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>2
 static void Process2()
 {	
+
+	
+
+
 /*
 	if(moving) {
 		if(LimiSw::IsPressed(LSW_LB)) {
@@ -504,116 +537,1109 @@
 #if USE_PROCESS_NUM>3
 static void Process3() 
 {
+	startFlag = true;
 	AllActuatorReset();
 	lineFase = 0;
+	lineCheck = false;
+	lineCount = 0;
+	count = 0;
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4() 
 {
+	LED_DEBUG0 = LED_ON;
 	
-	static int x,y;
-	static int count = 0;
+	/* ************************************** //
+	
+		  赤ゾーン	  赤ゾーン	  赤ゾーン
+	
+	// ************************************** */
+	
+	
+	for(int i = 0; i < 8; i++) {
+		linePara[i] = lineCast(LineHub::GetPara(i));
+	}
 	
-	linePara_U = lineCast(LineHub::GetPara(0));
-	linePara_B = lineCast(LineHub::GetPara(2));
-	linePara_L = lineCast(LineHub::GetPara(3));
-	linePara_R = lineCast(LineHub::GetPara(4));
+	if(lineFase == 0) {
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		if(linePara[0] != 'N' && linePara[0] != 'A') {
+			lineFase = 1;
+		}
+		motor[TIRE_FL].pwm = 30;
+		motor[TIRE_BL].pwm = 30;
+		motor[TIRE_BR].pwm = 30;
+		motor[TIRE_FR].pwm = 30;
 	
-	if(linePara_B == 'A' && count == 0) {
-		lineFase = 1;
-	}
-
-	if(lineFase == 0) {
-		pw = 0.5;
-		switch(linePara_U) {
+	} else if(lineFase == 1) {  // 前 ライントレース
+		switch(linePara[0]) {
 			case -2:
-				x = 5;
-				y = 3;
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 10;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -10;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 10;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -10;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+				tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+				tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+				tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 3) {
+			count = 0;
+			lineFase = 2;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 2) {  // 前 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		if(linePara[4] == 0) {
+			lineFase = 3;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16;
+	} else if(lineFase == 3) {  // 右 ライントレース
+		switch(linePara[4]) {
+			case -2:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
 				break;
 			case -3:
-				x = 5;
-				y = 3;
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -10;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 10;
+				adjAnable = true;
 				break;
 			case -1:
-				x = 6;
-				y = 3;
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -20;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 20;
+				adjAnable = true;
 				break;
 			case 0:
-				x = 7;
-				y = 3;
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
 				break;
 			case 1:
-				x = 8;
-				y = 3;
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
 				break;
 			case 3:
-				x = 9;
-				y = 3;
+				tirePWM[TIRE_FL] = 10;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -10;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
 				break;
 			case 2:
-				x = 9;
-				y = 3;
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
 				break;
 			case 'A':
-				lineCheck = true;
-				x = x;
-				y = y;
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		
+		if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
+			targetCount = 3;
+		} else if(LimitSw::IsPressed(TOWEL1_SW)) {
+			targetCount = 3;
+		} else {
+			targetCount = 2;
+		}
+		
+		if(count == targetCount) {
+			count = 0;
+			lineFase = 4;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 4) { // 右 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = FOR;
+		if (linePara[LINE_TOW_1] == 0) {
+			lineFase = 6;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 14;
+		motor[TIRE_BL].pwm = 14;
+		motor[TIRE_BR].pwm = 14;
+		motor[TIRE_FR].pwm = 14;
+	} else if (lineFase == 5) {
+		lineFase = 6;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+	} else if(lineFase == 6) {  // タオル1 検知
+		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
+			lineFase = 7;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+	} else if(LimitSw::IsPressed(TOW_1L)) { 
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BL].pwm = 50;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(LimitSw::IsPressed(TOW_1R)) { 
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		}
+	} else if(lineFase == 7) {  // ライン 修正
+		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else if(linePara[LINE_TOW_1] > 0) {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = FOR;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_1] < 0) {
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = BACK;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_1] == 0) {
+			lineFase = 8;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		}
+	} else if(lineFase == 8) { // タオル1 解放
+		//solenoid.TOWEL1 = SOLENOID_ON;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+		lineFase = 9;
+	} else if(lineFase == 9) {  // 前
+		switch(linePara[LINE_TOW_1]) {
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 10;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -10;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 10;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -10;
+				adjAnable = true;
+				break;
+			case -2:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
 				break;
 			case 'N':
-				x = 7;
-				y = 7;
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			adj = 0;
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 1) {
+			count = 0;
+			lineFase = 10;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 10) {
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		if(linePara[4] == 0) {
+			lineFase = 11;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16; 
+	} else if(lineFase == 11) {
+		switch(linePara[4]) {
+			case -2:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
 				break;
-				x = 7;
-				y = 7;
+			case -3:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -10;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 10;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -20;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 20;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 10;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -10;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
 			default:
-				x = 9;
-				y = 9;
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
 		}
-		if(lineCheck == true && (!(linePara_U) == 'A')) {
-			count++;
+		
+		if(adjAnable){
+			adj = 0;//linePara[3];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 1) {
+			count = 0;
+			lineFase = 12;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 12) { // 右 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = FOR;
+		if(linePara[LINE_TOW_2] == 0) {
+			lineFase = 13;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
 		}
-	} else if(lineFase == 1) {
-		pw = 0.3;
-			x = 7;
-			y = 9;
-		if(linePara_R == 0) {
-			lineFase = 2;
-			x = 7;
-			y = 7;
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16;
+	} else if (lineFase == 13) {
+		lineFase = 14;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+	} else if(lineFase == 14) {  // タオル2 竿検知
+		if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
+			lineFase = 15;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else if(LimitSw::IsPressed(TOW_2L)) { 
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(LimitSw::IsPressed(TOW_2R)) { 
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		}
+	} else if(lineFase == 15 ){  // ライン 修正
+		if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else if(linePara[LINE_TOW_2] > 0) {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = FOR;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_2] < 0) {
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = BACK;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_2] == 0) {
+			lineFase = 16;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
 		}
-	} else if(lineFase == 2) {
-		x = 7;
-		y = 7;
-	} else {
-		x = 7;
-		y = 7;
-	}
-	
-	int t = 0;
-	if((linePara_U + linePara_B) > 3) t = 1;
-	
-	if(controller->Button.A) {
-		motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] );
-		motor[TIRE_FL].dir = SetStatus(omni[y][x]);
-		motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]);
-		motor[TIRE_FR].dir = SetStatus(omni[x][14-y]);
-		   
-		motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw;
-		motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw;
-		motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw;
-		motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw;	
-	} else {
-		motor[TIRE_BL].dir = SetStatus(0);
-		motor[TIRE_FL].dir = SetStatus(0);
-		motor[TIRE_BR].dir = SetStatus(0);
-		motor[TIRE_FR].dir = SetStatus(0);
-		   
-		motor[TIRE_FR].pwm = SetPWM(0);
-		motor[TIRE_FL].pwm = SetPWM(0);
-		motor[TIRE_BR].pwm = SetPWM(0);
-		motor[TIRE_BL].pwm = SetPWM(0);	
+	} else if(lineFase == 16) { // タオル2 解放
+		//solenoid.TOWEL2 = SOLENOID_ON;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+		lineFase = 17;
+	} else if(lineFase == 17) { // 前
+		switch(linePara[LINE_TOW_2]) {
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 10;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -10;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 10;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -10;
+				adjAnable = true;
+				break;
+			case -2:
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			adj = 0;
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 1) {
+			count = 0;
+			lineFase = 18;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 18) {  // 前 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		if(linePara[4] != 'N') {
+			lineFase = 19;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16;
+	} else if(lineFase == 19) {  // 左
+		switch(linePara[3]) {
+			case -2:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 10;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = -10;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = 30
+				tirePWM[TIRE_FR] = -2;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = -20;
+				tirePWM[TIRE_BL] = 40;
+				tirePWM[TIRE_BR] = 20;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = -10;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 10;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 30;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = -30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		
+		/*
+		if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
+			targetCount = 3;
+		} else if(LimitSw::IsPressed(TOWEL1_SW)) {
+			targetCount = 3;
+		} else {
+			targetCount = 2;
+		}
+		*/
+		
+		targetCount = 3;
+		
+		if(count == targetCount) {
+			count = 0;
+			lineFase = 20;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 20) { // 左 低速
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_FR].dir = BACK;
+		if (linePara[2] == 'N') {
+			//(LimitSw::IsPressed(SHEETS_SW)) {
+			//ineFase = 20; 
+			//else if(LimitSw::IsPressed(TOWEL2_SW) {
+			//ineFase = 14;
+			//else {
+			lineFase = 21;
+			//
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16;
+	} else if(lineFase == 21) {
+		switch(linePara[2]) {
+			case -2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = -10;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 10;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = -20;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 20;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = -20;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 20;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = -10;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 10;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = -30;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = 30;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 3) {
+			count = 0;
+			lineFase = 22;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 22) {
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_FR].dir = BACK;
+		if (linePara[2] == 'N') {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 30;
+		motor[TIRE_BL].pwm = 30;
+		motor[TIRE_BR].pwm = 30;
+		motor[TIRE_FR].pwm = 30;
+	} else{
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
 	}
 	
 }
@@ -622,158 +1648,719 @@
 #if USE_PROCESS_NUM>5
 static void Process5() 
 {	
-	lineFase = 0;
-	lineCheck = true;
+	
+	/* ************************************** //
+	
+		  青ゾーン	  青ゾーン	  青ゾーン
+	
+	// ************************************** */
+	
+	for(int i = 0; i < 8; i++) {
+		linePara[i] = lineCast(LineHub::GetPara(i));
+	}
+	
+	if(lineFase == 0) {
+ 		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_FR].dir = FOR;
+		if(linePara[0] != 'N' && linePara[0] != 'A') {
+			lineFase = 1;
+		}
+		motor[TIRE_FL].pwm = 20;
+		motor[TIRE_BL].pwm = 20;
+		motor[TIRE_BR].pwm = 20;
+		motor[TIRE_FR].pwm = 20;
+	}else if(lineFase == 1) {  // 前 ライントレース
+		switch(linePara[0]) {
+			case -2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -30;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 40;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -40;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]) * 0.8;
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj) * 0.8;
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj) * 0.8;
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]) * 0.8;	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 3) {
+			count = 0;
+			lineFase = 2;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 2) {  // 前 低速
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_FR].dir = FOR;
+		if(linePara[3] != 'N') {
+			lineFase = 3;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 20;
+		motor[TIRE_BL].pwm = 20;
+		motor[TIRE_BR].pwm = 20;
+		motor[TIRE_FR].pwm = 20;
+	} else if(lineFase == 3) {  // 左 ライントレース
+		switch(linePara[4]) {
+			case -2:
+				tirePWM[TIRE_FL] = -50;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 50;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = -50;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = 50;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = -50;
+				tirePWM[TIRE_BL] = 40;
+				tirePWM[TIRE_BR] = 50;
+				tirePWM[TIRE_FR] = -40;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = -50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = -40;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 40;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = -20;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 20;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = -50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 2) {
+			count = 0;
+			lineFase = 4;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 4) { // 右 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = FOR;
+		if(linePara[LINE_TOW_1] == 0) {
+			lineFase = 5;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 20;
+		motor[TIRE_BL].pwm = 20;
+		motor[TIRE_BR].pwm = 20;
+		motor[TIRE_FR].pwm = 20;
+	} else if (lineFase == 5) {
+		lineFase = 6;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+	} else if(lineFase == 6) {  // タオル 竿検知
+		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
+			lineFase = 7;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else if(LimitSw::IsPressed(TOW_1L)) { 
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(LimitSw::IsPressed(TOW_1R)) { 
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 20;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 20;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 20;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 20;
+		}
+	} else if(lineFase == 7) {  // ライン 修正
+		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else if(linePara[LINE_TOW_1] > 0) {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = FOR;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_1] < 0) {
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 16;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 16;
+			motor[TIRE_BR].dir = BACK;
+			motor[TIRE_BR].pwm = 16;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 16;
+		} else if(linePara[LINE_TOW_1] == 0) {
+			lineFase = 8;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 50;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 50;
+		} else {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 30;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 30;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 30;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 30;
+		}
+	} else if(lineFase == 8) { // タオル1 解放
+		//solenoid.TOWEL1 = SOLENOID_ON;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+		lineFase = 9;
+	} else if(lineFase == 9) {  // 前
+		switch(linePara[LINE_TOW_1]) {
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 40;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -40;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 40;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -40;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 20;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -20;
+				adjAnable = true;
+				break;
+			case -2:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = -50;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			adj = 0;
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 1) {
+			count = 0;
+			lineFase = 10;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 10) {
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = BACK;
+		if(linePara[4] == 0) {
+			lineFase = 11;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 20;
+		motor[TIRE_BL].pwm = 20;
+		motor[TIRE_BR].pwm = 20;
+		motor[TIRE_FR].pwm = 20; 
+	} else if(lineFase == 11) {
+		switch(linePara[4]) {
+			case -2:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = true;
+				break;
+			case -3:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = -30;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 30;
+				adjAnable = true;
+				break;
+			case -1:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = -40;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 40;
+				adjAnable = true;
+				break;
+			case 0:
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = -50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 50;
+				adjAnable = true;
+				break;
+			case 1:
+				tirePWM[TIRE_FL] = 40;
+				tirePWM[TIRE_BL] = -50;
+				tirePWM[TIRE_BR] = -40;
+				tirePWM[TIRE_FR] = 50;
+				adjAnable = true;
+				break;
+			case 3:
+				tirePWM[TIRE_FL] = 20;
+				tirePWM[TIRE_BL] = -50;
+				tirePWM[TIRE_BR] = -20;
+				tirePWM[TIRE_FR] = 50;
+				adjAnable = true;
+				break;
+			case 2:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = -50;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 50;
+				adjAnable = true;
+				break;
+			case 'A':
+				if(lineCheck == false) {
+					lineCheck = true;
+					lineCount = 0;
+					count++;
+				}
+				tirePWM[TIRE_FL] = 50;
+				tirePWM[TIRE_BL] = -50;
+				tirePWM[TIRE_BR] = -50;
+				tirePWM[TIRE_FR] = 50;
+				adjAnable = true;
+				break;
+			case 'N':
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0.;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+				break;
+			default:
+				tirePWM[TIRE_FL] = 0;
+				tirePWM[TIRE_BL] = 0;
+				tirePWM[TIRE_BR] = 0;
+				tirePWM[TIRE_FR] = 0;
+				adjAnable = false;
+		}
+		
+		if(adjAnable){
+			adj = 0;//linePara[3];
+		} else {
+			adj = 0;
+		}
+		
+		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);	
+		
+		if(lineCheck == true)  {
+			lineCount++;
+			if(lineCount > 20) lineCheck = false;
+		}
+		if(count == 3) {
+			count = 0;
+			lineFase = 12;
+			lineCount = 0;
+			lineCheck = false;
+		}
+	} else if(lineFase == 12) { // 右 低速
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_FR].dir = FOR;
+		if(linePara[LINE_TOW_2] == 0) {
+			lineFase = 13;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_FR].dir = BRAKE;
+		}
+		motor[TIRE_FL].pwm = 16;
+		motor[TIRE_BL].pwm = 16;
+		motor[TIRE_BR].pwm = 16;
+		motor[TIRE_FR].pwm = 16;
+	} else if (lineFase == 13) {
+		lineFase = 14;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+	} else {
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_FR].dir = BRAKE;
+	}
+	
 }
 #endif
 
 #if USE_PROCESS_NUM>6
 static void Process6() 
 {
-	
-	for(int i = 0; i < 8; i++){
+	for(int i = 0; i < 8; i++) {
 		linePara[i] = lineCast(LineHub::GetPara(i));
 	}
 	
-	static int count = 100000;
-	count++;
-	
-	if(count < 10000) {
-		lineCheck = false;
-	} else {
-		lineCheck = true;
+	if(lineFase == 0) {
+		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
+			lineFase = 1;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 30;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 30;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 30;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 30;
+		} else if(LimitSw::IsPressed(TOW_1L)) { 
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 12;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 12;
+		} else if(LimitSw::IsPressed(TOW_1R)) { 
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 12;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 50;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 50;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 12;
+		} else {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 18;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 18;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 18;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 18;
+		}
+	} else if(lineFase == 1) {
+		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 30;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 30;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 30;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 30;
+		} else if(linePara[LINE_TOW_1] > 0) {
+			motor[TIRE_FL].dir = BACK;
+			motor[TIRE_FL].pwm = 15;
+			motor[TIRE_BL].dir = FOR;
+			motor[TIRE_BL].pwm = 15;
+			motor[TIRE_BR].dir = FOR;
+			motor[TIRE_BR].pwm = 15;
+			motor[TIRE_FR].dir = BACK;
+			motor[TIRE_FR].pwm = 15;
+		} else if(linePara[LINE_TOW_1] < 0) {
+			motor[TIRE_FL].dir = FOR;
+			motor[TIRE_FL].pwm = 15;
+			motor[TIRE_BL].dir = BACK;
+			motor[TIRE_BL].pwm = 15;
+			motor[TIRE_BR].dir = BACK;
+			motor[TIRE_BR].pwm = 15;
+			motor[TIRE_FR].dir = FOR;
+			motor[TIRE_FR].pwm = 15;
+		} else if(linePara[LINE_TOW_1] == 0) {
+			lineFase = 2;
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 30;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 30;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 30;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 30;
+		} else {
+			motor[TIRE_FL].dir = BRAKE;
+			motor[TIRE_FL].pwm = 30;
+			motor[TIRE_BL].dir = BRAKE;
+			motor[TIRE_BL].pwm = 30;
+			motor[TIRE_BR].dir = BRAKE;
+			motor[TIRE_BR].pwm = 30;
+			motor[TIRE_FR].dir = BRAKE;
+			motor[TIRE_FR].pwm = 30;
+		}
+	} else if(lineFase == 2) {
+		//solenoid::solenoid1 = SOLENOID_ON
+		
 	}
-	
-	if(lineFase == 0) {  // 前進 
-		switch(linePara[0]) {
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BRAKE;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = BRAKE;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 0;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 0;
-				break;
-			case -3:
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BACK;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = FOR;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 10;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 10;
-				break;
-			case -1:
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BACK;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = FOR;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 20;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 20;
-				break;
-			case 0:
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BACK;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = FOR;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 30;
-				break;
-			case 1:
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BACK;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = FOR;
-				motor[TIRE_FL].pwm = 20;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 20;
-				motor[TIRE_BL].pwm = 30;
-				break;
-			case 3:
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = FOR;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = BACK;
-				motor[TIRE_FL].pwm = 10;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 10;
-				motor[TIRE_BL].pwm = 30;
-				break;
-			case 2:
-				motor[TIRE_FL].dir = BRAKE;
-				motor[TIRE_BL].dir = FOR;
-				motor[TIRE_BR].dir = BRAKE;
-				motor[TIRE_FR].dir = BACK;
-				motor[TIRE_FL].pwm = 0;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 0;
-				motor[TIRE_BL].pwm = 30;
-				break;
-			case 'A':
-				motor[TIRE_FL].dir = FOR;
-				motor[TIRE_BL].dir = BACK;
-				motor[TIRE_BR].dir = BACK;
-				motor[TIRE_FR].dir = FOR;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 30;
-				if(lineCheck == true) {
-					lineCount++;
-					count = 0;
-				}
-			default:
-				motor[TIRE_FL].dir = BRAKE;
-				motor[TIRE_BL].dir = BRAKE;
-				motor[TIRE_BR].dir = BRAKE;
-				motor[TIRE_FR].dir = BRAKE;
-				motor[TIRE_FL].pwm = 30;
-				motor[TIRE_FR].pwm = 30;
-				motor[TIRE_BR].pwm = 30;
-				motor[TIRE_BL].pwm = 30;
-		}
-		if(lineCount == 1) {
-			lineFase = 1;
-		}
-	} else if(lineFase == 1) {  // 前進 低速
-		motor[TIRE_FL].dir = FOR;
-		motor[TIRE_BL].dir = FOR;
-		motor[TIRE_BR].dir = BACK;
-		motor[TIRE_FR].dir = BACK;
-		motor[TIRE_FL].pwm = 15;
-		motor[TIRE_FR].pwm = 15;
-		motor[TIRE_BR].pwm = 15;
-		motor[TIRE_BL].pwm = 15;
-		if(linePara[4] == 0) {  
-		 	lineFase = 2;
-		 	motor[TIRE_FL].dir = BRAKE;
-			motor[TIRE_BL].dir = BRAKE;
-			motor[TIRE_BR].dir = BRAKE;
-			motor[TIRE_FR].dir = BRAKE;
-			motor[TIRE_FL].pwm = 30;
-			motor[TIRE_FR].pwm = 30;
-			motor[TIRE_BR].pwm = 30;
-			motor[TIRE_BL].pwm = 30;
-		}
-	} else if(lineFase == 2){  // 位置調整
-		lineFase = 3;
-	} else if(lineFase == 3){  // 右 直進
-		motor[TIRE_FL].dir = BRAKE;
-		motor[TIRE_BL].dir = BRAKE;
-		motor[TIRE_BR].dir = BRAKE;
-		motor[TIRE_FR].dir = BRAKE;
-		motor[TIRE_FL].pwm = 30;
-		motor[TIRE_FR].pwm = 30;
-		motor[TIRE_BR].pwm = 30;
-		motor[TIRE_BL].pwm = 30;
-	}
-	
 }
 #endif