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 d33bbad..d78382a 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c @@ -60,6 +60,10 @@ void SetMoveMotorSpeed(float Speed) } } +#define ONCE_MOVE_COUNT 45 +int g_iMoveCount = 0; +int g_iMoveOnce = 0; + void Manual_State_Do(void) { double Rocker_Range = 2000; //摇杆范围 @@ -233,15 +237,58 @@ void low_Speed_Manual_State_Do(void) } else { - R1_Speed_Con = 0; - L1_Speed_Con = 0; - R2_Speed_Con = 0; - L2_Speed_Con = 0; + int iTargetTimeCount = (int)(ONCE_MOVE_COUNT / 1 * 45);//车速写死1m/min + if (GV.MK32_Key.CH5_SB == -1000) + { + if (g_iMoveOnce == 0) + { + g_iMoveOnce = 1; + g_iMoveCount = HAL_GetTick(); + } + R1_Speed_Con = -1; + L1_Speed_Con = 1; + R2_Speed_Con = -1; + L2_Speed_Con = 1; + if (HAL_GetTick() - g_iMoveCount > iTargetTimeCount) + { + R1_Speed_Con = 0; + L1_Speed_Con = 0; + R2_Speed_Con = 0; + L2_Speed_Con = 0; + } + } + else if (GV.MK32_Key.CH5_SB == 1000) + { + if (g_iMoveOnce == 0) + { + g_iMoveOnce = 1; + g_iMoveCount = HAL_GetTick(); + } + R1_Speed_Con = 1; + L1_Speed_Con = -1; + R2_Speed_Con = 1; + L2_Speed_Con = -1; + if (HAL_GetTick() - g_iMoveCount > iTargetTimeCount) + { + R1_Speed_Con = 0; + L1_Speed_Con = 0; + R2_Speed_Con = 0; + L2_Speed_Con = 0; + } + } + else + { + g_iMoveOnce = 0; + R1_Speed_Con = 0; + L1_Speed_Con = 0; + R2_Speed_Con = 0; + L2_Speed_Con = 0; + } } - GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed; - GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed; - GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed; - GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed; + GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * 166;//166对应1m/min + GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * 166; + GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * 166; + GV.RightBackMotor.Target_Velcity = R2_Speed_Con * 166; } void high_Speed_Manual_State_Do(void)