Revision 22:7d93f79a3686, committed 2019-09-17

Comitter:

M_souta

Date:

Tue Sep 17 04:40:17 2019 +0000

Parent:

21:e3b58d675c1c

Child:

23:c853372cf626

Commit message:

kkk

Changed in this revision

--- a/Communication/RS485/RS485.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ b/Communication/RS485/RS485.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -56,7 +56,6 @@
             }
             time = 0;
         } else if(readFase) {
-            RS485LineBuffer.PutData(data);
             buffer[time] = data;
             time++;
         } else {

--- a/Input/Encoder/Encoder.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/Encoder/Encoder.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -1,30 +1,20 @@
 #include "Encoder.h"
 #include "mbed.h"
 
-#include "../../System/Process/InterruptProcess.h"
-
-InterruptIn BoardECD[] = {
-    InterruptIn(ECD_A_0),
-    InterruptIn(ECD_A_1),
-    InterruptIn(ECD_A_2),
-    InterruptIn(ECD_A_3),
+const int PerRev = 256;
     
-    InterruptIn(ECD_B_0),
-    InterruptIn(ECD_B_1),
-    InterruptIn(ECD_B_2),
-    InterruptIn(ECD_B_3),
-};
+    QEI encoder[] = {
+        QEI(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING),
+        QEI(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING),
+        QEI(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING),
+        QEI(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING),
+    };
 
 namespace ENCODER {
+    
+    
+    
     void ECD::Initialize() {
-        BoardECD[0].mode(PullUp);
-        BoardECD[1].mode(PullUp);
-        BoardECD[2].mode(PullUp);
-        BoardECD[3].mode(PullUp);
-
-        BoardECD[0].fall(int2);
-        BoardECD[1].fall(int3);
-        BoardECD[2].fall(int4);
-        BoardECD[3].fall(int5);
+        
     }
 }
\ No newline at end of file

--- a/Input/Encoder/Encoder.h	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/Encoder/Encoder.h	Tue Sep 17 04:40:17 2019 +0000
@@ -1,6 +1,10 @@
 #ifndef ENCODER_H_
 #define ENCODER_H_
 
+#include "QEI.h"
+
+extern QEI encoder[];
+
 namespace ENCODER {
     #define ECD_A_0    PB_1
     #define ECD_A_1    PB_12
@@ -11,6 +15,7 @@
     #define ECD_B_1    PA_11
     #define ECD_B_2    PB_13
     #define ECD_B_3    PB_15
+    
 
     class ECD {
         public:

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Encoder/QEI.lib	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- a/Input/ExternalInt/ExternalInt.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/ExternalInt/ExternalInt.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -6,26 +6,14 @@
 InterruptIn BoardInt[] = {
     InterruptIn(INT0_PIN),
     InterruptIn(INT1_PIN),
-    InterruptIn(INT2_PIN),
-    InterruptIn(INT3_PIN),
-    InterruptIn(INT4_PIN),
-//    InterruptIn(INT5_PIN),
 };
 
 namespace EXTERNALINT {
     void Int::Initialize() {
         BoardInt[0].mode(PullUp);
         BoardInt[1].mode(PullUp);
-        BoardInt[2].mode(PullUp);
-        BoardInt[3].mode(PullUp);
-        BoardInt[4].mode(PullUp);
-//        BoardInt[5].mode(PullUp);
         
         BoardInt[0].fall(int0);
         BoardInt[1].fall(int1);
-        BoardInt[2].fall(int1);
-        BoardInt[3].fall(int1);
-        BoardInt[4].fall(int1);
-//        BoardInt[5].fall(int1);
     }
 }

--- a/Input/ExternalInt/ExternalInt.h	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/ExternalInt/ExternalInt.h	Tue Sep 17 04:40:17 2019 +0000
@@ -2,11 +2,8 @@
 #define EXTERNALINT_H_
 
 namespace EXTERNALINT {
-    #define INT0_PIN    PC_9
-    #define INT1_PIN    PC_8
-    #define INT2_PIN    PC_6
-    #define INT3_PIN    PC_5
-    #define INT4_PIN    PA_12
+    #define INT0_PIN    PA_12
+    #define INT1_PIN    PC_5
     
     class Int {
         public:

--- a/Input/Potentiometer/Potentiometer.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,13 +0,0 @@
-#include "Potentiometer.h"
-#include "mbed.h"
-
-namespace POTENTIOMETER {
-    AnalogIn adc[] = {
-        AnalogIn(ADC0_PIN),
-        AnalogIn(ADC1_PIN),
-        AnalogIn(ADC2_PIN),
-        AnalogIn(ADC3_PIN),
-        AnalogIn(ADC4_PIN),
-
-    };
-}

--- a/Input/Potentiometer/Potentiometer.h	Mon Sep 09 00:19:28 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#ifndef POTENTIOMETER_H_
-#define POTENTIOMETER_H_
-
-#include "mbed.h"
-
-namespace POTENTIOMETER {
-    #define ADC0_PIN    PC_3
-    #define ADC1_PIN    PC_0
-    #define ADC2_PIN    PC_1
-    #define ADC3_PIN    PC_0
-    #define ADC4_PIN    PB_0
-        
-    extern AnalogIn adc[];
-}
-
-#endif

--- a/Input/Switch/Switch.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/Switch/Switch.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -19,6 +19,14 @@
         DigitalOut(SELECT2_PIN),
         DigitalOut(SELECT3_PIN),
     };
+    
+    DigitalIn limitSw_N[] = {
+        DigitalIn(LS_16),
+        DigitalIn(LS_17),
+        DigitalIn(LS_18),
+        DigitalIn(LS_19),
+        DigitalIn(LS_20),
+    };
 
     void DipSw::Initialize() {
         for(uint8_t i=0; i < sizeof(dipSw)/sizeof(dipSw[0]); i++) {
@@ -43,55 +51,71 @@
     bool LimitSw::IsPressed(int index) {
 
         MP_Channel ch;
+        bool button;
         
         switch(index){
             case 0:
-            ch.all = 8;
-            break;
-            case 1:
             ch.all = 9;
             break;
-            case 2:
-            ch.all = 10;
-            break;
-            case 3:
+            case 1:
             ch.all = 11;
             break;
+            case 2:
+            ch.all = 13;
+            break;
+            case 3:
+            ch.all = 15;
+            break;
             case 4:
-            ch.all = 6;
+            ch.all = 8;
             break;
             case 5:
-            ch.all = 4;
+            ch.all = 10;
             break;
             case 6:
-            ch.all = 2;
+            ch.all = 12;
             break;
             case 7:
-            ch.all = 0;
+            ch.all = 14;
             break;
             case 8:
             ch.all = 7;
             break;
             case 9:
-            ch.all = 5;
+            ch.all = 4;
             break;
             case 10:
-            ch.all = 3;
+            ch.all = 2;
             break;
             case 11:
-            ch.all = 1;
+            ch.all = 0;
             break;
             case 12:
-            ch.all = 12;
+            ch.all = 6;
             break;
             case 13:
-            ch.all = 13;
+            ch.all = 5;
             break;
             case 14:
-            ch.all = 14;
+            ch.all = 3;
             break;
             case 15:
-            ch.all = 15;
+            ch.all = 1;
+            break;
+            case 16:
+            button = limitSw_N[0];
+            break;
+            case 17:
+            button = limitSw_N[1];
+            break;
+            case 18:
+            button = limitSw_N[2];
+            break;
+            case 19:
+            button = limitSw_N[3];
+            break;
+            case 20:
+            button = limitSw_N[4];
             break;
         }
         
@@ -103,6 +127,7 @@
         
         wait_us(1);
 
-        return limitSw ? false : true;
+        if(index < 16) return limitSw ? false : true;
+        else return button ? false : true;
     }
 }

--- a/Input/Switch/Switch.h	Mon Sep 09 00:19:28 2019 +0000
+++ b/Input/Switch/Switch.h	Tue Sep 17 04:40:17 2019 +0000
@@ -27,6 +27,13 @@
     #define DIP1    dipSw[1]
     #define DIP2    dipSw[2]
     #define DIP3    dipSw[3]
+    
+    //Normal
+    #define LS_16  PB_6
+    #define LS_17  PB_9
+    #define LS_18  PB_8
+    #define LS_19  PC_9
+    #define LS_20  PC_8
 
     class DipSw {
         public:

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/USS/LM61CIZ.lib	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/M_souta/code/LM61CIZ/#d2f99847b647

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/USS/Pulse.lib	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/NickRyder/code/Pulse/#fb79a4637a64

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/USS/USS.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,25 @@
+#include "USS.h"
+#include "mbed.h"
+
+USS::USS(PinName echoPin, PinName trigPin, PinName tempPin) 
+                        :echo_(echoPin), trig_(trigPin), temp_(tempPin)  
+{
+    distance_ = 0;
+    
+}
+
+double USS::ReadDis(void) {
+    int temp = temp_.getTemperature();
+    double duration = echo_.read_high_us(5000);
+    trig_.write_us(1,10);
+    if(duration > 0){
+        duration /= 2;
+        double sspead = 331.5+0.6*temp;
+        distance_ = duration*sspead*100/1000000;
+    } else distance_ = 0;
+    return distance_;
+}
+
+double USS::GetDis(void) {
+    return distance_;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/USS/USS.h	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,20 @@
+#ifndef USS_H_
+#define USS_H_
+
+#include "mbed.h"
+#include "LM61CIZ.h"
+#include "Pulse.h"
+
+class USS {
+    public:
+    USS(PinName echoPin, PinName trigPin, PinName tempPin);
+    double ReadDis(void);
+    double GetDis(void);
+    private:
+    float distance_;
+    PulseInOut echo_;
+    PulseInOut trig_;
+    LM61CIZ temp_;
+};
+
+#endif //ULTRASONI_H_
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/Ultrasonic.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,10 @@
+#include "Ultrasonic.h"
+
+namespace ULTRASONIC {
+    
+    
+    
+    
+    
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Input/Ultrasonic/Ultrasonic.h	Tue Sep 17 04:40:17 2019 +0000
@@ -0,0 +1,18 @@
+#ifndef  ULTRASONIC_H_
+#define  ULTRASONIC_H_ 
+
+#include "USS.h"
+
+#define  ECHO_0  PC_3
+#define  ECHO_1  PC_1
+    
+#define  TRIG_0  PC_0
+#define  TRIG_1  PB_0
+        
+#define  TEMP    PC_2
+
+namespace ULTRASONIC {
+    
+}
+
+#endif //ULREASONIC_H_
\ No newline at end of file

--- a/QEI.lib	Mon Sep 09 00:19:28 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- a/System/Process/Process.cpp	Mon Sep 09 00:19:28 2019 +0000
+++ b/System/Process/Process.cpp	Tue Sep 17 04:40:17 2019 +0000
@@ -1,7 +1,6 @@
 
 #include "mbed.h"
 #include "Process.h"
-#include "QEI.h"
 
 #include "../../CommonLibraries/PID/PID.h"
 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
@@ -9,8 +8,8 @@
 #include "../../Communication/Controller/Controller.h"
 #include "../../Input/ExternalInt/ExternalInt.h"
 #include "../../Input/Switch/Switch.h"
-#include "../../Input/Potentiometer/Potentiometer.h"
 #include "../../Input/Encoder/Encoder.h"
+#include "../../Input/Ultrasonic/Ultrasonic.h"
 #include "../../LED/LED.h"
 #include "../../Safty/Safty.h"
 #include "../Using.h"
@@ -18,8 +17,10 @@
 using namespace SWITCH;
 using namespace PID_SPACE;
 using namespace ENCODER;
+using namespace ULTRASONIC;
 using namespace LINEHUB;
 
+
 static CONTROLLER::ControllerData *controller;
 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
@@ -38,15 +39,12 @@
 
 /*Replace here with the definition code of your variables.*/
 
-Serial pc(USBTX, USBRX);
+USS ultrasonic[] = {
+        USS(ECHO_0,TRIG_0,TEMP),
+        USS(ECHO_1,TRIG_1,TEMP),
+    };
 
-//**************Encoder***************
-const int PerRev = 256;
-QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
-QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
-//**************Encoder***************
+Serial pc(USBTX, USBRX);
 
 //**************Buzzer****************
 //DigitalOut buzzer(BUZZER_PIN);
@@ -64,6 +62,95 @@
 Ticker tapeLedTimer;
 //************TapaLed*****************
 
+//*************** lift ***************
+#define LOWER   1
+#define MIDDLRE 2
+#define UPPER   3
+uint8_t  liftState = LOWER;
+bool moving = false;
+bool switchFlag_LB = false;
+bool switchFlag_RB = false;
+
+//*************** lift ***************
+
+//*************tire*************
+PID rotaconPID[] = {
+	PID(0.0001,-1,1,0.05,0,0),  //LF
+	PID(0.0001,-1,1,0.05,0,0),  //LB
+	PID(0.0001,-1,1,0.05,0,0),  //RB
+	PID(0.0001,-1,1,0.05,0,0),  //RF
+};
+
+#define FL 0
+#define BL 1
+#define BR 2
+#define FR 3
+
+#define PI 3.141592
+
+const float tireR = 101.6;  //タイヤの半径
+const float ucR = 420.0;    //中心からのタイヤの距離
+
+typedef struct {
+	float Vx;  //X方向の速度
+	float Vy;  //Y方向の速度
+	float Va;  //角速度
+} Vvector;
+
+Vvector move;           //進む速度
+Vvector correction_LT;  //ライントレースの補正速度
+Vvector synthetic;      //合成速度
+
+float sita = 0;
+
+bool PIDflag = false;
+
+int linePara[8];
+int linePara_U;
+int linePara_B;
+int linePara_L;
+int linePara_R;
+
+#define FL 0
+#define BL 1
+#define BR 2
+#define FR 3
+
+float tireProcessRPM[4];
+float tireTargetMaxRPM[4];
+float tireTargetRPM[4];
+
+float tirePWM[4];
+
+float timePV[4];
+float timeCV[4];
+float pulsePV[4];
+float pulseCV[4];
+
+void tirePID();
+int lineCast(char k);
+
+Timer  rotaconSampling;
+Ticker rotaconPIDtimer;
+
+bool countFlag;
+//*************tire**************//
+
+// ************* Line ************** //
+
+float pw = 0;
+int lineFase = 0;
+bool lineCheck = false;
+int linePWM;
+int adj_F;
+int adj_B;
+
+int mode = 0;
+
+int lineCount = 0;
+
+// ************* Line ************** //
+
 const int omni[15][15] =
 {
 {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
@@ -137,7 +224,7 @@
 {
 	#pragma region USER-DEFINED_VARIABLE_INIT
 	/*Replace here with the initialization code of your variables.*/	
-	
+	rotaconPIDtimer.attach(tirePID,0.1);
 	
 	#pragma endregion USER-DEFINED_VARIABLE_INIT
 
@@ -224,9 +311,17 @@
 	{
 		int g[8];
 		for(int i = 0; i < 8; i++){
-			g[i] = LineHub::GetPara(i);
+			g[i] = lineCast(LineHub::GetPara(i));
 		}
-		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]);
+		
+		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);
+		
 		
 		buzzer.period(1.0/800);
 		
@@ -257,7 +352,9 @@
 			}
 		}
 		
+		
 		//Emergency!
+		/*
 		if(!EMG_0 && !EMG_1 && !EMGflag){
 			buzzer = 0;
 			BuzzerTimer.attach(BuzzerTimer_func, 1);
@@ -269,6 +366,7 @@
 			BuzzerTimer.detach();
 			EMGflag = false;
 		}
+		*/
 		SystemProcessUpdate();
 	}
 }
@@ -288,7 +386,9 @@
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
-    /*
+    
+    PIDflag = false;
+    
     if(controller->Button.UP) {
     	motor[LIFT_LB].dir = FOR;
     	motor[LIFT_LB].pwm = 180;
@@ -305,7 +405,7 @@
     	motor[LIFT_RB].dir = BRAKE;
     	motor[LIFT_RB].pwm = 200;
     }
-    */
+    
     
     if(controller->Button.X) {
     	motor[LIFT_U].dir = FOR;
@@ -324,10 +424,10 @@
 		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_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]) ;
-		motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X])    ;
-		motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]) ;
-		motor[TIRE_BL].pwm = SetPWM(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_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)   ;	
 	} else {
 		motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
 		motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
@@ -345,28 +445,219 @@
 #if USE_PROCESS_NUM>2
 static void Process2()
 {	
-	
+/*
+	if(moving) {
+		if(LimiSw::IsPressed(LSW_LB)) {
+			if(switchFlag_LB) {
+				switchFlag_LB = false;
+				motor[LIFT_LB].dir = BRAKE;  
+	    		motor[LIFT_LB].pwm = 200;
+	    	} else {
+	    		seitchFlag_LB = true;
+	    	}
+		}
+		if(LimiSw::IsPressed(LSW_RB)) {
+			if(switchFlag_RB) {
+				switchFlag_RB = false;
+				motor[LIFT_RB].dir = BRAKE;
+	    		motor[LIFT_RB].pwm = 200;
+	    	} else {
+	    		seitchFlag_RB = true;
+	    	}
+		}
+		if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
+	} else {
+		if(controller->Button.UP) {
+			if(!(state == UPPER)) {
+				state++;
+				motor[LIFT_LB].dir = BACK;
+				motor[LIFT_RB].dir = FOR;
+    			motor[LIFT_LB].pwm = 200;
+    			motor[LIFT_RB].pwm = 200;
+			}
+		} else if(controller->Button.DOWN) {
+			if(!(state == LOWER)) {
+				state--;
+				moving = true;
+				motor[LIFT_LB].dir = FOR;
+				motor[LIFT_RB].dir = BACK;
+    			motor[LIFT_LB].pwm = 200;
+    			motor[LIFT_RB].pwm = 200;
+			}
+		} else {
+			motor[LIFT_LB].dir = BRAKE;
+			motor[LIFT_RB].dir = BRAKE;
+    		motor[LIFT_LB].pwm = 200;
+    		motor[LIFT_RB].pwm = 200;
+		}
+	}
+*/
 }
 #endif
 
 #if USE_PROCESS_NUM>3
 static void Process3() 
 {
-	
+	AllActuatorReset();
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4() 
-{	
+{
+	
+	static int x,y;
+	static int count = 0;
+	
+	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(linePara_U == 'A' && count == 0) {
+		lineFase = 1;
+	}
 
+	if(lineFase == 0) {
+		pw = 0.55;
+		switch(linePara_U) {
+			case -2:
+				x = 5;
+				y = 3;
+				break;
+			case -3:
+				x = 5;
+				y = 3;
+				break;
+			case -1:
+				x = 6;
+				y = 3;
+				break;
+			case 0:
+				x = 7;
+				y = 3;
+				break;
+			case 1:
+				x = 8;
+				y = 3;
+				break;
+			case 3:
+				x = 9;
+				y = 3;
+				break;
+			case 2:
+				x = 9;
+				y = 3;
+				break;
+			case 'A':
+				lineCheck = true;
+				x = x;
+				y = y;
+				break;
+			case 'N':
+				x = 7;
+				y = 7;
+				break;
+				x = 7;
+				y = 7;
+			default:
+				x = 9;
+				y = 9;
+		}
+		if(lineCheck == true && (!(linePara_U) == 'A')) {
+			count++;
+		}
+	} else if(lineFase == 1) {
+		pw = 0.4;
+		switch(linePara_B) {
+			case -2:
+				x = 5;
+				y = 3;
+				break;
+			case -3:
+				x = 5;
+				y = 3;
+				break;
+			case -1:
+				x = 6;
+				y = 3;
+				break;
+			case 0:
+				x = 7;
+				y = 3;
+				break;
+			case 1:
+				x = 8;
+				y = 3;
+				break;
+			case 3:
+				x = 9;
+				y = 3;
+				break;
+			case 2:
+				x = 9;
+				y = 3;
+				break;
+			case 'A':
+				x = 7;
+				y = 7;
+				break;
+			case 'N':
+				x = 7;
+				y = 7;
+				break;
+				x = 7;
+				y = 7;
+			default:
+				x = 9;
+				y = 9;
+		}
+		if(linePara_R == 0 && linePara_L == 0) {
+			lineFase = 2;
+			x = 7;
+			y = 7;
+		}
+	} 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);	
+	}
+	
 }
 #endif
 
 #if USE_PROCESS_NUM>5
 static void Process5() 
 {	
-	
+	lineFase = 0;
+	lineCheck = true;
 }
 #endif
 
@@ -374,20 +665,295 @@
 static void Process6() 
 {
 	
+	for(int i = 0; i < 8; i++){
+		linePara[i] = lineCast(LineHub::GetPara(i));
+	}
+	
+	static int count = 0;
+	count++;
+	
+	if(count < 10000) {
+		lineCheck = false;
+	} else {
+		lineCheck = true;
+	}
+	
+	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 = FOR;
+				motor[TIRE_BR].dir = BACK;
+				motor[TIRE_FR].dir = BACK;
+				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 = FOR;
+				motor[TIRE_BR].dir = BACK;
+				motor[TIRE_FR].dir = BACK;
+				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 = FOR;
+				motor[TIRE_BR].dir = BACK;
+				motor[TIRE_FR].dir = BACK;
+				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 = FOR;
+				motor[TIRE_BR].dir = BACK;
+				motor[TIRE_FR].dir = BACK;
+				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 = FOR;
+				motor[TIRE_BR].dir = BACK;
+				motor[TIRE_FR].dir = BACK;
+				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
 
 #if USE_PROCESS_NUM>7
 static void Process7()
 {
-	
+	if(lineFase == 0) { // 前進
+		switch(linePara[0]) {
+			case -2: adj_F = 10;
+				break;
+			case -3: adj_F = 5;
+				break;
+			case -1: adj_F = 2;
+				break;
+			case 0:  adj_F = 0;
+				break;
+			case 1:  adj_F = -2;
+				break;
+			case 3:  adj_F = -5;
+				break;
+			case 2:  adj_F = -10;
+				break;
+			case 'A':
+				break;
+			case 'N':
+				break;
+			default:
+		}
+		switch(linePara[2]) {
+			case -2: adj_F = -10;
+				break;
+			case -3: adj_F = -5;
+				break;
+			case -1: adj_F = -2;
+				break;
+			case 0:  adj_F = 0;
+				break;
+			case 1:  adj_F = 2;
+				break;
+			case 3:  adj_F = 5;
+				break;
+			case 2:  adj_F = 10;
+				break;
+			case 'A': adj_F = 0; 
+				break;
+			case 'N':
+				break;
+			default:
+		}
+		
+		if(mode == 0) linePWM = 40;
+		else if (mode == 1) linePWM = 20;
+		else if (mode == 2) linePWM = 0;
+		else linePWM = 0;
+		
+		motor[TIRE_FL].dir = SetStatus(linePWM - adj_F);
+		motor[TIRE_BL].dir = SetStatus(linePWM - adj_B);
+		motor[TIRE_BR].dir = SetStatus(-linePWM - adj_B);
+		motor[TIRE_FR].dir = SetStatus(-linePWM - adj_F);
+		motor[TIRE_FR].pwm = SetPWM(linePWM - adj_F);
+		motor[TIRE_FL].pwm = SetPWM(linePWM - adj_B);
+		motor[TIRE_BR].pwm = SetPWM(-linePWM - adj_B);
+		motor[TIRE_BL].pwm = SetPWM(-linePWM - adj_F);	
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>8 
 static void Process8()
 {
-	
+	if(controller->Button.A) {
+		rotaconSampling.start();
+		PIDflag = true;
+		
+		//linePara_U = LineHub::GetPara(0);
+		//linePara_B = LineHub::GetPara(3);
+		
+		
+		pulsePV[FL] = encoder[FL].getPulses();
+	    pulsePV[BL] = encoder[BL].getPulses();
+	    pulsePV[BR] = encoder[BR].getPulses();
+	    pulsePV[FR] = encoder[FR].getPulses();
+	    
+	    
+	    for (int i = 0; i < 4; i++) {
+	        timeCV[i] = timePV[i];
+	        timePV[i] = rotaconSampling.read();
+	        tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
+	        pulseCV[i] = pulsePV[i];
+	    }
+	    
+	    move.Vx = 0.5;
+	    move.Vy = 0.5;
+	    move.Va = 0;
+	    
+	    correction_LT.Vx = 0;  //0.1 * linePara_U;
+	    correction_LT.Vy = 0;
+	    correction_LT.Va = 0;
+	    
+	    synthetic.Vx = move.Vx + correction_LT.Vx;
+	    synthetic.Vy = move.Vy + correction_LT.Vy;
+	    synthetic.Va = move.Va + correction_LT.Va;
+	    	
+		sita = 0;
+		
+		//タイヤの目標速度算出
+	    float sinR = 0.7071 * (float)sin(sita);
+	    float cosR = 0.7071 * (float)cos(sita);
+	    float nv = (60 * 1000) / ( 2.00 * PI * tireR);
+	    tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+	    tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+	    tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+	    tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+	    
+	    //pc.printf("process : %f   target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
+	    
+	    //PIDによるPWM算出
+	    
+	    //モータの駆動
+	    for (int i = 0; i < 4; i++) {
+	    	if (tirePWM[i] > 255){
+	    		tirePWM[i] = 255;
+	    	} else if (tirePWM[i] < -255) {
+	    		tirePWM[i] = -255;
+	    	}
+	    }
+	    
+	    for(int i = 0;i < 4;i++){
+	    	motor[i].dir = SetStatus(tirePWM[i]);
+	    	motor[i].pwm = SetPWM(tirePWM[i]);
+	    }
+	} else {
+		PIDflag = false;
+		rotaconSampling.stop();
+		rotaconSampling.reset();
+		for(int i = 0;i < 4;i++){
+			encoder[i].reset();
+			pulsePV[i] = 0;
+			pulseCV[i] = 0;
+			timePV[i] = 0;
+			timeCV[i] = 0;
+			tirePWM[i] = 0;
+	    	motor[i].dir = SetStatus(tirePWM[i]);
+	    	motor[i].pwm = SetPWM(tirePWM[i]);
+	    }
+	}
 }
 #endif
 
@@ -426,6 +992,37 @@
 }
 
 #pragma region USER-DEFINED-FUNCTIONS
+void tirePID() {
+	if(PIDflag == true) {
+		//加算するPID値の算出
+	    rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
+	    rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
+	    rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
+	    rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
+	    //PID値の加算
+	    tirePWM[FL] += rotaconPID[0].GetMV();
+	    tirePWM[BL] += rotaconPID[1].GetMV();
+	    tirePWM[FR] += rotaconPID[2].GetMV();
+	    tirePWM[BR] += rotaconPID[3].GetMV();
+	}
+}
 
+int lineCast(char k) {
+	int l;
+	switch(k) {
+		case 255:
+			l = -1;
+			break;
+		case 254:
+			l = -2;
+			break;
+		case 253:
+			l = -3;
+			break;
+		default:
+			l = k;
+	}
+	return l;
+}
 
 #pragma endregion

--- a/System/Process/Process.h	Mon Sep 09 00:19:28 2019 +0000
+++ b/System/Process/Process.h	Tue Sep 17 04:40:17 2019 +0000
@@ -3,22 +3,23 @@
 
 #include "mbed.h"
 
-#define BUZZER_PIN PA_5
+#define BUZZER_PIN PB_3
 #define EMG_0 LimitSw::IsPressed(0)
 #define EMG_1 LimitSw::IsPressed(1)
 
 void SystemProcess();
 
-#define  TIRE_FL  3
-#define  TIRE_FR  2
-#define  TIRE_BL  1
-#define  TIRE_BR  0
+#define  TIRE_FL  0
+#define  TIRE_FR  1
+#define  TIRE_BL  3
+#define  TIRE_BR  2
 
 #define  LIFT_LB  4
-#define  LIFT_RB  6
-#define  LIFT_U   7
+#define  LIFT_RB  5
+#define  LIFT_U   6
 
-
+#define  LSW_LB   0
+#define  LSW_RB   1