From 2e53afede58d628e5640a00a23028939a2e82058 Mon Sep 17 00:00:00 2001 From: "L1NG\\42961" <429616286@qq.com> Date: Fri, 13 Mar 2026 10:57:22 +0800 Subject: [PATCH] EmergencyStop --- Core/BASE/Protobuf/PSource/bsp_GV.pb.h | 10 +++--- Core/BASE/Protobuf/Proto/bsp_GV.proto | 4 +-- Core/FSM/Inc/Handset_Status_Setting.h | 2 ++ Core/FSM/Src/Handset_Status_Setting.c | 48 ++++++++++++++++++++++++-- Core/FSM/Src/fsm_state_control.c | 13 +++++++ Core/FSM/Src/robot_move_actions.c | 7 ++-- 疑问记录.txt | 15 ++++++-- 7 files changed, 84 insertions(+), 15 deletions(-) diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index 7150ff8..14d5324 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -19,8 +19,8 @@ /* Struct definitions */ typedef struct _GV_struct_define { int32_t TempatureE_2C; /* E_2C=0.01 Celsius 0.01摄氏度 */ - int32_t Left_Compensation; /* E_2D =0.01 Degree 0.01度 */ - int32_t Right_Compensation; /* 0.01度 */ + float Left_Compensation; /* E_2D =0.01 Degree 0.01度 */ + float Right_Compensation; /* 0.01度 */ float Robot_Angle_Desire; /* 机器人期望角度 //改为 float，单位：度（不再需要0.01缩放） */ float Robot_Move_Speed; /* 0.1rpm */ float Robot_Desired_Speed; /* 0.1rpm */ @@ -80,8 +80,8 @@ extern "C" { /* Struct field encoding specification for nanopb */ #define GV_struct_define_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, INT32, TempatureE_2C, 1) \ -X(a, STATIC, SINGULAR, INT32, Left_Compensation, 2) \ -X(a, STATIC, SINGULAR, INT32, Right_Compensation, 3) \ +X(a, STATIC, SINGULAR, FLOAT, Left_Compensation, 2) \ +X(a, STATIC, SINGULAR, FLOAT, Right_Compensation, 3) \ X(a, STATIC, SINGULAR, FLOAT, Robot_Angle_Desire, 4) \ X(a, STATIC, SINGULAR, FLOAT, Robot_Move_Speed, 5) \ X(a, STATIC, SINGULAR, FLOAT, Robot_Desired_Speed, 6) \ @@ -116,7 +116,7 @@ extern const pb_msgdesc_t GV_struct_define_msg; /* Maximum encoded size of messages (where known) */ #define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size -#define GV_struct_define_size 1768 +#define GV_struct_define_size 1756 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/Proto/bsp_GV.proto b/Core/BASE/Protobuf/Proto/bsp_GV.proto index 06bee0e..0c24462 100644 --- a/Core/BASE/Protobuf/Proto/bsp_GV.proto +++ b/Core/BASE/Protobuf/Proto/bsp_GV.proto @@ -12,8 +12,8 @@ import "bsp_IO.proto"; message GV_struct_define { int32 TempatureE_2C=1; // E_2C=0.01 Celsius 0.01摄氏度 - int32 Left_Compensation=2; // E_2D =0.01 Degree 0.01度 - int32 Right_Compensation=3; //0.01度 + float Left_Compensation=2; // E_2D =0.01 Degree 0.01度 + float Right_Compensation=3; //0.01度 float Robot_Angle_Desire=4; //机器人期望角度 //改为 float，单位：度（不再需要0.01缩放） float Robot_Move_Speed=5; // 0.1rpm float Robot_Desired_Speed=6; // 0.1rpm diff --git a/Core/FSM/Inc/Handset_Status_Setting.h b/Core/FSM/Inc/Handset_Status_Setting.h index e2b6edd..8cf9c44 100644 --- a/Core/FSM/Inc/Handset_Status_Setting.h +++ b/Core/FSM/Inc/Handset_Status_Setting.h @@ -25,6 +25,8 @@ typedef enum { INPUT_KEY_AUTO_WORK_UP, // CH7_SD = -1000 自动作业上 INPUT_KEY_AUTO_WORK_DOWN, // CH7_SD = 1000 自动作业下 + EMERGENCE_STOP, // SE和SF同时按下触发急停 + KEY_COUNT, // 输入总数 } InputEvent; diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c index f46a35b..9e7e6e9 100644 --- a/Core/FSM/Src/Handset_Status_Setting.c +++ b/Core/FSM/Src/Handset_Status_Setting.c @@ -147,56 +147,94 @@ static InputEvent CalculateRockerEvent(void) return INPUT_ROCKER_STOP; } - +// 摇杆事件计算 +static InputEvent Check_Emergency_Event(void) +{ + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } +} // ============ 各模式事件获取函数 ============ static InputEvent GetHaltModeEvents(void) { - + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckAllKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetManualModeEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckCommonKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetHorizontalModeEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckCommonKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetFlatModeEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckCommonKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetVerticalLeftModeEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckCommonKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetVerticalRightModeEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } InputEvent key = CheckCommonKeys(); return (key != INPUT_NONE) ? key : CalculateRockerEvent(); } static InputEvent GetRegionalHorizontalTaskEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP; return CalculateRockerEvent(); } static InputEvent GetRegionalFlatTaskEvents(void) { + if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) + { + return EMERGENCE_STOP; + } if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP; return CalculateRockerEvent(); } @@ -222,9 +260,11 @@ void PV_control(void) void IV_control(void) { IV.Robot_Move_Deri_Speed = 0; + IV.SystemError = GV.SystemErrorData.Com_Error_Code; IV.Robot_Gyro = GV.TL720DParameters.RF_Angle_Roll; IV.Is_Online = GV.P_MK32.IsOnline; IV.SystemError = GV.SystemErrorData.Com_Error_Code; + IV.Robot_Move_Deri_Speed = GV.Left_Speed_M_min*10; // IV.Left_Motor_Err = GV.LeftMotor.Motor_Fault; // IV.Right_Motor_Err = GV.RightMotor.Motor_Fault; // IV.TimeStamp = SystemTimeMiliCount; @@ -243,7 +283,7 @@ void IV_control(void) * @param pComp 指向补偿变量的指针（int32_t*） * @param pCnt 指向计数器的指针（int*） */ -static void Update_Single_Compensation(int32_t chVal, int32_t* pComp, int* pCnt) +static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt) { if (chVal > COMP_DEADZONE) { (*pCnt)++; @@ -271,8 +311,10 @@ void Compensation_Update() static int leftCnt = 0; /* 左通道计数器 */ static int rightCnt = 0; /* 右通道计数器 */ + Update_Single_Compensation(P_MK32->CH14_LT, &GV.Left_Compensation, &leftCnt); Update_Single_Compensation(P_MK32->CH15_RT, &GV.Right_Compensation, &rightCnt); + } diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index 18400e9..dbd8089 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -38,6 +38,7 @@ static void change_Road_group(void); static void vertical_forward_group(void); /* 虽未在表中使用，但保留 */ static void vertical_backward_group(void); static void horizontal_auto_group(void); +static void Emergency_Stop(void); /*=========================== 6. 函数指针表 ===========================*/ /* 动作映射表：模式 × 按键 -> 执行函数 */ @@ -54,6 +55,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = { [INPUT_ROCKER_TURN_RIGHT] = manual_right_group, [INPUT_KEY_FORWARD_CRUISE] = manual_forward_group, [INPUT_KEY_BACKWARD_CRUISE] = manual_backward_group, + [EMERGENCE_STOP] = Emergency_Stop, }, // Horizontal_Mode - 水平模式 @@ -67,6 +69,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = { [INPUT_KEY_BACKWARD_CRUISE] = horizontal_backward_group, [INPUT_KEY_AUTO_WORK_UP] = horizontal_auto_group, [INPUT_KEY_LANE_CHANGE_UP] = change_Road_group, + [EMERGENCE_STOP] = Emergency_Stop, // 其他按键暂未实现，留空（NULL） }, @@ -85,6 +88,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = { [INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group, [INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group, [INPUT_KEY_LANE_CHANGE_UP] = change_Road_group, + [EMERGENCE_STOP] = Emergency_Stop, // 其他预留 }, @@ -98,6 +102,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = { [INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group, [INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group, [INPUT_KEY_LANE_CHANGE_UP] = change_Road_group, + [EMERGENCE_STOP] = Emergency_Stop, // 未实现 }, @@ -178,6 +183,14 @@ static void vertical_backward_group(void) vertical_forward(); } +static void Emergency_Stop(void) +{ + GV.SwingMotor.Position_immediately1_Lag2=1; + GV.SwingMotor.Tar_Position_Velcity_Degree_S=0; + GV.Left_Speed_M_min=0; + GV.Right_Speed_M_min=0; + flag_reset(); +} /*=========================== 8. 对外接口函数（需外部调用） ===========================*/ /*------------------------------------------------------------------- diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index 35fd8b6..b984c1a 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -93,7 +93,7 @@ int Change_stop_flag=0; static int S1_Last_Value; static int S2_Last_Value; -int center_angle; +float center_angle=0; int offset_angle; int center_position; //中间位置-944334 @@ -226,6 +226,7 @@ void Turn_Right(void) void Robot_Stop(void) { + Compensation_Update(); get_swing_mode(); Robot_Swing_Operation_Function(); GV.Left_Speed_M_min = 0; @@ -484,10 +485,10 @@ static void get_swing_mode(void) { if(GV.PV.Robot_symmetricalOrNot==1) { - if(GV.PV.Robot_Swing_Range_Angle!=offset_angle) + if(GV.PV.Robot_Swing_Range_Angle!=offset_angle && GV.SwingMotor.Real_Position!=0) { offset_angle=GV.PV.Robot_Swing_Range_Angle; - center_angle=-(GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2; + center_angle=-((float)GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2; } diff --git a/疑问记录.txt b/疑问记录.txt index 5dba9f7..8493e46 100644 --- a/疑问记录.txt +++ b/疑问记录.txt @@ -44,8 +44,19 @@ GV_Robot_backMode=(int)CV.PV.Robot_backMode; 绑定电机 测距 调试摆臂 调内部PID - 问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 实现急停两个按键 + 问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合） EPRROM的CV的配置 自动巡航√ 非对称摆臂√ 实现急停两个按键 - 变量的传导 用不同的标志位 + 变量的传导√ 用不同的标志位√ 调整左补偿与右补偿 + + +B03-14摆臂机器人3月13日计划完成： + + 1.在水平模式中，实现作业过程中的左补偿与右补偿 + + 2.读取报错信息 + + 3.在android软件上将能够显示的参数显示出来 + + 4.最小二乘法拟合换道精度，之前的参数测试过之后发现不太准，今天需要再测一遍