You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
414 lines
12 KiB
414 lines
12 KiB
/*
|
|
* fsm_state_control.c
|
|
*
|
|
* Created on: Jan 13, 2026
|
|
* Author: bm673
|
|
*/
|
|
|
|
/*=========================== 1. 头文件包含 ===========================*/
|
|
#include "fsm_state_control.h"
|
|
#include "Handset_Status_Setting.h"
|
|
#include "robot_move_actions.h"
|
|
#include "paint_gun_action.h"
|
|
#include "swing_action.h"
|
|
#include "motors_power_action.h"
|
|
#include "BHBF_ROBOT.h"
|
|
|
|
/*=========================== 2. 宏定义 ===========================*/
|
|
/* 本文件无宏定义，预留 */
|
|
|
|
/*=========================== 3. 类型定义 ===========================*/
|
|
/* 本文件无自定义类型，预留 */
|
|
|
|
/*=========================== 4. 全局变量 ===========================*/
|
|
/* 调试变量（普通全局变量） */
|
|
Robot_Mode g_debug_prev_mode = Halt_Mode;
|
|
InputEvent g_debug_curr_key = INPUT_NONE;
|
|
ActionFunc g_debug_current_action_func = NULL;
|
|
int error_detcet=0; //获取机器人错误
|
|
uint32_t NVIC_Priority_Group; // 中断优先级分组
|
|
uint32_t TIM8_PreemptPrio; // 定时器8 抢占优先级
|
|
uint32_t TIM8_SubPrio; // 定时器8 子优先级
|
|
|
|
/*=========================== 5. 静态函数声明 ===========================*/
|
|
/* 组函数（仅本文件使用） */
|
|
static void manual_forward_group(void);
|
|
static void manual_backward_group(void);
|
|
static void manual_left_group(void);
|
|
static void manual_right_group(void);
|
|
static void manual_auto_group(void);
|
|
static void horizontal_forward_group(void);
|
|
static void horizontal_backward_group(void);
|
|
static void weld_auto_group(void);
|
|
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 vertical_auto_group(void);
|
|
static void Emergency_Stop_group(void);
|
|
// 读取指定中断的抢占/子优先级，存入全局变量
|
|
void Read_IRQ_Priority(IRQn_Type IRQn, uint32_t *preempt, uint32_t *sub);
|
|
|
|
/*=========================== 6. 函数指针表 ===========================*/
|
|
/* 动作映射表：模式 × 按键 -> 执行函数 */
|
|
ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
|
|
// Halt_Mode - 所有输入都无操作
|
|
[Halt_Mode] = { [0 ... KEY_COUNT-1] = Robot_Stop },
|
|
|
|
// Manual_Mode - 手动模式
|
|
[Manual_Mode] = {
|
|
[INPUT_ROCKER_STOP] = Robot_Stop,
|
|
[INPUT_ROCKER_FORWARD] = manual_forward_group,
|
|
[INPUT_ROCKER_BACKWARD] = manual_backward_group,
|
|
[INPUT_ROCKER_TURN_LEFT] = manual_left_group,
|
|
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group,
|
|
[INPUT_KEY_FORWARD_CRUISE] = manual_forward_group,
|
|
[INPUT_KEY_BACKWARD_CRUISE] = manual_backward_group,
|
|
[INPUT_KEY_AUTO_WORK_UP] = manual_auto_group,
|
|
[INPUT_KEY_LANE_CHANGE_UP] = Robot_Stop,
|
|
[EMERGENCE_STOP] = Emergency_Stop_group,
|
|
|
|
},
|
|
|
|
// Horizontal_Mode - 水平模式
|
|
[Horizontal_Mode] = {
|
|
[INPUT_ROCKER_STOP] = Robot_Stop,
|
|
[INPUT_ROCKER_FORWARD] = horizontal_forward_group,
|
|
[INPUT_ROCKER_BACKWARD] = horizontal_backward_group,
|
|
[INPUT_ROCKER_TURN_LEFT] = manual_left_group,
|
|
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group, //等待调整 INPUT_KEY_FORWARD_CRUISE
|
|
[INPUT_KEY_FORWARD_CRUISE] = horizontal_forward_group,
|
|
[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_group,
|
|
// 其他按键暂未实现，留空（NULL）
|
|
},
|
|
|
|
// Flat_Mode - 暂时用于焊缝模式（预留）
|
|
[Flat_Mode] = {
|
|
[INPUT_ROCKER_STOP] = Robot_Stop,
|
|
[INPUT_ROCKER_FORWARD] = manual_forward_group,
|
|
[INPUT_ROCKER_BACKWARD] = manual_backward_group,
|
|
[INPUT_ROCKER_TURN_LEFT] = manual_left_group,
|
|
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group,
|
|
[INPUT_KEY_AUTO_WORK_UP] = weld_auto_group,
|
|
[EMERGENCE_STOP] = Emergency_Stop_group,
|
|
// 暂时用于焊缝跟踪
|
|
},
|
|
|
|
// Vertical_Mode_Left - 左侧垂直模式
|
|
[Vertical_Mode_Left] = {
|
|
[INPUT_ROCKER_STOP] = Robot_Stop,
|
|
[INPUT_ROCKER_FORWARD] = vertical_forward_group, // 注意：水平前进，可能是复用
|
|
[INPUT_ROCKER_BACKWARD] = vertical_backward_group,
|
|
[INPUT_ROCKER_TURN_LEFT] = manual_left_group,
|
|
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group, //等待调整
|
|
[INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group,
|
|
[INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group,
|
|
[INPUT_KEY_LANE_CHANGE_UP] = change_Road_group,
|
|
[INPUT_KEY_AUTO_WORK_UP] = vertical_auto_group,
|
|
[EMERGENCE_STOP] = Emergency_Stop_group,
|
|
// 其他预留
|
|
},
|
|
|
|
// Vertical_Mode_Right - 右侧垂直模式（预留）
|
|
[Vertical_Mode_Right] = {
|
|
[INPUT_ROCKER_STOP] = Robot_Stop,
|
|
[INPUT_ROCKER_FORWARD] = vertical_forward_group, // 注意：水平前进，可能是复用
|
|
[INPUT_ROCKER_BACKWARD] = vertical_backward_group,
|
|
[INPUT_ROCKER_TURN_LEFT] = manual_left_group,
|
|
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group, //等待调整
|
|
[INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group,
|
|
[INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group,
|
|
[INPUT_KEY_LANE_CHANGE_UP] = change_Road_group,
|
|
[INPUT_KEY_AUTO_WORK_UP] = vertical_auto_group,
|
|
[EMERGENCE_STOP] = Emergency_Stop_group,
|
|
// 未实现
|
|
},
|
|
|
|
// Regional_Horizontal_Automatic_Task - 区域水平自动任务
|
|
[Regional_Horizontal_Automatic_Task] = {
|
|
// 未实现
|
|
},
|
|
|
|
// Regional_Flat_Automatic_Task - 区域平面自动任务
|
|
[Regional_Flat_Automatic_Task] = {
|
|
// 未实现
|
|
}
|
|
|
|
};
|
|
|
|
/*=========================== 7. 静态函数实现 ===========================*/
|
|
/*-------------------------------------------------------------------
|
|
* 组函数实现（静态）
|
|
*-------------------------------------------------------------------*/
|
|
static void manual_forward_group(void)
|
|
{
|
|
Manually_Forward();
|
|
PaintGun_Contronl();
|
|
Robot_Swing_Operation_Function();
|
|
|
|
}
|
|
|
|
static void manual_backward_group(void)
|
|
{
|
|
Manually_Backward();
|
|
PaintGun_Contronl();
|
|
Robot_Swing_Operation_Function();
|
|
|
|
}
|
|
|
|
static void manual_left_group(void)
|
|
{
|
|
Turn_Left();
|
|
PaintGun_Contronl();
|
|
Robot_Swing_Operation_Function();
|
|
|
|
}
|
|
|
|
static void manual_right_group(void)
|
|
{
|
|
Turn_Right();
|
|
PaintGun_Contronl();
|
|
Robot_Swing_Operation_Function();
|
|
|
|
}
|
|
|
|
static void horizontal_forward_group(void)
|
|
{
|
|
GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
|
|
horizontal_forward();
|
|
PaintGun_Contronl_Press();
|
|
Robot_Swing_Operation_Function();
|
|
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
static void horizontal_backward_group(void)
|
|
{
|
|
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
|
|
horizontal_forward();
|
|
PaintGun_Contronl_Press();
|
|
Robot_Swing_Operation_Function();
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
|
|
static void manual_auto_group(void)
|
|
{
|
|
Move_Manual_Auto_Sub_Func();
|
|
PaintGun_Contronl_Press();
|
|
}
|
|
|
|
static void horizontal_auto_group(void)
|
|
{
|
|
// horizontal_work();
|
|
Move_Horizontal_Auto_Sub_Func();
|
|
PaintGun_Contronl_Press();
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
|
|
static void weld_auto_group(void)
|
|
{
|
|
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
|
|
Weld_Auto_Sub_Func();
|
|
PaintGun_Contronl_Press();
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
|
|
static void change_Road_group(void)
|
|
{
|
|
Change_Road_Func();
|
|
PaintGun_Contronl_Press();
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
|
|
|
|
static void vertical_forward_group(void)
|
|
{
|
|
GV.Robot_Desired_Speed=GV.Robot_Move_Speed;
|
|
vertical_forward();
|
|
PaintGun_Contronl_Press();
|
|
Robot_Swing_Operation_Function();
|
|
}
|
|
|
|
static void vertical_backward_group(void)
|
|
{
|
|
GV.Robot_Desired_Speed=-GV.Robot_Move_Speed;
|
|
vertical_forward();
|
|
PaintGun_Contronl_Press();
|
|
Robot_Swing_Operation_Function();
|
|
}
|
|
|
|
static void vertical_auto_group(void)
|
|
{
|
|
// horizontal_work();
|
|
Move_Vertical_Auto_Sub_Func();
|
|
PaintGun_Contronl_Press();
|
|
/* 若需喷枪控制，可在此添加 */
|
|
}
|
|
|
|
static void Emergency_Stop_group(void)
|
|
{
|
|
Emergency_Stop_Action();
|
|
Is_All_Button_Reset = 0;
|
|
}
|
|
|
|
/*=========================== 8. 对外接口函数（需外部调用） ===========================*/
|
|
int AbnormalDetect(void);
|
|
/*-------------------------------------------------------------------
|
|
* 初始化函数
|
|
*-------------------------------------------------------------------*/
|
|
void Fsm_Init(void)
|
|
{
|
|
// 原注释掉的初始化代码
|
|
// current_robot_move_state.p_state = &robot_halt_state;
|
|
// current_paintgun_state.p_state = &paintgun_off_state;
|
|
// current_motor_power_state.p_state = &motor_power_off_state;
|
|
|
|
|
|
GV.GroundManagementValue.MaualControlPower=0;
|
|
|
|
GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
|
}
|
|
|
|
/*-------------------------------------------------------------------
|
|
* 主调度函数（2ms 周期调用）
|
|
*-------------------------------------------------------------------*/
|
|
Robot_Mode g_debug_pre_mode = Halt_Mode;
|
|
int robot_start_flag=0;
|
|
int watch_dog=0;
|
|
void GF_Dispatch(void)
|
|
{
|
|
Read_IRQ_Priority(TIM8_UP_TIM13_IRQn, &TIM8_PreemptPrio, &TIM8_SubPrio);
|
|
static int32_t start_time=0; //机器人上电启动时间约30s
|
|
if(start_time<20000)
|
|
{
|
|
start_time++;
|
|
//SystemErrorCode = 0;
|
|
}
|
|
else
|
|
{
|
|
error_detcet=AbnormalDetect();
|
|
IV.robot_start=2; //表示机器人成功启动
|
|
}
|
|
|
|
|
|
IV_control();
|
|
|
|
static Robot_Mode prev_mode = Halt_Mode;
|
|
static InputEvent prev_key = INPUT_ROCKER_STOP;
|
|
static ActionFunc prev_action_func = NULL;
|
|
g_debug_pre_mode=prev_mode;
|
|
|
|
// 获取当前模式和按键
|
|
InputEvent curr_key = RemoteControl_GetKeyIndex(prev_mode);
|
|
|
|
// 仅按键变化时更新动作
|
|
if (curr_key != prev_key)
|
|
{
|
|
robot_start_flag=1;
|
|
PV_control();
|
|
prev_mode = RobotRockerState();
|
|
prev_key = curr_key;
|
|
|
|
//若有错误只保留手动模式
|
|
if(error_detcet)
|
|
{
|
|
prev_mode=Manual_Mode;
|
|
}
|
|
|
|
prev_action_func = actionTable[prev_mode][prev_key];
|
|
|
|
}
|
|
|
|
// 执行动作
|
|
if (prev_action_func != NULL)
|
|
{
|
|
prev_action_func();
|
|
g_debug_current_action_func = prev_action_func;
|
|
}
|
|
|
|
if(robot_start_flag==0) //保证上电之后摆臂就能动,不加这句的话，就要先动其它摇杆，摆臂才能动
|
|
{
|
|
Robot_Swing_Operation_Function();
|
|
}
|
|
|
|
flag_reset();
|
|
|
|
// 更新调试变量
|
|
g_debug_prev_mode = prev_mode;
|
|
g_debug_curr_key = curr_key;
|
|
Compensation_Update();
|
|
get_weld_data();
|
|
}
|
|
|
|
/*-------------------------------------------------------------------
|
|
* 异常检测
|
|
*-------------------------------------------------------------------*/
|
|
int cnt=0;
|
|
int AbnormalDetect(void)
|
|
{
|
|
|
|
/* SBUS 出错 */
|
|
if (Get_BIT(SystemErrorCode, ComError_Mk32_SBus) == DISCONNECTED
|
|
||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID1_LeftMotor) == DISCONNECTED)
|
|
||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID2_RightMotor) == DISCONNECTED)
|
|
||(Get_BIT(SystemErrorCode, ComError_ZQ_CAN_ID3_SwingMotor) == DISCONNECTED)
|
|
||(Get_BIT(SystemErrorCode, ComError_Ground_Management) == DISCONNECTED))
|
|
{
|
|
cnt++;
|
|
if(cnt>250)
|
|
{
|
|
//触发软急停
|
|
GV.GroundManagementValue.MaualControlPower=1;
|
|
GV.GroundManagementValue.MaualPowerState=0;
|
|
cnt=0;
|
|
return 1;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/* 串口出错 */
|
|
if (Get_BIT(SystemErrorCode, ComError_MK32_Serial) == DISCONNECTED)
|
|
{
|
|
return 2;
|
|
}
|
|
|
|
|
|
|
|
/* 陀螺仪无信号 */
|
|
if (Get_BIT(SystemErrorCode, ComError_TL720D) == DISCONNECTED)
|
|
{
|
|
return 4;
|
|
}
|
|
|
|
|
|
/* 遥控器关机或失联 */
|
|
if (P_MK32->IsOnline == 0)
|
|
{
|
|
GV.GroundManagementValue.MaualControlPower=1;
|
|
GV.GroundManagementValue.MaualPowerState=0;
|
|
Is_All_Button_Reset = 0;
|
|
return 6;
|
|
}
|
|
|
|
|
|
/* 按钮未复位 */
|
|
if (Get_BIT(SystemErrorCode, ComError_MK32_InitialState) != Has_Reset)
|
|
{
|
|
return 3;
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
// 读取指定中断的抢占/子优先级，存入全局变量
|
|
void Read_IRQ_Priority(IRQn_Type IRQn, uint32_t *preempt, uint32_t *sub)
|
|
{
|
|
// 调用你官方的4参数HAL函数
|
|
HAL_NVIC_GetPriority(IRQn, NVIC_Priority_Group, preempt, sub);
|
|
}
|
|
|