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.
172 lines
4.1 KiB
172 lines
4.1 KiB
/*
|
|
* bsp_FSM.c
|
|
*
|
|
* Created on: Aug 2, 2024
|
|
* Author: akeguo
|
|
*/
|
|
|
|
#include <FSM.h>
|
|
#include "bsp_hardware_hash_table.h"
|
|
#include "MSP/msp_MK32.h"
|
|
void GF_Dispatch();
|
|
|
|
//Manual=0,
|
|
// Auto,
|
|
// Abnormal,
|
|
// SetLaneChangeDistance,
|
|
// SetBackwardsDistance,
|
|
// LaneChangeMode,
|
|
// Forwards,
|
|
// Backwards,
|
|
// TurnLeft,
|
|
// TurnRight
|
|
|
|
|
|
ACTION_MAP_t actionMap[] =
|
|
{
|
|
{ Manual, Manual_State_Entry, Manual_State_Do, Manual_State_Exit },
|
|
{ Auto, Auto_State_Entry, Auto_State_Do, Auto_State_Exit },
|
|
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Entry, LaneChangeDistance_Setting_State_Do, LaneChangeDistance_Setting_State_Exit },
|
|
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Entry, BackWardsDistance_Setting_State_Do, BackWardsDistance_Setting_State_Exit },
|
|
{ Forwards, Forwards_State_Entry, Forwards_State_Do, Forwards_State_Exit },
|
|
{ Backwards, Backwards_State_Entry, Backwards_State_Do, Backwards_State_Exit },
|
|
{ TurnLeft, TurnLeft_State_Entry, TurnLeft_State_Do, TurnLeft_State_Exit },
|
|
{ TurnRight, TurnRight_State_Entry, TurnRight_State_Do, TurnRight_State_Exit },
|
|
|
|
};
|
|
|
|
|
|
|
|
EVENT_MAP_t eventMap[] =
|
|
{
|
|
{ EVENT_Manual, Manual, Manual },
|
|
{ EVENT_Auto, Auto, Auto },
|
|
{ EVENT_Abnormal, Abnormal, Abnormal },
|
|
{ EVENT_SetLaneChangeDistance, SetLaneChangeDistance, SetLaneChangeDistance },
|
|
{ EVENT_SetBackwardsDistance, SetBackwardsDistance, SetBackwardsDistance },
|
|
{ EVENT_LaneChangeMode, LaneChangeMode, LaneChangeMode },
|
|
{ EVENT_Forwards, Forwards, Forwards },
|
|
{ EVENT_Backwards, Backwards, Backwards },
|
|
{ EVENT_TurnLeft, TurnLeft, TurnLeft },
|
|
{ EVENT_TurnRight, TurnRight, TurnRight },
|
|
{ EVENT_MAP_END, 0, 0 }, };
|
|
|
|
FSM_t RobotFSM; //定义状态机
|
|
|
|
void fsm_init(FSM_t *pFsm, EVENT_MAP_t *pEventMap, ACTION_MAP_t *pActionMap)
|
|
{
|
|
//pFsm->stCurState = 0;
|
|
pFsm->stCurState = EVENT_Abnormal;
|
|
pFsm->stNextState = EVENT_MAP_END;
|
|
pFsm->pEventMap = pEventMap;
|
|
pFsm->pActionMap = pActionMap;
|
|
}
|
|
void fsm_state_transfer(FSM_t *pFsm, EVENT_t stEventID)
|
|
{
|
|
char i = 0;
|
|
|
|
for (i = 0; pFsm->pEventMap[i].stEventID < EVENT_MAP_END; i++)
|
|
{
|
|
if (stEventID == pFsm->pEventMap[i].stEventID) //MAKE SURE THIS STATE EXISTS
|
|
{
|
|
if (pFsm->stCurState != stEventID)
|
|
{
|
|
pFsm->stNextState = stEventID; //pFsm->pEventMap[i].stNextState;
|
|
} else
|
|
{
|
|
pFsm->stNextState = EVENT_MAP_END; //pFsm->pEventMap[i].stNextState;
|
|
}
|
|
return;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
void action_perfrom(FSM_t *pFsm)
|
|
{
|
|
if (EVENT_MAP_END != pFsm->stNextState)
|
|
{
|
|
pFsm->pActionMap[pFsm->stCurState].ExitAct();
|
|
pFsm->pActionMap[pFsm->stNextState].EnterAct();
|
|
|
|
pFsm->stCurState = pFsm->stNextState;
|
|
pFsm->stNextState = EVENT_MAP_END;
|
|
} else
|
|
{
|
|
pFsm->pActionMap[pFsm->stCurState].RunningAct();
|
|
}
|
|
}
|
|
|
|
void Fsm_Init()
|
|
{
|
|
fsm_init(&RobotFSM, eventMap, actionMap); //注册状态机
|
|
GF_BSP_Interrupt_Add_CallBack(
|
|
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
|
}
|
|
|
|
bool IsAbnormal = false;
|
|
|
|
int GF_Dispacher_Counter = 0;
|
|
void GF_Dispatch()
|
|
{
|
|
if(GV.DH_CAN.Work_Start==1)
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_Auto);
|
|
}else
|
|
{
|
|
// fsm_state_transfer(&RobotFSM, EVENT_Manual);
|
|
|
|
if (GV.DH_CAN.Swing_Right == 1)
|
|
{
|
|
GV.SwingMotor.Target_Position -= 100;
|
|
|
|
} else if (GV.DH_CAN.Swing_Left == 1)
|
|
{
|
|
GV.SwingMotor.Target_Position += 100;
|
|
}
|
|
|
|
if (GV.DH_CAN.Mode_Selection == 9) //换道距离设定
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_SetLaneChangeDistance);
|
|
} else if (GV.DH_CAN.Mode_Selection == 10) //后退距离设定
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_SetBackwardsDistance);
|
|
}
|
|
//前进
|
|
if (GV.DH_CAN.Move_Forward == 1)
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_Forwards);
|
|
|
|
} else if (GV.DH_CAN.Move_Backward == 1) //后退左转右转
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_Backwards);
|
|
|
|
} else if (GV.DH_CAN.Move_Turn_Left == 1)
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_TurnLeft); //左转
|
|
|
|
|
|
} else if (GV.DH_CAN.Move_Turn_Right == 1)//右转
|
|
{
|
|
fsm_state_transfer(&RobotFSM, EVENT_TurnRight);
|
|
|
|
}
|
|
|
|
}
|
|
#if 0
|
|
if (IsAbnornalStatus == 1)
|
|
{
|
|
IsAbnormal = true;
|
|
//进入异常状态；
|
|
fsm_state_transfer(&RobotFSM, EVENT_Abnormal);
|
|
} else
|
|
{
|
|
//退出异常状态
|
|
IsAbnormal = false;
|
|
}
|
|
#endif 0
|
|
//}
|
|
|
|
|
|
action_perfrom(&RobotFSM);
|
|
}
|
|
|