diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
index 15fa460..07f8dd5 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
index fb278e6..14ada2d 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
@@ -108,13 +108,25 @@ void Limit_Detection()
uint8_t Mode_Select_State = MANUAL_OPERATION;
+uint8_t Mode_Init = 0;// 初始化时第一次升刀的流程
void Mode_Control()
{
- Mode_Select_State = GV.PV.robot_operation_mode;
+ if (0 == Mode_Init)
+ {
+ Mode_Select_State = -255;// 这里单纯是为了区分正常值，对无符号类型赋值为负会使其变为一个大值
+ }
+ else
+ {
+ Mode_Select_State = GV.PV.robot_operation_mode;
+ }
switch(Mode_Select_State)
{
+ case -255:
+ // 上电时刀具先回到最高位置
+ Automatic_Init();
+ break;
case MANUAL_OPERATION:
// 机器人本体控制
Robot_Control();
@@ -169,6 +181,13 @@ void IO_Control()
}
+void Automatic_Init()
+{
+ CurrentMoveState = Move_HALT;
+ CurrentFrontEndState = Manual_UP_STATE;
+ StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE;
+}
+
#define RISE_KINFE_STATE -11
#define FORWARD_STATE 11
#define BACKWARD_STATE 21
@@ -185,10 +204,10 @@ void Automatic_Laser_Scanning()
CurrentFrontEndState = HALT_STATE;
knife_descent_height = 0;
+ StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; //刀具不转动
+
if(GV.MK32_Key.CH7_SD == 1000)
{
- StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; //刀具不转动
-
int32_t a = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance;
int32_t b = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance;
int32_t c = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance;
@@ -245,7 +264,7 @@ void Automatic_Laser_Scanning()
ScanTimeCount = HAL_GetTick();
ScanState = RISE_KINFE_STATE;
CurrentMoveState = Move_HALT;
- CurrentFrontEndState = HALT_STATE;
+ CurrentFrontEndState = HALT_STATE;
MaxLaserSensor = 10000;// 恢复非法值
}
else
@@ -253,7 +272,7 @@ void Automatic_Laser_Scanning()
ScanTimeCount = HAL_GetTick();
ScanState = RISE_KINFE_STATE;
CurrentMoveState = Move_HALT;
- CurrentFrontEndState = HALT_STATE;
+ CurrentFrontEndState = HALT_STATE;
}
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 b792187..4451a23 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
@@ -236,6 +236,8 @@ int StrongSingleShotFlag = 0;
#define SAFE_MOTOR_COUNT (67*20000) // 电机脉冲安全保护，防止光电限位失效后导致的意外情况
+extern uint8_t Mode_Init;// 初始化时第一次升刀的流程
+
void Manual_Up_State_Do(void)
{
int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(SliderSpeed);
@@ -253,6 +255,7 @@ void Manual_Up_State_Do(void)
{
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
+ Mode_Init = 1;// 表示自动初始化完成
}
if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
@@ -305,6 +308,7 @@ void Manual_Low_Speed_Up_State_Do(void)
{
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
+ Mode_Init = 1;// 表示自动初始化完成
}
if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
@@ -360,6 +364,7 @@ void Manual_step_Up_State_Do(void)
{
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
+ Mode_Init = 1;// 表示自动初始化完成
}
if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)