Revision 4:ba9df71868df, committed 2018-10-01

Comitter:

7ka884

Date:

Mon Oct 01 13:47:19 2018 +0000

Parent:

3:e10d8736fd22

Child:

6:10e22bc327ce

Commit message:

huhuhuh

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommonLibraries/PID/PID.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -0,0 +1,84 @@
+/*
+* PID.cpp
+*
+* Created: 2016/07/01 17:47:16
+*  Author: yuuki
+*/
+
+#include "PID.h"
+#include <stdlib.h>
+
+namespace PID_SPACE
+{
+    
+    PID::PID(double deltaTime)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = 0;
+        this->dataRangeUpper = 100;
+        kp = 0;
+        ki = 0;
+        kd = 0;
+    }
+    
+    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = dataRangeLower;
+        this->dataRangeUpper = dataRangeUpper;
+        kp = 0;
+        ki = 0;
+        kd = 0;
+    }
+    
+    PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
+    {
+        this->deltaTime = deltaTime;
+        this->dataRangeLower = dataRangeLower;
+        this->dataRangeUpper = dataRangeUpper;
+        kp = KP;
+        ki = KI;
+        kd = KD;
+    }
+    
+    void PID::SetParam(double KP, double KI, double KD)
+    {
+        kp = KP;
+        ki = KI;
+        kd = KD;
+    }
+    
+    double PID::SetPV(double sensorData, double targetData)
+    {
+        double p, i, d;
+        
+        diff[0] = diff[1];
+        diff[1] = sensorData - targetData;
+        integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
+        
+        p = kp * diff[1];
+        i = ki * integral;
+        d = kd * ((diff[1] - diff[0]) / deltaTime);
+        mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
+        return mv;
+    }
+    
+    double PID::GetMV()
+    {
+        return mv;
+    }
+    
+    double PID::limit(double data,double lower,double upper)
+    {
+        if(data < lower)        return lower;
+        else if(data > upper)   return upper;
+        else                    return data;
+    }
+
+    //速度系PID関数
+    double PID::SetSpeed(double sensorData,double targetData)
+    {
+        //TODO:速度単位系の変換
+    }
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CommonLibraries/PID/PID.h	Mon Oct 01 13:47:19 2018 +0000
@@ -0,0 +1,52 @@
+/*
+ * PID.h
+ *
+ * Created: 2016/07/01 17:47:45
+ *  Author: yuuki
+ */ 
+
+
+#ifndef PID_H_
+#define PID_H_
+
+namespace PID_SPACE
+{
+    class PID
+    {
+        private:
+        double diff[2];
+        double integral;
+        double deltaTime;
+        double dataRangeLower;
+        double dataRangeUpper;
+        double kp,ki,kd;
+        double mv;
+        
+        public:
+        // deltaTime:1サイクル時間( 1 / Process Frequency )
+        PID(double deltaTime);
+        PID(double deltaTime, double dataRangeLower, double dataRangeUpper);
+        PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD);
+        
+        //パラメータを設定
+        void SetParam(double KP, double KI, double KD);
+        
+        //測定量を入力し操作量を取得
+        double SetPV(double sensorData, double targetData);
+        
+        //操作量を取得
+        double GetMV();
+        
+        //入力した値を制限して取得
+        double limit(double data, double lower, double upper);
+
+        //TODO:速度系PID関数
+        double SetSpeed(double sensorData,double targetData);
+    };
+    
+    
+}
+
+
+
+#endif /* PID_H_ */
\ No newline at end of file

--- a/Input/Rotaryencoder/Rotaryencoder.cpp	Sat Sep 22 10:58:25 2018 +0000
+++ b/Input/Rotaryencoder/Rotaryencoder.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -3,7 +3,7 @@
 
 #include "../../System/Process/InterruptProcess.h"
 
-InterruptIn BoardRtInt[] = {
+InterruptIn BoardRt[] = {
     InterruptIn(RT11_PIN),
     InterruptIn(RT12_PIN),
     
@@ -12,15 +12,15 @@
 };
 
 namespace ROTARYENCODER {
-    void Int::Initialize() {
-        BoardRtInt[0].mode(PullUp);
-        BoardRtInt[1].mode(PullUp);
-        BoardRtInt[2].mode(PullUp);
-        BoardRtInt[3].mode(PullUp);
+    void Rot::Initialize() {
+        BoardRt[0].mode(PullUp);
+        BoardRt[1].mode(PullUp);
+        BoardRt[2].mode(PullUp);
+        BoardRt[3].mode(PullUp);
 
-        BoardRtInt[0].fall(int0);
-        BoardRtInt[1].fall(int1);
-        BoardRtInt[2].fall(int2);
-        BoardRtInt[3].fall(int3);
+        BoardRt[0].fall(int2);
+        BoardRt[1].fall(int3);
+        BoardRt[2].fall(int4);
+        BoardRt[3].fall(int5);
     }
 }
\ No newline at end of file

--- a/Input/Rotaryencoder/Rotaryencoder.h	Sat Sep 22 10:58:25 2018 +0000
+++ b/Input/Rotaryencoder/Rotaryencoder.h	Mon Oct 01 13:47:19 2018 +0000
@@ -8,7 +8,7 @@
     #define RT21_PIN    PC_2
     #define RT22_PIN    PA_1
 
-    class Int {
+    class Rot {
         public:
         static void Initialize();
     };

--- a/Input/Switch/Switch.cpp	Sat Sep 22 10:58:25 2018 +0000
+++ b/Input/Switch/Switch.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -12,6 +12,7 @@
     };
 
     DigitalIn limitSw(LS_PIN);
+    
     DigitalOut selectPin[] = {
         DigitalOut(SELECT0_PIN),
         DigitalOut(SELECT1_PIN),
@@ -84,7 +85,7 @@
         selectPin[1] = ch.s1;
         selectPin[2] = ch.s2;
         selectPin[3] = ch.s3;
-
+        
         wait_us(1);
 
         return limitSw ? false : true;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Oct 01 13:47:19 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- a/System/Process/InterruptProcess.cpp	Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/InterruptProcess.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -21,7 +21,7 @@
 
 void int3()
 {
-    
+
 }
 
 void int4()

--- a/System/Process/InterruptProcess.h	Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/InterruptProcess.h	Mon Oct 01 13:47:19 2018 +0000
@@ -9,12 +9,5 @@
 void int5();
 void int6();
 void int7();
-void int8();
-void int9();
-void int10();
-void int11();
-void int12();
-void int13();
-void int14();
 
 #endif

--- a/System/Process/Process.cpp	Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/Process.cpp	Mon Oct 01 13:47:19 2018 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "Process.h"
+#include "QEI.h"
 
+#include "../../CommonLibraries/PID/PID.h"
 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
 #include "../../Communication/Controller/Controller.h"
 #include "../../Input/ExternalInt/ExternalInt.h"
@@ -17,6 +19,8 @@
 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 +59,13 @@
     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 Angul_R 4 //角度調節右
+#define Angul_L 5 //角度調節左
 
 const int mecanum[15][15]=
 {
@@ -105,32 +109,33 @@
 //************ライントレース変数*******************
 	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 startP = 35;
+	int downP = 5;
 //************ライントレース変数*******************
+//ROタコン
+ QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
+ 
+ Ticker get_rpm;
+ int rpm;
+ int palse;
+ double goalpoint = 3000.0000;
+ 
+ PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
+ 
+//
 
-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();
 
@@ -256,10 +261,15 @@
 	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());
+	{     
+	   /*wait(0.1);
+	   //wheel.getPulses()...どちらの方向にどれだけ回ったか
+	   pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+	   //軸が何回転したか
+	   pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
+	   */
+	   
+	   /*	float x = 0, y= 0, z = 0;
 	    
 	    x = acc[0]*1000;
 	    y = acc[1]*1000;
@@ -269,8 +279,12 @@
 	    
 	    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);
+	    float rotateZ = (z - 300)/1.18 - 90;
+	    pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
 		wait_ms(50);
+		*/
+		/*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
+		else LED_DEBUG0 = LED_OFF;*/
 
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
@@ -312,36 +326,62 @@
 static void Process0()
 {
 	ColorDetection();
+	pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]);
+	pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]);
+	pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]);
+	pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]);
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
-    motor[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;
+    }
+    
     
-    }
+	   //wheel.getPulses()...どちらの方向にどれだけ回ったか
+	   pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+	   //軸が何回転したか
+	   pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
 }
 #endif
 
-bool buttoncomp = false;
 #if USE_PROCESS_NUM>2
 static void Process2()
 {
-	/*ColorDetection();
+	static bool color_flag = false;
+	
+	static bool traceon = false;//fase1
+	static bool yokofla = false;//fase2
+	static bool boxslip = false;//fase3
 	
+	static bool compA = false;
+	static bool compB = false;
+	static bool compC = false;
+	static bool compD = false;
+	
+	static bool invationA = false;
+	static bool invationB = false;
+	static bool invationC = false;
+	static bool invationD = false;
+	
+	ColorDetection();
+	//
 	if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
 	{
 	invationA ^= 1;//start false,over true
@@ -349,21 +389,203 @@
 	}	
 	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)//白
+	{
+	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)//白
 	{
-		motor[0].dir = dir;
-		motor[0].pwm = startP;
+	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(invationA)
+	if(traceon && !yokofla && !boxslip)
 	{
-	   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;
+		
+		wait(2);
+		
+		yokofla = true;
+		traceon = false;
+		}
+		else{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		}
+	}
+		
+	if(!traceon && yokofla && !boxslip)
+	 {
+		if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+		{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		wait(2);
+		
+		boxslip = true;
+		yokofla = false;
+		}
+		else if(invationA && !compA && invationB)
+		{
+		motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else if(!invationA && !compB && !invationB)
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
 		
-	}*/
-	
-	
-	
-	
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else if(invationA && compA && !invationB && !compB)
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else if(compB && invationB)
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;	
+		}
+	}
+		
+	if(!traceon && !yokofla && boxslip)
+	 {
+	 	if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+	 	{
+	 	motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+	 	}
+	 	else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4))
+	 	{
+	 	motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		}
+	 }
+		/*////
+		motor[0].dir = BACK;
+		motor[1].dir = BACK;
+		motor[2].dir = FOR;
+		motor[3].dir = FOR;
+		
+		motor[0].pwm = startP;
+		motor[1].pwm = startP;
+		motor[2].pwm = startP;
+		motor[3].pwm = startP;
+		else if()
+		{
+		motor[0].dir = BRAKE;
+		motor[1].dir = BRAKE;
+		motor[2].dir = BRAKE;
+		motor[3].dir = BRAKE;
+		
+		motor[0].pwm = 255;
+		motor[1].pwm = 255;
+		motor[2].pwm = 255;
+		motor[3].pwm = 255;
+		}*///////
 }
 #endif
 
@@ -371,68 +593,129 @@
 static void Process3()
 {
 	if(controller->Button.R){
-	    motor[4].dir = FOR;
-	    motor[5].dir = BACK;
-	    motor[4].pwm = 150;
-	    motor[5].pwm = 150;
+	    motor[Angul_R].dir = FOR;
+	    motor[Angul_L].dir = BACK;
+	    motor[Angul_R].pwm = 150;
+	    motor[Angul_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[Angul_R].dir = BACK;
+	    motor[Angul_L].dir = FOR;
+	    motor[Angul_R].pwm = 150;
+	    motor[Angul_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[Angul_R].dir = BRAKE;
+	    motor[Angul_L].dir = BRAKE;
+	}
+	
+	if(LimitSw::IsPressed(0) && motor[4].dir == FOR && motor[5].dir == BACK){
+	    motor[Angul_R].dir = BRAKE;
+	    motor[Angul_L].dir = BRAKE;
+	    
+	    motor[Angul_R].pwm = 255;
+	    motor[Angul_L].pwm = 255;
+	}else if(LimitSw::IsPressed(1) && motor[4].dir == BACK && motor[5].dir == FOR){
+	    motor[Angul_R].dir = BRAKE;
+	    motor[Angul_L].dir = BRAKE;
+	    
+	    motor[Angul_R].pwm = 255;
+	    motor[Angul_L].pwm = 255;
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4()
 {
-	//ColorDetection();
+	static bool color_flag = false;
+	
+	static bool traceon = false;//fase1
+	static bool yokofla = false;//fase2
+	static bool boxslip = false;//fase3
+	
+	static bool compA = false;
+	static bool compB = false;
+	static bool compC = false;
+	static bool compD = false;
+	
+	static bool invationA = false;
+	static bool invationB = false;
+	static bool invationC = false;
+	static bool invationD = false;
+	
+	ColorDetection();
+	//
+	if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+	{
+	invationA ^= 1;//start false,over true
+	compA = true;//on true,noon false
+	}	
+	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
+	
+	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
+	{
+	invationB ^= 1;//start false,over true
+	compB = true;//on true,noon false
+	}	
+	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
+	
+	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
+	{
+	invationC ^= 1;//start false,over true
+	compC = true;//on true,noon false
+	}	
+	else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
 	
-    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];
-    }
-    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);
+	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;
 	
-	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;
+	if(traceon)
+	{
+	 if(compA && compD)
+	 {
+	    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 if(compB && compC)
+	{
+	    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
+	 {
+	 	motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+	 }
+	}
 }
 #endif
 
@@ -450,14 +733,30 @@
 #if USE_PROCESS_NUM>6
 static void Process6()
 {
-
+	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);
+	
+	/*void Anglecontrol(){
+		if(rotateX>) && */
 }
 #endif
 
 #if USE_PROCESS_NUM>7
 static void Process7()
 {
-
+  
 }
 #endif
 
@@ -515,17 +814,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 +826,10 @@
     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");*/
 }
 #pragma endregion

--- a/System/Process/Process.h	Sat Sep 22 10:58:25 2018 +0000
+++ b/System/Process/Process.h	Mon Oct 01 13:47:19 2018 +0000
@@ -4,66 +4,14 @@
 #include "mbed.h"
 
 void SystemProcess();
-
+/*
 #define ROLLER_LF motor[ROLLER_LF_NUM]
 #define ROLLER_LL motor[ROLLER_LL_NUM]
-#define ROLLER_LB motor[ROLLER_LB_NUM]
-#define ROLLER_LR motor[ROLLER_LR_NUM]
-#define ROLLER_CF motor[ROLLER_CF_NUM]
-#define ROLLER_CL motor[ROLLER_CL_NUM]
-#define ROLLER_CB motor[ROLLER_CB_NUM]
-#define ROLLER_CR motor[ROLLER_CR_NUM]
-#define ROLLER_RF motor[ROLLER_RF_NUM]
-#define ROLLER_RL motor[ROLLER_RL_NUM]
-#define ROLLER_RB motor[ROLLER_RB_NUM]
-#define ROLLER_RR motor[ROLLER_RR_NUM]
+*/
 
-#define ROLLER_LF_NUM 8
-#define ROLLER_LL_NUM 9
-#define ROLLER_LB_NUM 13
-#define ROLLER_LR_NUM 14
-#define ROLLER_CF_NUM 15
-#define ROLLER_CL_NUM 16
-#define ROLLER_CB_NUM 17
-#define ROLLER_CR_NUM 18
-#define ROLLER_RF_NUM 19
-#define ROLLER_RL_NUM 20
-#define ROLLER_RB_NUM 21
-#define ROLLER_RR_NUM 22
-
-#define FRONT_R tire[0]
-#define FRONT_L tire[1]
-#define REAR_L tire[2]
-#define REAR_R tire[3]
 
-#define TIRE_FR motor[0]
-#define TIRE_FL motor[1]
-#define TIRE_RL motor[2]
-#define TIRE_RR motor[3]
-#define STR_FR motor[4]
-#define STR_FL motor[5]
-#define STR_RL motor[6]
-#define STR_RR motor[7]
-
-#define SENSOR_FR POTENTIOMETER::adc[1].read_u16()
-#define SENSOR_FL (POTENTIOMETER::adc[2].read_u16() / 65535.0 * 1024.0)
-#define SENSOR_RL POTENTIOMETER::adc[3]
-#define SENSOR_RR POTENTIOMETER::adc[4]
+#define ROTATE_PER_REVOLUTIONS 500
 
-#define PLUS90_FR 605
-// #define PLUS90_FL 31100
-#define PLUS90_FL 480
-#define PLUS90_RL 505
-#define PLUS90_RR 550
-
-#define ZERO_FR 880
-// #define ZERO_FL 48800
-#define ZERO_FL 760
-#define ZERO_RL 775
-#define ZERO_RR 800
-
-extern Timer rollerTimer[4];
-extern float rollerSpeed[4];
 
 
 #endif