diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
index 8ac6494..c72102f 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h
@@ -39,7 +39,8 @@ typedef enum _Strong_Grinding_Machine_MoveSTATE_t
typedef enum _Robot_Operation_Mode_t
{
- MANUAL_OPERATION = 1,
+ INIT_OPERATION = 1,
+ MANUAL_OPERATION,
AUTO_OPERATION,
AUTO_LASER_SCAN
} Robot_Operation_Mode_t;
diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
index 14ada2d..e4ef2f8 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c
@@ -114,7 +114,7 @@ void Mode_Control()
{
if (0 == Mode_Init)
{
- Mode_Select_State = -255;// 这里单纯是为了区分正常值，对无符号类型赋值为负会使其变为一个大值
+ Mode_Select_State = INIT_OPERATION;
}
else
{
@@ -123,7 +123,7 @@ void Mode_Control()
switch(Mode_Select_State)
{
- case -255:
+ case INIT_OPERATION:
// 上电时刀具先回到最高位置
Automatic_Init();
break;
@@ -299,7 +299,7 @@ void Automatic_Operation()
if (knife_flag)
{
// 由于这里是循环，每次开自动作业只执行一次
- knife_descent_height += GV.PV.knife_descending_height * 100;
+ knife_descent_height += GV.PV.step_len * 100;
knife_flag = 0;
}
@@ -556,6 +556,12 @@ void IV_Control()
IV.laser_sensor_2_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance;
IV.laser_sensor_3_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance;
IV.kinfe_complete_signal = IsAllowRotation;
+
+ if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition < 0)
+ {
+ KinfePosition = GV.LS_FrontEnd_Motor.Real_Position; // 这里是确保不会出现负值
+ }
+
IV.kinfe_position = (KinfePosition == 1000) ? KinfePosition : (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition);// 这里上传的是原始数据，需要APP上缩小20000倍
IV.max_laser_sensor = MaxLaserSensor;
}
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 4451a23..aa84a89 100644
--- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
+++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
@@ -238,6 +238,19 @@ int StrongSingleShotFlag = 0;
extern uint8_t Mode_Init;// 初始化时第一次升刀的流程
+static void LimitDownPosition(void)
+{
+ if (!Pin3)
+ {
+ GV.LS_FrontEnd_Motor.Target_Velcity = 0;
+ }
+
+ if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
+ {
+ GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
+ }
+}
+
void Manual_Up_State_Do(void)
{
int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(SliderSpeed);
@@ -257,11 +270,6 @@ void Manual_Up_State_Do(void)
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
Mode_Init = 1;// 表示自动初始化完成
}
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
}
void Manual_Down_State_Do(void)
@@ -277,15 +285,7 @@ void Manual_Down_State_Do(void)
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
}*/
- if (!Pin3)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0;
- }
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
+ LimitDownPosition();
}
// 定义前端升降电机速度，单位0.1mm/s
@@ -310,11 +310,6 @@ void Manual_Low_Speed_Up_State_Do(void)
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
Mode_Init = 1;// 表示自动初始化完成
}
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
}
void Manual_low_speed_Down_State_Do(void)
@@ -330,15 +325,7 @@ void Manual_low_speed_Down_State_Do(void)
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
}*/
- if (!Pin3)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0;
- }
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
+ LimitDownPosition();
}
#define STEP_LEN_COUNT 20000 // 位置环每20000个count对应1mm
@@ -366,11 +353,6 @@ void Manual_step_Up_State_Do(void)
KinfePosition = GV.LS_FrontEnd_Motor.Real_Position;
Mode_Init = 1;// 表示自动初始化完成
}
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
}
void Manual_step_Down_State_Do(void)
@@ -390,15 +372,7 @@ void Manual_step_Down_State_Do(void)
GV.LS_FrontEnd_Motor.Target_Velcity = 0;
}*/
- if (!Pin3)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0;
- }
-
- if (GV.LS_FrontEnd_Motor.Real_Position - KinfePosition >= SAFE_MOTOR_COUNT)
- {
- GV.LS_FrontEnd_Motor.Target_Velcity = 0; // 电机脉冲数急停
- }
+ LimitDownPosition();
}
void FrontEnd_Halt_State_Do(void)
diff --git a/jueyuanzijiance/Insulator_Inspection/.settings/language.settings.xml b/jueyuanzijiance/Insulator_Inspection/.settings/language.settings.xml
index e4dea11..a25c702 100644
--- a/jueyuanzijiance/Insulator_Inspection/.settings/language.settings.xml
+++ b/jueyuanzijiance/Insulator_Inspection/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+