pid调试暂存

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

@@ -7,6 +7,7 @@
 #include "robot_state.h"
 #include "bsp_GPIO.h"
 #include "msp_Strong_grinding_machine.h"
+#include "msp_PID.h"
 
 #define PI 3.1415926
 
@@ -21,18 +22,42 @@ double RP_Speed_Ctrl = 0;
 
 float speedAdj = 0;
 #define MOTOR_TO_COUNT 164.433
+float Outspeed[2] = {0};
+float Inspeed;
+float g_zoom = 0.2;
+int g_flag = 0;
+int g_flaga = 1;
 
-void SetMoveMotorSpeed(double Speed)
+void SetMoveMotorSpeed(float Speed)
 {
-    double Inspeed = (Speed*360/100/101/6*3.1415926*0.325);//转换为m/min进入PID
+    Inspeed = (float)(Speed*360/100/101/6*3.1415926f*0.325f);//转换为m/min进入PID
+    float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
+    
+    TwoWheel_AngleControl(robotRoll, 0.0f, Inspeed, g_zoom, Outspeed);
 
-    float Outspeed[2] = {0};
+    int a = 0;
-    TwoWheel_AngleControl(GV.TL720DParameters.RF_Angle_Roll, 0, Inspeed, 1, Outspeed);
+    int b = 1;
 
-    GV.LeftFrontMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
+    if (0 == g_flaga)
-	GV.RightFrontMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
+    {
-	GV.LeftBackMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
+    	b = 0;
-	GV.RightBackMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
+    	a = 1;
+    }
+
+	if (0 == g_flag)
+	{
+		GV.LeftFrontMotor.Target_Velcity = Outspeed[a] * MOTOR_TO_COUNT;
+		GV.RightFrontMotor.Target_Velcity = -Outspeed[b] * MOTOR_TO_COUNT;
+		GV.LeftBackMotor.Target_Velcity = Outspeed[a] * MOTOR_TO_COUNT;
+		GV.RightBackMotor.Target_Velcity = -Outspeed[b] * MOTOR_TO_COUNT;
+	}
+	else
+	{
+		GV.LeftFrontMotor.Target_Velcity = 0;
+		GV.RightFrontMotor.Target_Velcity = 0;
+		GV.LeftBackMotor.Target_Velcity = 0;
+		GV.RightBackMotor.Target_Velcity = 0;
+	}
 }
 
 void Manual_State_Do(void)
@@ -122,10 +147,10 @@ void Manual_State_Do(void)
 		R2_Speed_Con = 0;
 		L2_Speed_Con = 0;
 	}
-	GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
+    GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
-	GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
+    GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
-	GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
+    GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
-	GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
+    GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
 }
 
 void low_Speed_Manual_State_Do(void)
@@ -406,10 +431,17 @@ void Strong_Grinding_Machine_Motion_State_Do(void)
 
 void auto_forward_state_do(void)
 {
-	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
+	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
 	{
-		speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
+		Speed_Ctrl =  (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
-		Speed_Ctrl =  ((speedAdj + 1000) * 360 / 1000);
+	}
+	else {
+		Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
+	}
+
+	if (GV.MK32_Key.CH4_SA == -1000)
+	{
+		Speed_Ctrl = 15;
 	}
 
 	Ref_Speed =   Speed_Ctrl;
@@ -421,10 +453,17 @@ void auto_forward_state_do(void)
 
 void auto_backward_state_do(void)
 {
-	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
+	if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
 	{
-		speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
+		Speed_Ctrl =  (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
-		Speed_Ctrl =  ((speedAdj + 1000) * 360 / 1000);
+	}
+	else {
+		Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
+	}
+
+	if (GV.MK32_Key.CH4_SA == -1000)
+	{
+		Speed_Ctrl = 15;
 	}
 
 	Ref_Speed =   Speed_Ctrl;