aa

Dependencies:   mbed

Revision:
21:e3b58d675c1c
Parent:
20:eae8c84f318c
Child:
22:7d93f79a3686
diff -r eae8c84f318c -r e3b58d675c1c System/Process/Process.cpp
--- a/System/Process/Process.cpp	Tue Aug 27 04:33:08 2019 +0000
+++ b/System/Process/Process.cpp	Mon Sep 09 00:19:28 2019 +0000
@@ -5,6 +5,7 @@
 
 #include "../../CommonLibraries/PID/PID.h"
 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
+#include "../../Communication/RS485/LineHub/LineHub.h"
 #include "../../Communication/Controller/Controller.h"
 #include "../../Input/ExternalInt/ExternalInt.h"
 #include "../../Input/Switch/Switch.h"
@@ -17,6 +18,7 @@
 using namespace SWITCH;
 using namespace PID_SPACE;
 using namespace ENCODER;
+using namespace LINEHUB;
 
 static CONTROLLER::ControllerData *controller;
 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -62,26 +64,39 @@
 Ticker tapeLedTimer;
 //************TapaLed*****************
 
-/*************LineHub****************
-Serial linehubUart(PC_10,PC_11);
-char data[6];
-
-void LineRead()
+const int omni[15][15] =
 {
-    //__disable_irq();
-    if(linehubUart.readable()) {
-        for(int i=0; i<=5; i++) {
-            data[i] = linehubUart.getc();
-        }
-    }
-    //__enable_irq();
+{    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
+{   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
+{  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
+{  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
+{  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
+{ -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
+{ -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
+{ -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
+{ -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
+{ -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
+{ -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
+{ -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
+{ -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
+{ -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
+{ -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
+};
+
+const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
+
+uint8_t SetStatus(int);
+uint8_t SetStatus(int pwmVal) {
+ if (pwmVal < 0) return BACK;
+ else if (pwmVal > 0) return FOR;
+ else if (pwmVal == 0) return BRAKE;
+ else return BRAKE;
 }
-
-//*************LineHub****************/
-
-float tireProRPM[4];
-float tireTarRPM[4];
-float tirePWM[4];
+uint8_t SetPWM(int);
+uint8_t SetPWM(int pwmVal) {
+ if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
+ else return abs(pwmVal);
+}
 
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
@@ -122,6 +137,8 @@
 {
 	#pragma region USER-DEFINED_VARIABLE_INIT
 	/*Replace here with the initialization code of your variables.*/	
+	
+	
 	#pragma endregion USER-DEFINED_VARIABLE_INIT
 
 	lock = true;
@@ -192,6 +209,7 @@
 
 	#ifdef USE_RS485
 	ACTUATORHUB::ActuatorHub::Update();
+	//LINEHUB::LineHub::Update();
 	#endif
 	
 }
@@ -204,15 +222,12 @@
 
 	while(1)
 	{
-		
-		pc.printf("aaa\n\r");
+		int g[8];
+		for(int i = 0; i < 8; i++){
+			g[i] = 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]);
 		
-		if(LimitSw::IsPressed(10)) {
-			LED_DEBUG0 = LED_ON;
-		} else {
-			LED_DEBUG0 = LED_OFF;
-		}
-		//printf("%d\r\n",ECD_0.getPulses());
 		buzzer.period(1.0/800);
 		
 		#ifdef USE_MU
@@ -266,14 +281,64 @@
 #if USE_PROCESS_NUM>0
 static void Process0() 
 {	
-	
+	AllActuatorReset();
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
+    /*
+    if(controller->Button.UP) {
+    	motor[LIFT_LB].dir = FOR;
+    	motor[LIFT_LB].pwm = 180;
+    	motor[LIFT_RB].dir = BACK;
+    	motor[LIFT_RB].pwm = 180;
+    } else if(controller->Button.DOWN) {
+    	motor[LIFT_LB].dir = BACK;
+    	motor[LIFT_LB].pwm = 180;
+    	motor[LIFT_RB].dir = FOR;
+    	motor[LIFT_RB].pwm = 180;
+    } else {
+    	motor[LIFT_LB].dir = BRAKE;
+    	motor[LIFT_LB].pwm = 200;
+    	motor[LIFT_RB].dir = BRAKE;
+    	motor[LIFT_RB].pwm = 200;
+    }
+    */
     
+    if(controller->Button.X) {
+    	motor[LIFT_U].dir = FOR;
+    	motor[LIFT_U].pwm = 180;
+    } else if(controller->Button.Y) {
+    	motor[LIFT_U].dir = BACK;
+    	motor[LIFT_U].pwm = 180;
+    } else {
+    	motor[LIFT_U].dir = BRAKE;
+    	motor[LIFT_U].pwm = 180;
+    }
+
+	if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
+	    motor[TIRE_BL].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_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])   ;	
+	} else {
+		motor[TIRE_BL].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_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]);
+	}
 }
 #endif