From 1c44cd6e148dab4b2635df260bf2ff4a25bf6a1c Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Thu, 30 Apr 2026 16:03:04 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E8=B0=83=E9=80=9A=E6=89=93=E6=A0=87?= =?UTF-8?q?=E3=80=91=E5=88=80=E5=85=B7=E6=94=AF=E6=8C=81=E6=AD=A5=E8=BF=9B?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F=E5=92=8C=E5=88=80=E4=BD=8D=E7=BD=AE=E6=98=BE?= =?UTF-8?q?=E7=A4=BA=EF=BC=8C=E9=9C=80=E8=A6=81=E5=85=88=E6=89=8B=E5=8A=A8?= =?UTF-8?q?=E5=B0=86=E5=88=B0=E7=A7=BB=E5=8A=A8=E5=88=B0=E6=9C=80=E9=AB=98?= =?UTF-8?q?=E7=82=B9=E7=9B=B4=E5=88=B0=E4=BD=8D=E7=BD=AE=E6=98=BE=E7=A4=BA?= =?UTF-8?q?=E4=B8=BA0=E5=90=8E=EF=BC=8CSB=E6=8B=A8=E5=88=B0=E6=9C=80?= =?UTF-8?q?=E4=B8=8B=E8=BE=B9=EF=BC=8C=E6=8E=A8=E5=8F=B3=E6=91=87=E6=9D=86?= =?UTF-8?q?=E4=B8=80=E6=AC=A1=E5=8D=B3=E5=8F=AF=E5=89=8D=E8=BF=9B=E5=B1=8F?= =?UTF-8?q?=E5=B9=95=E9=85=8D=E7=BD=AE=E7=9A=84=E6=AD=A5=E8=BF=9B=E8=B7=9D?= =?UTF-8?q?=E7=A6=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h | 5 +- .../Core/Inc/robot_state.h | 2 + .../BHBF_Robot_Lifting_Lug/Core/Src/FSM.c | 57 +++++++++++++++-- .../Core/Src/robot_state.c | 64 +++++++++++++++++++ .../services/USBSerialPortHelper.java | 9 ++- 5 files changed, 129 insertions(+), 8 deletions(-) diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h index 6d9e9ef..31cd32f 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/FSM.h @@ -25,8 +25,9 @@ typedef enum _Front_MoveSTATE_t Manual_UP_STATE, Manual_DOWN_STATE, MANUAL_LOW_SPEED_UP_STATE, - MANUAL_LOW_SPEED_DOWN_STATE - + MANUAL_LOW_SPEED_DOWN_STATE, + MANUAL_STEP_UP_STATE, + MANUAL_STEP_DOWN_STATE } Front_MoveSTATE_t; typedef enum _Strong_Grinding_Machine_MoveSTATE_t diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h index 91c4055..73c9fea 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Inc/robot_state.h @@ -27,6 +27,8 @@ extern void Manual_Down_State_Do(void); extern void FrontEnd_Halt_State_Do(void); extern void Manual_Low_Speed_Up_State_Do(void); extern void Manual_low_speed_Down_State_Do(void); +extern void Manual_step_Up_State_Do(void); +extern void Manual_step_Down_State_Do(void); // ********强磨机运动控制*********** extern void Strong_Grinding_Machine_Halt_State_Do(void); diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c index ef2c2d0..9f1f45a 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c @@ -46,7 +46,8 @@ transition_t FrontendMoveTransitions[] = { Manual_DOWN_STATE, Manual_Down_State_Do}, { MANUAL_LOW_SPEED_UP_STATE, Manual_Low_Speed_Up_State_Do}, { MANUAL_LOW_SPEED_DOWN_STATE, Manual_low_speed_Down_State_Do}, - + { MANUAL_STEP_UP_STATE, Manual_step_Up_State_Do}, + { MANUAL_STEP_DOWN_STATE, Manual_step_Down_State_Do} }; transition_t StrongGrindingMachineTransitions[] = @@ -164,15 +165,16 @@ void IO_Control() } -uint8_t IsAllowRotation_AutoMode = 0; // 是否允许转刀 -int32_t knife_descent_height = 0; // 屏幕配置值转换后 -int32_t knife_rising_height = 500; // 自动作业停止后默认上升高度为5mm -int32_t knife_current_altitude = 0; // 激光1读数 +#define knife_rising_height 500 // 自动作业停止后默认上升高度为5mm uint8_t auto_work_mode_states = 5; uint8_t auto_work_halt_states = 10; void Automatic_Operation() { + int32_t knife_descent_height = 0; // 屏幕配置值转换后 + int32_t knife_current_altitude = 0; // 激光1读数 + uint8_t IsAllowRotation_AutoMode = 0; // 是否允许转刀 + knife_descent_height = GV.PV.knife_descending_height * 100; if(GV.MK32_Key.CH7_SD == -1000) @@ -291,8 +293,11 @@ int Knife_Detection(void) } +extern int32_t KinfePosition; + void Frontend_Control() { + static int SingleShotFlag = 0; int angle; angle = atan2(GV.MK32_Key.CH1_RY_V, GV.MK32_Key.CH0_RY_H) * 180 / M_PI; @@ -366,7 +371,47 @@ void Frontend_Control() } else{ + // 前进 + if((angle >= 45) && (angle <= 135)) + { + if (SingleShotFlag == 0) + { + CurrentFrontEndState = MANUAL_STEP_UP_STATE; + KinfePosition -= GV.PV.step_len; + SingleShotFlag = 1; + } + return; + } + else if(P_MK32->CH3_LY_H > 100) + { + // 右转 + if((angle >= -45) && (angle <=45)) + { + + return; + } + } + // 后退 + else if((angle >= -135) && (angle < -45)) + { + if (SingleShotFlag == 0) + { + CurrentFrontEndState = MANUAL_STEP_DOWN_STATE; + KinfePosition += GV.PV.step_len; + SingleShotFlag = 1; + } + return; + } + // 左转 + else if(((angle > 135) && (angle <= 180)) || (angle < -135) && (angle >= -179)) + { + return; + } + else { + CurrentFrontEndState = HALT_STATE; + SingleShotFlag = 0; + } } @@ -375,6 +420,7 @@ void Frontend_Control() double real_speed; + void IV_Control() { real_speed = (Act_Speed*360/100/101/6*3.1415926*0.325); @@ -385,6 +431,7 @@ 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; + IV.kinfe_position = KinfePosition; } void Robot_Control() 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 f99f151..b6d1ff7 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c @@ -231,6 +231,8 @@ const uint8_t down_limit = 0; uint8_t Pin2 = -1; //上限位，为0表示限位触发 uint8_t Pin3 = -1; //下限位，为0表示限位触发 +int32_t KinfePosition = 1000; // 初始化为一个不合理的大值，等于1000时显示为未标定 + void Manual_Up_State_Do(void) { int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(SliderSpeed); @@ -247,6 +249,11 @@ void Manual_Up_State_Do(void) if (!Pin2) { GV.LS_FrontEnd_Motor.Target_Velcity = 0; + KinfePosition = 0; + } + else + { + KinfePosition = 1000; //只要不是在步进模式下移动了主轴，且未在限位就恢复为未标定状态 } } @@ -267,6 +274,10 @@ void Manual_Down_State_Do(void) { GV.LS_FrontEnd_Motor.Target_Velcity = 0; } + else + { + KinfePosition = 1000; //只要不是在步进模式下移动了主轴，且未在限位就恢复为未标定状态 + } } // 定义前端升降电机速度，单位0.1mm/s @@ -288,6 +299,11 @@ void Manual_Low_Speed_Up_State_Do(void) if (!Pin2) { GV.LS_FrontEnd_Motor.Target_Velcity = 0; + KinfePosition = 0; + } + else + { + KinfePosition = 1000; //只要不是在步进模式下移动了主轴，且未在限位就恢复为未标定状态 } } @@ -308,11 +324,59 @@ void Manual_low_speed_Down_State_Do(void) { GV.LS_FrontEnd_Motor.Target_Velcity = 0; } + else + { + KinfePosition = 1000; //只要不是在步进模式下移动了主轴，且未在限位就恢复为未标定状态 + } } +#define STEP_LEN_COUNT 18000 // 位置环每18000个count对应1mm + +void Manual_step_Up_State_Do(void) +{ + int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(leisai_low_speed); + GV.LS_FrontEnd_Motor.Target_Position = GV.LS_FrontEnd_Motor.Real_Position - (GV.PV.step_len * STEP_LEN_COUNT); + GV.LS_FrontEnd_Motor.Target_Velcity = SliderSpeed_PPS; + + /*if((GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance >= up_limit) + || (GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance >= up_limit) + || (GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance >= up_limit)) + { + GV.LS_FrontEnd_Motor.Target_Velcity = 0; + }*/ + + if (!Pin2) + { + GV.LS_FrontEnd_Motor.Target_Velcity = 0; + KinfePosition = 0; + } + else + { + } +} +void Manual_step_Down_State_Do(void) +{ + int32_t SliderSpeed_PPS = SliderSpeed_mmps_2_pps(leisai_low_speed); + GV.LS_FrontEnd_Motor.Target_Position = GV.LS_FrontEnd_Motor.Real_Position + (GV.PV.step_len * STEP_LEN_COUNT); + GV.LS_FrontEnd_Motor.Target_Velcity = SliderSpeed_PPS; + /*if((GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance <= down_limit) + || (GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance <= down_limit) + || (GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance <= down_limit)) + { + GV.LS_FrontEnd_Motor.Target_Velcity = 0; + }*/ + if (!Pin3) + { + GV.LS_FrontEnd_Motor.Target_Velcity = 0; + } + else + { + + } +} void FrontEnd_Halt_State_Do(void) { diff --git a/diaoerqiege/diaoerqige_V1.0/app/src/main/java/com/example/diaoerqiegeapp/services/USBSerialPortHelper.java b/diaoerqiege/diaoerqige_V1.0/app/src/main/java/com/example/diaoerqiegeapp/services/USBSerialPortHelper.java index ff3f91f..2d613b9 100644 --- a/diaoerqiege/diaoerqige_V1.0/app/src/main/java/com/example/diaoerqiegeapp/services/USBSerialPortHelper.java +++ b/diaoerqiege/diaoerqige_V1.0/app/src/main/java/com/example/diaoerqiegeapp/services/USBSerialPortHelper.java @@ -358,7 +358,14 @@ public class USBSerialPortHelper implements SerialInputOutputManager.Listener { activityPolish.mainBinding.kinfeSignal.setTextColor(activityPolish.getResources().getColor(android.R.color.holo_red_light)); } - activityPolish.mainBinding.kinfePosition.setText(_toReceiveIV1.getKinfePosition()); + if (_toReceiveIV1.getKinfePosition() == 1000) + { + activityPolish.mainBinding.kinfePosition.setText("未标定"); + } + else + { + activityPolish.mainBinding.kinfePosition.setText(String.format("%d", _toReceiveIV1.getKinfePosition())); + } } }