From c60c9b2247b290bdf1130c9e5fbc1ad4c6ba60b7 Mon Sep 17 00:00:00 2001 From: "HJB\\13752" <13752551070@163.com> Date: Wed, 1 Apr 2026 16:18:39 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E7=94=B5=E7=A3=81=E9=98=80?= =?UTF-8?q?=E9=BB=98=E8=AE=A4=E4=B8=8A=E7=94=B5=E9=AB=98=E7=82=B9=E5=B9=B3?= =?UTF-8?q?=EF=BC=9B=E5=B1=8F=E5=B9=95=E6=98=BE=E7=A4=BA=E5=A4=B9=E5=88=80?= =?UTF-8?q?=E4=BF=A1=E5=8F=B7=EF=BC=9B=E4=BF=AE=E6=94=B9=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E4=BD=9C=E4=B8=9A=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BHBF_Robot_Lifting_Lug.ioc | 8 +- .../BASE/Src/MSP/msp_zhr29_laser_sensor.c | 20 +++-- .../Core/Protobuf/PSource/bsp_IV.pb.h | 11 ++- .../Core/Protobuf/Proto/bsp_IV.proto | 1 + .../BHBF_Robot_Lifting_Lug/Core/Src/FSM.c | 81 ++++++++++++------- .../BHBF_Robot_Lifting_Lug/Core/Src/gpio.c | 6 +- .../BHBF_Robot_Lifting_Lug/Core/Src/main.c | 14 ++-- .../Core/Src/robot_state.c | 4 +- 8 files changed, 93 insertions(+), 52 deletions(-) diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/BHBF_Robot_Lifting_Lug.ioc b/diaoerqiege/BHBF_Robot_Lifting_Lug/BHBF_Robot_Lifting_Lug.ioc index 980000f..445adbe 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/BHBF_Robot_Lifting_Lug.ioc +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/BHBF_Robot_Lifting_Lug.ioc @@ -420,15 +420,17 @@ PA3.GPIO_Label=OUT_3 PA3.Locked=true PA3.PinAttribute=Free PA3.Signal=GPIO_Output -PA4.GPIOParameters=GPIO_Label,PinAttribute +PA4.GPIOParameters=PinState,GPIO_Label,PinAttribute PA4.GPIO_Label=OUT_4 PA4.Locked=true PA4.PinAttribute=Free +PA4.PinState=GPIO_PIN_SET PA4.Signal=GPIO_Output -PA5.GPIOParameters=GPIO_Label,PinAttribute +PA5.GPIOParameters=PinState,GPIO_Label,PinAttribute PA5.GPIO_Label=OUT_5 PA5.Locked=true PA5.PinAttribute=Free +PA5.PinState=GPIO_PIN_SET PA5.Signal=GPIO_Output PA6.GPIOParameters=GPIO_Label,PinAttribute PA6.GPIO_Label=ETH_RST @@ -876,7 +878,7 @@ USART3.FIFOMode=FIFOMODE_ENABLE USART3.IPParameters=VirtualMode-Asynchronous,DMADisableonRxErrorParam,OverrunDisableParam,FIFOMode,BaudRate USART3.OverrunDisableParam=ADVFEATURE_OVERRUN_DISABLE USART3.VirtualMode-Asynchronous=VM_ASYNC -USART6.BaudRate=9600 +USART6.BaudRate=115200 USART6.DMADisableonRxErrorParam=ADVFEATURE_DMA_ENABLEONRXERROR USART6.FIFOMode=FIFOMODE_ENABLE USART6.IPParameters=VirtualMode,OverrunDisableParam,FIFOMode,DMADisableonRxErrorParam,BaudRate,SwapParam diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c index 9d8d430..af32e29 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/BASE/Src/MSP/msp_zhr29_laser_sensor.c @@ -88,7 +88,11 @@ void decode_laser_sensor(uint8_t *buffer, uint16_t length) { if((buffer[0] == 0x01) && (buffer[1] == 0x04) && (buffer[2] == 0x04)) { - *g_zhr29_200_laser_sensor_1 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000; + *g_zhr29_200_laser_sensor_1 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 10 - 15840; + if(*g_zhr29_200_laser_sensor_1 <= 0) + { + *g_zhr29_200_laser_sensor_1 = 0; + } // if(*g_zhr29_200_laser_sensor_1 >= 280) // { // *g_zhr29_200_laser_sensor_1 = 280; @@ -100,8 +104,11 @@ void decode_laser_sensor(uint8_t *buffer, uint16_t length) } else if((buffer[0] == 0x02) && (buffer[1] == 0x04) && (buffer[2] == 0x04)) { - *g_zhr29_200_laser_sensor_2 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000; - + *g_zhr29_200_laser_sensor_2 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 10 - 15850; + if(*g_zhr29_200_laser_sensor_2 <= 0) + { + *g_zhr29_200_laser_sensor_2 = 0; + } // if(*g_zhr29_200_laser_sensor_2 >= 280) // { // *g_zhr29_200_laser_sensor_2 = 280; @@ -114,8 +121,11 @@ void decode_laser_sensor(uint8_t *buffer, uint16_t length) } else if((buffer[0] == 0x03) && (buffer[1] == 0x04) && (buffer[2] == 0x04)) { - *g_zhr29_200_laser_sensor_3 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 1000; - + *g_zhr29_200_laser_sensor_3 = ((buffer[3] << 24) | (buffer[4] << 16) | (buffer[5] << 8) | buffer[6]) / 10 - 15880; + if(*g_zhr29_200_laser_sensor_3 <= 0) + { + *g_zhr29_200_laser_sensor_3 = 0; + } // if(*g_zhr29_200_laser_sensor_3 >= 280) // { // *g_zhr29_200_laser_sensor_3 = 280; diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h index aff5417..4d5d45c 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/PSource/bsp_IV.pb.h @@ -17,6 +17,7 @@ typedef struct _IV_struct_define { int32_t laser_sensor_1_measure_distance; int32_t laser_sensor_2_measure_distance; int32_t laser_sensor_3_measure_distance; + int32_t kinfe_complete_signal; } IV_struct_define; @@ -25,8 +26,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0} -#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0} +#define IV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define IV_struct_define_Robot_Move_Speed_tag 1 @@ -35,6 +36,7 @@ extern "C" { #define IV_struct_define_laser_sensor_1_measure_distance_tag 4 #define IV_struct_define_laser_sensor_2_measure_distance_tag 5 #define IV_struct_define_laser_sensor_3_measure_distance_tag 6 +#define IV_struct_define_kinfe_complete_signal_tag 7 /* Struct field encoding specification for nanopb */ #define IV_struct_define_FIELDLIST(X, a) \ @@ -43,7 +45,8 @@ X(a, STATIC, SINGULAR, INT32, RF_Angle_Roll, 2) \ X(a, STATIC, SINGULAR, INT32, Remote_Status, 3) \ X(a, STATIC, SINGULAR, INT32, laser_sensor_1_measure_distance, 4) \ X(a, STATIC, SINGULAR, INT32, laser_sensor_2_measure_distance, 5) \ -X(a, STATIC, SINGULAR, INT32, laser_sensor_3_measure_distance, 6) +X(a, STATIC, SINGULAR, INT32, laser_sensor_3_measure_distance, 6) \ +X(a, STATIC, SINGULAR, INT32, kinfe_complete_signal, 7) #define IV_struct_define_CALLBACK NULL #define IV_struct_define_DEFAULT NULL @@ -54,7 +57,7 @@ extern const pb_msgdesc_t IV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_IV_PB_H_MAX_SIZE IV_struct_define_size -#define IV_struct_define_size 64 +#define IV_struct_define_size 75 #ifdef __cplusplus } /* extern "C" */ diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto index de24420..b3d914f 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Protobuf/Proto/bsp_IV.proto @@ -11,5 +11,6 @@ message IV_struct_define{ int32 laser_sensor_1_measure_distance = 4; int32 laser_sensor_2_measure_distance = 5; int32 laser_sensor_3_measure_distance = 6; + int32 kinfe_complete_signal = 7; }; diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c index 484983a..ec48a38 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/FSM.c @@ -67,13 +67,14 @@ void Fsm_Init() void GF_Dispatch() { IV_Control(); - - + IO_Control(); Mode_Control(); // 限位安全检测 Limit_Detection(); + // 急停 + EmergencyStopControl(); action_perfrom(MoveTransitions,sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState); action_perfrom(FrontendMoveTransitions, @@ -82,6 +83,19 @@ void GF_Dispatch() sizeof(StrongGrindingMachineTransitions) / sizeof(transition_t), StrongGrindingMachineCurrentState); } +void EmergencyStopControl() +{ + if((GV.MK32_Key.CH8_SE == -1000) && GV.MK32_Key.CH9_SF == -1000) + { + CurrentMoveState = Move_HALT; + CurrentFrontEndState = HALT_STATE; + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; + } + else{ + + } +} + uint8_t upper_limit_position = 0; uint8_t lower_limit_position = 0; @@ -128,6 +142,19 @@ void Mode_Control() void IO_Control() { GF_BSP_GPIO_SetIO(4, 1); + + if(StrongGrindingMachineCurrentState == STRONG_GRINDING_MACHINE_MOTION_STATE) + { + GF_BSP_GPIO_SetIO(5, 0); + } + else if(StrongGrindingMachineCurrentState == STRONG_GRINDING_MACHINE_HALT_STATE) + { + GF_BSP_GPIO_SetIO(5, 1); + } + else{ + + } + } uint8_t IsAllowRotation_AutoMode = 0; @@ -150,32 +177,34 @@ void Automatic_Operation() case 5: knife_current_altitude = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; auto_work_mode_states = 10; + auto_work_halt_states = 10; break; case 10: CurrentFrontEndState = Manual_DOWN_STATE; + IsAllowRotation_AutoMode = Knife_Detection(); + if(IsAllowRotation_AutoMode == 1) + { + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE; + } + else{ + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; + } if(knife_current_altitude - GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance >= knife_descent_height) { CurrentFrontEndState = HALT_STATE; - auto_work_halt_states = 10; auto_work_mode_states = 20; + } break; case 20: - IsAllowRotation_AutoMode = Knife_Detection(); - if(IsAllowRotation_AutoMode == 1) + + delay_counts = delay_counts + 1; + if(delay_counts >= 500) { - StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_MOTION_STATE; - delay_counts = delay_counts + 1; - if(delay_counts >= 500) - { - delay_counts = 0; - CurrentMoveState = AUTO_FORWARD; - } - } - else{ - StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; + delay_counts = 0; + CurrentMoveState = AUTO_FORWARD; } break; @@ -192,15 +221,13 @@ void Automatic_Operation() else{ delay_counts = 0; CurrentMoveState = Move_HALT; - StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; - if(auto_work_mode_states == 10) + if(auto_work_mode_states == 5) { CurrentFrontEndState = HALT_STATE; - auto_work_mode_states = 5; + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; } - else if(auto_work_mode_states == 20) - { + else{ switch(auto_work_halt_states) { case 10: @@ -209,20 +236,17 @@ void Automatic_Operation() break; case 20: CurrentFrontEndState = Manual_UP_STATE; - if(GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance - knife_current_altitude >= knife_rising_height) { - auto_work_mode_states = 5; CurrentFrontEndState = HALT_STATE; + StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; + auto_work_mode_states = 5; } break; } + } } - else{ - auto_work_mode_states = 5; - } - } } @@ -238,15 +262,11 @@ void Strong_Grinding_Machine_Control() } else{ StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; - // 只有在刀到位且不转的时候和刀未到位的时候可以控制 - IO_Control(); } } else{ StrongGrindingMachineCurrentState = STRONG_GRINDING_MACHINE_HALT_STATE; - // 只有在刀到位且不转的时候和刀未到位的时候可以控制 - IO_Control(); } } @@ -372,6 +392,7 @@ void IV_Control() IV.laser_sensor_1_measure_distance = GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; 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; } void Robot_Control() diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/gpio.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/gpio.c index abfd0ac..f97d86f 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/gpio.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/gpio.c @@ -56,8 +56,10 @@ void MX_GPIO_Init(void) |RS485_4_DIR_Pin|E22_RST_Pin, GPIO_PIN_RESET); /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOA, OUT_2_Pin|OUT_3_Pin|OUT_4_Pin|OUT_5_Pin - |ETH_RST_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(GPIOA, OUT_2_Pin|OUT_3_Pin|ETH_RST_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOA, OUT_4_Pin|OUT_5_Pin, GPIO_PIN_SET); /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOE, EEPROM_WP_Pin|EEPROM_SCL_Pin|E22_M0_Pin, GPIO_PIN_RESET); diff --git a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/main.c b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/main.c index b917441..be4adbe 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/main.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/main.c @@ -272,12 +272,14 @@ void CV_GV_Init() P_MK32 = &GV.MK32_Key; - // 强磨机默认发送停止命令 + // 强磨机默认发送停止命? strong_grinding_machine_cmd = 0; - // 电磁阀默认关闭 + // 电磁?默认关闭 GF_BSP_GPIO_SetIO(4, 1); + // 冷却通气?默认关闭，动作与主轴同步 + GF_BSP_GPIO_SetIO(5, 1); - // �?光测距传感器指针同步 + // ??光测距传感器指针同步 g_zhr29_200_laser_sensor_1 = &GV.ZHR29_200_measure_results.laser_sensor_1_measure_distance; g_zhr29_200_laser_sensor_2 = &GV.ZHR29_200_measure_results.laser_sensor_2_measure_distance; g_zhr29_200_laser_sensor_3 = &GV.ZHR29_200_measure_results.laser_sensor_3_measure_distance; @@ -342,17 +344,17 @@ void GF_Robot_Init() SlideMotor_Controller_intialize_CAN2(&FD_CAN_2_Handler);//雷赛电机 - // �?光测距传感器�?115200 + // ??光测距传感器??115200 zhr29_200_laser_sensor_intialize(&RS_485_3_UART_Handler); - // 强磨机 + // 强磨? strong_grinding_machine_intialize(&RS_485_2_UART_Handler); Fsm_Init(); uint8_t _state = 1; - _state = _state & GF_BSP_TIMER_Init(); //定时器最后启�????? + _state = _state & GF_BSP_TIMER_Init(); //定时器最后启?????? } 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 ad7a88a..85127c0 100644 --- a/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c +++ b/diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c @@ -225,8 +225,8 @@ int32_t SliderSpeed_mmps_2_pps(int32_t mmps) // 定义前端升降电机速度，单位0.1mm/s static const int32_t SliderSpeed = 50; -const uint8_t up_limit = 215; -const uint8_t down_limit = 150; +const uint32_t up_limit = 6200; +const uint8_t down_limit = 0; void Manual_Up_State_Do(void) {