增加立面平面逻辑

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

@@ -448,7 +448,17 @@ void auto_forward_state_do(void)
 	Act_Speed =Speed_Ctrl;
 	if(Ref_Speed >= 5000) Ref_Speed = 5000;
 
-    SetMoveMotorSpeed(Ref_Speed);
+    if (0 == GV.PV.knife_descending_height)
+    {
+        SetMoveMotorSpeed(Ref_Speed);
+    }
+    else
+    {
+        GV.LeftFrontMotor.Target_Velcity = Ref_Speed;
+    	GV.RightFrontMotor.Target_Velcity = -Ref_Speed;
+    	GV.LeftBackMotor.Target_Velcity = Ref_Speed;
+    	GV.RightBackMotor.Target_Velcity = -Ref_Speed;        
+    }
 }
 
 void auto_backward_state_do(void)
@@ -470,7 +480,17 @@ void auto_backward_state_do(void)
 	Act_Speed =Speed_Ctrl;
 	if(Ref_Speed >= 5000) Ref_Speed = 5000;
 
-    SetMoveMotorSpeed(-Ref_Speed);
+    if (0 == GV.PV.knife_descending_height)
+    {
+        SetMoveMotorSpeed(-Ref_Speed);
+    }
+    else
+    {
+        GV.LeftFrontMotor.Target_Velcity = -Ref_Speed;
+        GV.RightFrontMotor.Target_Velcity = Ref_Speed;
+        GV.LeftBackMotor.Target_Velcity = -Ref_Speed;
+        GV.RightBackMotor.Target_Velcity = Ref_Speed;        
+    }
 }