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())); + } } }