增加立面平面逻辑

diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

@@ -448,8 +448,18 @@ void auto_forward_state_do(void)
 	Act_Speed =Speed_Ctrl;
 	if(Ref_Speed >= 5000) Ref_Speed = 5000;
 
+    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,8 +480,18 @@ void auto_backward_state_do(void)
 	Act_Speed =Speed_Ctrl;
 	if(Ref_Speed >= 5000) Ref_Speed = 5000;
 
+    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;        
+    }
+}