|
|
|
@ -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,28 +611,57 @@ 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](); |
|
|
|
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(); |
|
|
|
} |
|
|
|
@ -610,8 +678,14 @@ void horizontal_work(void) |
|
|
|
|
|
|
|
void swing_work(void) |
|
|
|
{ |
|
|
|
|
|
|
|
if (swing_currentState >= 0 && swing_currentState < STATE_COUNT) { |
|
|
|
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(); |
|
|
|
} |
|
|
|
|
|
|
|
|
