diff --git a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h index 14d5324..71d9c15 100644 --- a/Core/BASE/Protobuf/PSource/bsp_GV.pb.h +++ b/Core/BASE/Protobuf/PSource/bsp_GV.pb.h @@ -45,6 +45,8 @@ typedef struct _GV_struct_define { ErrorData SystemErrorData; bool has_PV; PV_struct_define PV; /* 用户配置数据 */ + float Tar_Position_angle; /* 位置环模式，期望角度 */ + int32_t Tar_Position_Velcity_Degree_S; /* 位置环模式，速度(m/min) */ } GV_struct_define; @@ -53,8 +55,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define GV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, MSP_TL720DParameters_init_default, false, IO_Data_init_default, false, ErrorData_init_default, false, PV_struct_define_init_default} -#define GV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, MSP_TL720DParameters_init_zero, false, IO_Data_init_zero, false, ErrorData_init_zero, false, PV_struct_define_init_zero} +#define GV_struct_define_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, TT_MotorParameters_init_default, false, MSP_TL720DParameters_init_default, false, IO_Data_init_default, false, ErrorData_init_default, false, PV_struct_define_init_default, 0, 0} +#define GV_struct_define_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false, SP_MSP_MK32_Button_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, TT_MotorParameters_init_zero, false, MSP_TL720DParameters_init_zero, false, IO_Data_init_zero, false, ErrorData_init_zero, false, PV_struct_define_init_zero, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define GV_struct_define_TempatureE_2C_tag 1 @@ -76,6 +78,8 @@ extern "C" { #define GV_struct_define_IO_tag 17 #define GV_struct_define_SystemErrorData_tag 18 #define GV_struct_define_PV_tag 19 +#define GV_struct_define_Tar_Position_angle_tag 20 +#define GV_struct_define_Tar_Position_Velcity_Degree_S_tag 21 /* Struct field encoding specification for nanopb */ #define GV_struct_define_FIELDLIST(X, a) \ @@ -97,7 +101,9 @@ X(a, STATIC, OPTIONAL, MESSAGE, SwingMotor, 15) \ X(a, STATIC, OPTIONAL, MESSAGE, TL720DParameters, 16) \ X(a, STATIC, OPTIONAL, MESSAGE, IO, 17) \ X(a, STATIC, OPTIONAL, MESSAGE, SystemErrorData, 18) \ -X(a, STATIC, OPTIONAL, MESSAGE, PV, 19) +X(a, STATIC, OPTIONAL, MESSAGE, PV, 19) \ +X(a, STATIC, SINGULAR, FLOAT, Tar_Position_angle, 20) \ +X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_Degree_S, 21) #define GV_struct_define_CALLBACK NULL #define GV_struct_define_DEFAULT NULL #define GV_struct_define_P_MK32_MSGTYPE SP_MSP_MK32_Button @@ -116,7 +122,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 1756 +#define GV_struct_define_size 1720 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h b/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h index 3dc86ed..83f036a 100644 --- a/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h +++ b/Core/BASE/Protobuf/PSource/msp_ZQ_MotorParameters.pb.h @@ -39,8 +39,6 @@ typedef struct _TT_MotorParameters { int32_t Position_immediately1_Lag2; /* 位置环模式，立即生效1，延后生效2 */ int32_t Tar_Position_count; /* 位置环模式，期望位置 */ int32_t Tar_Position_Velcity_RPM; /* 位置环模式，速度 */ - float Tar_Position_angle; /* 位置环模式，期望角度 */ - int32_t Tar_Position_Velcity_Degree_S; /* 位置环模式，速度(m/min) */ } TT_MotorParameters; @@ -49,8 +47,8 @@ extern "C" { #endif /* Initializer values for message structs */ -#define TT_MotorParameters_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} -#define TT_MotorParameters_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define TT_MotorParameters_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define TT_MotorParameters_init_zero {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} /* Field tags (for use in manual encoding/decoding) */ #define TT_MotorParameters_MotorID_tag 1 @@ -81,8 +79,6 @@ extern "C" { #define TT_MotorParameters_Position_immediately1_Lag2_tag 46 #define TT_MotorParameters_Tar_Position_count_tag 47 #define TT_MotorParameters_Tar_Position_Velcity_RPM_tag 49 -#define TT_MotorParameters_Tar_Position_angle_tag 50 -#define TT_MotorParameters_Tar_Position_Velcity_Degree_S_tag 51 /* Struct field encoding specification for nanopb */ #define TT_MotorParameters_FIELDLIST(X, a) \ @@ -113,9 +109,7 @@ X(a, STATIC, SINGULAR, DOUBLE, Real_Disatnce, 44) \ X(a, STATIC, SINGULAR, INT32, Start_Measuring, 45) \ X(a, STATIC, SINGULAR, INT32, Position_immediately1_Lag2, 46) \ X(a, STATIC, SINGULAR, INT32, Tar_Position_count, 47) \ -X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_RPM, 49) \ -X(a, STATIC, SINGULAR, FLOAT, Tar_Position_angle, 50) \ -X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_Degree_S, 51) +X(a, STATIC, SINGULAR, INT32, Tar_Position_Velcity_RPM, 49) #define TT_MotorParameters_CALLBACK NULL #define TT_MotorParameters_DEFAULT NULL @@ -126,7 +120,7 @@ extern const pb_msgdesc_t TT_MotorParameters_msg; /* Maximum encoded size of messages (where known) */ #define MSP_ZQ_MOTORPARAMETERS_PB_H_MAX_SIZE TT_MotorParameters_size -#define TT_MotorParameters_size 346 +#define TT_MotorParameters_size 328 #ifdef __cplusplus } /* extern "C" */ diff --git a/Core/BASE/Protobuf/Proto/bsp_GV.proto b/Core/BASE/Protobuf/Proto/bsp_GV.proto index 0c24462..1da43c2 100644 --- a/Core/BASE/Protobuf/Proto/bsp_GV.proto +++ b/Core/BASE/Protobuf/Proto/bsp_GV.proto @@ -30,6 +30,8 @@ message GV_struct_define IO_Data IO=17; ErrorData SystemErrorData=18; PV_struct_define PV=19;//用户配置数据 + float Tar_Position_angle=20; //位置环模式，期望角度 + int32 Tar_Position_Velcity_Degree_S=21; //位置环模式，速度(m/min) }; diff --git a/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto b/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto index d97761f..4d34100 100644 --- a/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto +++ b/Core/BASE/Protobuf/Proto/msp_ZQ_MotorParameters.proto @@ -31,8 +31,7 @@ message TT_MotorParameters { int32 Tar_Position_Velcity_RPM=49; //位置环模式，速度 double Real_Disatnce=44; int32 Start_Measuring=45;//开始测距 - float Tar_Position_angle=50; //位置环模式，期望角度 - int32 Tar_Position_Velcity_Degree_S=51; //位置环模式，速度(m/min) + } diff --git a/Core/FSM/Inc/Handset_Status_Setting.h b/Core/FSM/Inc/Handset_Status_Setting.h index 8cf9c44..41dddac 100644 --- a/Core/FSM/Inc/Handset_Status_Setting.h +++ b/Core/FSM/Inc/Handset_Status_Setting.h @@ -10,7 +10,8 @@ // 按键定义 typedef enum { - INPUT_NONE = 0, // 无输入 + EMERGENCE_STOP=0, // SE和SF同时按下触发急停 + INPUT_NONE , // 无输入 // 摇杆方向 INPUT_ROCKER_STOP, // 摇杆停止 INPUT_ROCKER_FORWARD, // 摇杆前进 @@ -25,7 +26,6 @@ typedef enum { INPUT_KEY_AUTO_WORK_UP, // CH7_SD = -1000 自动作业上 INPUT_KEY_AUTO_WORK_DOWN, // CH7_SD = 1000 自动作业下 - EMERGENCE_STOP, // SE和SF同时按下触发急停 KEY_COUNT, // 输入总数 diff --git a/Core/FSM/Inc/robot_move_actions.h b/Core/FSM/Inc/robot_move_actions.h index b610897..e4be1a9 100644 --- a/Core/FSM/Inc/robot_move_actions.h +++ b/Core/FSM/Inc/robot_move_actions.h @@ -28,5 +28,6 @@ void horizontal_forward(void); /* 水平前进（PID控制） */ void vertical_forward(void); /* 垂直前进（PID控制） */ void flag_reset(void); +void Emergency_Stop_Action(void); #endif /* FSM_INC_ROBOT_MOVE_ACTIONS_H_ */ diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c index 9e7e6e9..a591c11 100644 --- a/Core/FSM/Src/Handset_Status_Setting.c +++ b/Core/FSM/Src/Handset_Status_Setting.c @@ -30,6 +30,7 @@ // ============ 类型定义 ============ typedef InputEvent (*ModeEventHandler)(void); +static inline InputEvent CheckEmergencyStop(void); static InputEvent GetHaltModeEvents(void); static InputEvent GetManualModeEvents(void); static InputEvent GetHorizontalModeEvents(void); @@ -147,13 +148,14 @@ static InputEvent CalculateRockerEvent(void) return INPUT_ROCKER_STOP; } -// 摇杆事件计算 -static InputEvent Check_Emergency_Event(void) +// 急停检测 +static inline InputEvent CheckEmergencyStop(void) { if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) { return EMERGENCE_STOP; } + return INPUT_NONE; } @@ -161,80 +163,64 @@ static InputEvent Check_Emergency_Event(void) static InputEvent GetHaltModeEvents(void) { - if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000) - { - return EMERGENCE_STOP; - } - InputEvent key = CheckAllKeys(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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(); + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; + 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; - } + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; 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; - } + InputEvent key = CheckEmergencyStop(); + if(key!=INPUT_NONE) return key; if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP; return CalculateRockerEvent(); } @@ -265,6 +251,8 @@ void IV_control(void) 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.LeftCompensation = GV.Left_Compensation; + IV.RightCompensation = GV.Right_Compensation; // IV.Left_Motor_Err = GV.LeftMotor.Motor_Fault; // IV.Right_Motor_Err = GV.RightMotor.Motor_Fault; // IV.TimeStamp = SystemTimeMiliCount; diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c index dbd8089..a265cc4 100644 --- a/Core/FSM/Src/fsm_state_control.c +++ b/Core/FSM/Src/fsm_state_control.c @@ -38,7 +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); +static void Emergency_Stop_group(void); /*=========================== 6. 函数指针表 ===========================*/ /* 动作映射表：模式 × 按键 -> 执行函数 */ @@ -55,7 +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, + [EMERGENCE_STOP] = Emergency_Stop_group, }, // Horizontal_Mode - 水平模式 @@ -69,7 +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, + [EMERGENCE_STOP] = Emergency_Stop_group, // 其他按键暂未实现，留空（NULL） }, @@ -88,7 +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, + [EMERGENCE_STOP] = Emergency_Stop_group, // 其他预留 }, @@ -102,7 +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, + [EMERGENCE_STOP] = Emergency_Stop_group, // 未实现 }, @@ -150,23 +150,24 @@ static void manual_right_group(void) static void horizontal_forward_group(void) { GV.Robot_Desired_Speed=GV.Robot_Move_Speed; - horizontal_forward(); + //horizontal_forward(); /* 若需喷枪控制，可在此添加 */ } static void horizontal_backward_group(void) { GV.Robot_Desired_Speed=-GV.Robot_Move_Speed; - horizontal_forward(); + //horizontal_forward(); /* 若需喷枪控制，可在此添加 */ } static void horizontal_auto_group(void) { - horizontal_work(); +// horizontal_work(); + Move_Horizontal_Auto_Sub_Func(); /* 若需喷枪控制，可在此添加 */ } static void change_Road_group(void) { - Change_Road_Func(); + //Change_Road_Func(); /* 若需喷枪控制，可在此添加 */ } @@ -174,23 +175,21 @@ static void change_Road_group(void) static void vertical_forward_group(void) { GV.Robot_Desired_Speed=GV.Robot_Move_Speed; - vertical_forward(); + //vertical_forward(); } static void vertical_backward_group(void) { GV.Robot_Desired_Speed=-GV.Robot_Move_Speed; - vertical_forward(); + //vertical_forward(); } -static void Emergency_Stop(void) +static void Emergency_Stop_group(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(); + Emergency_Stop_Action(); + Is_All_Button_Reset = 0; } + /*=========================== 8. 对外接口函数（需外部调用） ===========================*/ /*------------------------------------------------------------------- @@ -249,12 +248,6 @@ void GF_Dispatch(void) *-------------------------------------------------------------------*/ int AbnormalDetect(void) { - /* 急停 */ - if (P_MK32->CH8_SE == -1000 && P_MK32->CH9_SF == -1000) - { - Is_All_Button_Reset = 0; // 必须复位才能动 - return 1; - } /* 按钮未复位 */ if (Get_BIT(SystemErrorCode, ComError_MK32_InitialState) != Has_Reset) diff --git a/Core/FSM/Src/motor.c b/Core/FSM/Src/motor.c index 2a6d0f5..f30b5aa 100644 --- a/Core/FSM/Src/motor.c +++ b/Core/FSM/Src/motor.c @@ -123,8 +123,8 @@ void MotorCommandsLoop() void MotorCommandsLoop_2_Position() { - GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.SwingMotor.Tar_Position_angle*TT_One_Deg_Count_motor); - GV.SwingMotor.Tar_Position_Velcity_RPM=GV.SwingMotor.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor; + GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.Tar_Position_angle*TT_One_Deg_Count_motor); + GV.SwingMotor.Tar_Position_Velcity_RPM=GV.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor; if (TT_Motor_Need_To_Activate_1 == 1) { diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c index b984c1a..3c0d24d 100644 --- a/Core/FSM/Src/robot_move_actions.c +++ b/Core/FSM/Src/robot_move_actions.c @@ -72,14 +72,31 @@ enum swing_state { SWING_RIGHT_MOVE, /* 向右运动 */ }; +/** + * 状态机类型枚举（用于多状态机管理） + */ +typedef enum { + FSM_init_state = 0, // 默认初始化的值（占位，不使用） + FSM_HORIZONTAL_AUTO_Flag, // 水平单道自动 + FSM_HORIZONTAL_FORWARD_Flag, // 水平前进（简化版） + FSM_VERTICAL_FORWARD_Flag, // 垂直前进 + FSM_VERTICAL_CHANGE_ROAD_Flag, // 垂直左换道 + FSM_SWING, //摆臂 + REGION_STATE_AUTO_CONTINUOUS_Flag, //区域自动作业 + FSM_COUNT // 状态机总数 +} FsmType; + /*=========================== 4.全局变量 ===========================*/ ////局部静态变量 /* 状态机当前状态（多个状态机共用，可能存在潜在冲突，但是现在还不知道咋搞呢） */ +static FsmType s_activeFsm; +static FsmType s_lastActiveFsm=FSM_init_state; +static int s_fsmState[FSM_COUNT] = {0}; int s_currentState = STATE_ATTITUDE_JUDGE; int horizontal_s_currentState = STATE_ATTITUDE_JUDGE; int vertical_s_currentState = STATE_ATTITUDE_JUDGE; -int ChangeRoad_currentState = STATE_ATTITUDE_JUDGE; +int ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_JUDGE; int swing_currentState = SWING_START_STATE; /* 换道计时变量 */ @@ -130,7 +147,7 @@ static void swing_left_move(void); static void swing_right_move(void); static void get_swing_mode(void); - +void Emergency_Stop_Action(void); /* PID系数 */ int factor_1= 1; int factor_2= -1; @@ -196,6 +213,24 @@ static const void (*const swing_move_control[])(void) = { /*=========================== 7.静态函数实现 ===========================*/ +/*----------------------------------------------------------------- + * 状态机切换管理 + *-----------------------------------------------------------------*/ +static void check_and_reset_fsm(FsmType current) { + if (s_lastActiveFsm != current) { + // 发生了状态机切换，重置其他所有状态机 + for (int i = 0; i < FSM_COUNT; i++) { + if (i != current) { + s_fsmState[i] = 0; // 重置为初始状态 + } + } + s_lastActiveFsm = current; // 更新上次类型 + } + // 如果相同，不做任何操作 +} + + + /*----------------------------------------------------------------- * 基础运动控制（手动模式） @@ -226,6 +261,9 @@ void Turn_Right(void) void Robot_Stop(void) { + check_and_reset_fsm(FSM_init_state); + swing_currentState = 0; + Compensation_Update(); get_swing_mode(); Robot_Swing_Operation_Function(); @@ -269,7 +307,7 @@ static void handleAttitudeAdjustHorizontal(void) float errorAbs; Robot_Posture_Adjus_Gravity(&errorAbs); if (errorAbs < ANGLE_ERROR_THRESHOLD) { - horizontal_s_currentState++; + s_fsmState[s_activeFsm]++; } } @@ -278,7 +316,7 @@ static void handleAttitudeAdjustVertical(void) float errorAbs; Robot_Posture_Adjus_Gravity(&errorAbs); if (errorAbs < ANGLE_ERROR_THRESHOLD) { - vertical_s_currentState++; + s_fsmState[s_activeFsm]++; } } @@ -288,7 +326,7 @@ static void handleAttitudeAdjustChangeRoad(void) float errorAbs; Robot_Posture_Adjus_Gravity(&errorAbs); if (errorAbs < ANGLE_ERROR_THRESHOLD) { - ChangeRoad_currentState++; + s_fsmState[s_activeFsm]++; } } /*----------------------------------------------------------------- @@ -303,7 +341,7 @@ static void handleAttitudeJudge_horizontal(void) } else { GV.Robot_Angle_Desire = 90.0f + (float)GV.Left_Compensation / 100.0f; } - horizontal_s_currentState = STATE_ATTITUDE_ADJUST; + s_fsmState[s_activeFsm]=STATE_ATTITUDE_ADJUST; } static void handleAttitudeJudge_vertical(void) @@ -318,7 +356,7 @@ static void handleAttitudeJudge_vertical(void) GV.Robot_Angle_Desire -= 360.0f; } } - vertical_s_currentState = STATE_ATTITUDE_ADJUST; + s_fsmState[s_activeFsm] = STATE_ATTITUDE_ADJUST; } @@ -327,9 +365,9 @@ static void handleWorkWay(void) { int mode = GV.PV.Robot_backMode; if (mode == 1) { - horizontal_s_currentState = STATE_FIGHT_ALTERNATELY; + s_fsmState[s_activeFsm]= STATE_FIGHT_ALTERNATELY; } else if (mode == 2) { - horizontal_s_currentState = STATE_FIGHT_RETREATING; + s_fsmState[s_activeFsm]= STATE_FIGHT_RETREATING; } /* 其他模式保持当前状态？目前没有 else，可能默认留在该状态 */ } @@ -344,7 +382,7 @@ static void handleLaneChangeAttitudeJudgeLeft(void) Change_stop_flag=0; get_ChangeAngle(); GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[0]; - ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_ADJUST; + s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_ATTITUDE_ADJUST; } } @@ -400,11 +438,11 @@ static void handleLaneChangeCalcRetreatTime(void) uint32_t tickNow = HAL_GetTick(); s_retreatStartTick = tickNow; - ChangeRoad_currentState = LANE_CHANGE_STATE_CONTINUOUS_RETREAT; + s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_CONTINUOUS_RETREAT; if(P_MK32->CH4_SA == -1000&&Change_stop_flag==1) { - ChangeRoad_currentState = LANE_CHANGE_STATE_ATTITUDE_JUDGE; + s_fsmState[s_activeFsm] = LANE_CHANGE_STATE_ATTITUDE_JUDGE; } } @@ -415,7 +453,7 @@ static void handleLaneChangeContinuousRetreat(void) auto_drive_pid_vertical(); if ((tickNow - s_retreatStartTick) >= s_retreatDurationTicks) { - ChangeRoad_currentState++; + s_fsmState[s_activeFsm]++; GV.Robot_Angle_Desire = LEFT_TARGET_ANGLES[1]; } } @@ -440,7 +478,9 @@ void Move_Horizontal_Vertical_Task_Backwards_Do_Backward(void) - +/*----------------------------------------------------------------- + * 摆臂运动函数 + *-----------------------------------------------------------------*/ int present_angle; static void swing_start_move(void) { @@ -520,7 +560,7 @@ static void auto_drive_pid_horizontal(void) float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f; float speeds[2]; - TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, GV.Robot_Desired_Speed,2, speeds); + TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, GV.Robot_Desired_Speed,1, speeds); GV.Left_Speed_M_min = factor_1*speeds[0]; GV.Right_Speed_M_min = factor_2*speeds[1]; /* 根据电机方向可能需要调整符号 */ @@ -551,16 +591,15 @@ static void handleHalt(void) } /*=========================== 8.对外接口函数（需外部调用） ===========================*/ -void Move_Horizontal_Auto_Init(void) -{ - horizontal_s_currentState = STATE_ATTITUDE_JUDGE; -} - void Move_Horizontal_Auto_Sub_Func(void) { - if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) { - if (horizontal_single_lane_auto_operation[horizontal_s_currentState] != NULL) { - horizontal_single_lane_auto_operation[horizontal_s_currentState](); + s_activeFsm = FSM_HORIZONTAL_AUTO_Flag; + check_and_reset_fsm(FSM_HORIZONTAL_AUTO_Flag); + int state = s_fsmState[FSM_HORIZONTAL_AUTO_Flag]; + + if (state >= 0 && state < STATE_COUNT) { + if (horizontal_single_lane_auto_operation[state] != NULL) { + horizontal_single_lane_auto_operation[state](); } else { /* 遇到未实现的状态，停机处理 */ handleHalt(); @@ -572,31 +611,60 @@ void Move_Horizontal_Auto_Sub_Func(void) void Change_Road_Func(void) { - /* 注意：此函数实际使用换道状态表，但命名可能引起混淆，建议后续改为垂直换道相关名称 */ - if (ChangeRoad_currentState >= 0 && ChangeRoad_currentState < LANE_CHANGE_STATE_COUNT) { - vertical_lane_change_left[ChangeRoad_currentState](); + s_activeFsm = FSM_VERTICAL_CHANGE_ROAD_Flag; + check_and_reset_fsm(FSM_VERTICAL_CHANGE_ROAD_Flag); + int state = s_fsmState[FSM_VERTICAL_CHANGE_ROAD_Flag]; + + if (state >= 0 && state < STATE_COUNT) { + if (vertical_lane_change_left[state] != NULL) { + vertical_lane_change_left[state](); + } else { + /* 遇到未实现的状态，停机处理 */ + handleHalt(); + } } else { handleHalt(); } + } void vertical_forward(void) { - if (vertical_s_currentState >= 0 && vertical_s_currentState < STATE_COUNT) { - robot_vertical_drive[vertical_s_currentState](); + s_activeFsm = FSM_VERTICAL_FORWARD_Flag; + check_and_reset_fsm(FSM_VERTICAL_FORWARD_Flag); + int state = s_fsmState[FSM_VERTICAL_FORWARD_Flag]; + + if (state >= 0 && state < STATE_COUNT) { + if (robot_vertical_drive[state] != NULL) { + robot_vertical_drive[state](); + } else { + /* 遇到未实现的状态，停机处理 */ + handleHalt(); + } } else { handleHalt(); } + } + void horizontal_forward(void) { - if (horizontal_s_currentState >= 0 && horizontal_s_currentState < STATE_COUNT) { - robot_horizontal_drive[horizontal_s_currentState](); - } else { - handleHalt(); - } + s_activeFsm = FSM_HORIZONTAL_AUTO_Flag; + check_and_reset_fsm(FSM_HORIZONTAL_AUTO_Flag); + int state = s_fsmState[FSM_HORIZONTAL_AUTO_Flag]; + + if (state >= 0 && state < STATE_COUNT) { + if (robot_horizontal_drive[state] != NULL) { + robot_horizontal_drive[state](); + } else { + /* 遇到未实现的状态，停机处理 */ + handleHalt(); + } + } else { + handleHalt(); + } } void horizontal_work(void) @@ -610,8 +678,14 @@ void horizontal_work(void) void swing_work(void) { + if (swing_currentState >= 0 && swing_currentState < STATE_COUNT) { - swing_move_control[swing_currentState](); + if (swing_move_control[swing_currentState] != NULL) { + swing_move_control[swing_currentState](); + } else { + /* 遇到未实现的状态，停机处理 */ + handleHalt(); + } } else { handleHalt(); } @@ -634,3 +708,12 @@ void flag_reset(void) } +void Emergency_Stop_Action(void) +{ + GV.SwingMotor.Position_immediately1_Lag2=1; + GV.Tar_Position_Velcity_Degree_S=0; + GV.Left_Speed_M_min=0; + GV.Right_Speed_M_min=0; + flag_reset(); +} + diff --git a/Core/FSM/Src/swing_action.c b/Core/FSM/Src/swing_action.c index 7130187..7f16fad 100644 --- a/Core/FSM/Src/swing_action.c +++ b/Core/FSM/Src/swing_action.c @@ -31,15 +31,15 @@ void Robot_Swing_Operation_Function() if(P_MK32->CH0_RY_H>300) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_angle=90; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + GV.Tar_Position_angle=90; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } else if(P_MK32->CH0_RY_H<-300) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_angle=-90; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + GV.Tar_Position_angle=-90; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } else { @@ -53,8 +53,8 @@ void Robot_Swing_Operation_Function() void Move_Swing_Left_Func_Do_lay(void) { GV.SwingMotor.Position_immediately1_Lag2=2; - GV.SwingMotor.Tar_Position_angle=left_compare_value; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + GV.Tar_Position_angle=left_compare_value; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } //摆臂电机右转延时生效 @@ -62,8 +62,8 @@ void Move_Swing_Right_Func_Do_lay(void) { GV.SwingMotor.Position_immediately1_Lag2=2; - GV.SwingMotor.Tar_Position_angle=right_compare_value; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + GV.Tar_Position_angle=right_compare_value; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } //摆臂电机左转立即执行 @@ -71,8 +71,9 @@ void Move_Swing_Left_Func_Do_imm(void) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_angle=left_compare_value; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + + GV.Tar_Position_angle=left_compare_value; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } @@ -82,8 +83,8 @@ void Move_Swing_Right_Func_Do_imm(void) { GV.SwingMotor.Position_immediately1_Lag2=1; - GV.SwingMotor.Tar_Position_angle=right_compare_value; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; + GV.Tar_Position_angle=right_compare_value; + GV.Tar_Position_Velcity_Degree_S=GV.PV.Robot_Swing_Speed; } @@ -91,6 +92,6 @@ void Move_Swing_Halt_Func_Do(void) { GV.SwingMotor.Position_immediately1_Lag2=1; GV.SwingMotor.Tar_Position_count=0; - GV.SwingMotor.Tar_Position_Velcity_Degree_S=0; + GV.Tar_Position_Velcity_Degree_S=0; } diff --git a/疑问记录.txt b/疑问记录.txt index 8493e46..f51ffb7 100644 --- a/疑问记录.txt +++ b/疑问记录.txt @@ -44,9 +44,9 @@ GV_Robot_backMode=(int)CV.PV.Robot_backMode; 绑定电机 测距 调试摆臂 调内部PID - 问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合） EPRROM的CV的配置 自动巡航√ 非对称摆臂√ 实现急停两个按键 + 问题：摆臂CAN解析√ 变量的传导√ 报错信息√ 焊缝跟踪的验证√ 换道精度（最小二乘拟合） EPRROM的CV的配置 自动巡航√ 非对称摆臂√ 实现急停两个按键√ - 变量的传导√ 用不同的标志位√ 调整左补偿与右补偿 + 变量的传导√ 用不同的标志位√ 调整左补偿与右补偿√ B03-14摆臂机器人3月13日计划完成： @@ -60,3 +60,17 @@ B03-14摆臂机器人3月13日计划完成： 4.最小二乘法拟合换道精度，之前的参数测试过之后发现不太准，今天需要再测一遍 + +图片里尚未完成的： + +1.区域自动作业的相关功能 + +2.推杆 + +3.按键SC相关功能 + +4.平面模式 + +5.参数预设 + +6.竖直微调 \ No newline at end of file