diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c index 7fc0942..3f84ae9 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c +++ b/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; + } }