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.
147 lines
3.2 KiB
147 lines
3.2 KiB
1 week ago
|
/*
|
||
|
|
* fsm.c
|
||
|
|
*
|
||
|
|
* Created on: Oct 18, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "fsm.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
#include "BSP/bsp_include.h"
|
||
|
|
#include "robot_state.h"
|
||
|
|
void action_perfrom(transition_t transitions[], int length ,int state);
|
||
|
|
void GF_Dispatch();
|
||
|
|
void action_perfrom();
|
||
|
|
MoveSTATE_t CurrentMoveState;
|
||
|
|
SwingSTATE_t CurrentSwingState;
|
||
|
|
SetSTATE_t CurrentSetState;
|
||
|
|
int index_counter = 0;
|
||
|
|
|
||
|
|
//手动下 机器人运动
|
||
|
|
transition_t MoveTransitions[] =
|
||
|
|
{
|
||
|
|
|
||
|
|
{ Move_Forwards, Forwards_State_Do },
|
||
|
|
{ Move_Backwards, Backwards_State_Do },
|
||
|
|
{ Move_TurnLeft, TurnLeft_State_Do },
|
||
|
|
{ Move_TurnRight, TurnRight_State_Do },
|
||
|
|
{ Move_HALT, HALT_State_Do }, };
|
||
|
|
|
||
|
|
transition_t SwingTransitions[] =
|
||
|
|
{
|
||
|
|
|
||
|
|
{ SwingHALT, SwingHALT_State_Do },
|
||
|
|
{ SwingLeft, SwingLeft_State_Do },
|
||
|
|
{ SwingRight, SwingRight_State_Do },
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
transition_t SetTransitions[] =
|
||
|
|
{
|
||
|
|
|
||
|
|
{ OtherMode, OtherMode_State_Do },
|
||
|
|
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
|
||
|
|
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
void Fsm_Init()
|
||
|
|
{
|
||
|
|
GF_BSP_Interrupt_Add_CallBack(
|
||
|
|
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
||
|
|
}
|
||
|
|
//状态迁移
|
||
|
|
|
||
|
|
void GF_Dispatch()
|
||
|
|
{
|
||
|
|
if (GV.DH_CAN.Work_Start == 1)
|
||
|
|
{
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
//Swing 部分
|
||
|
|
if (GV.DH_CAN.Swing_Right == 1)
|
||
|
|
{
|
||
|
|
CurrentSwingState = SwingRight;
|
||
|
|
|
||
|
|
} else if (GV.DH_CAN.Swing_Left == 1)
|
||
|
|
{
|
||
|
|
CurrentSwingState = SwingLeft;
|
||
|
|
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
CurrentSwingState = SwingHALT;
|
||
|
|
}
|
||
|
|
//设定部分 setting region
|
||
|
|
if (GV.DH_CAN.Mode_Selection == 9) //换道距离设定
|
||
|
|
{
|
||
|
|
CurrentSetState = SetLaneChangeDistance;
|
||
|
|
|
||
|
|
} else if (GV.DH_CAN.Mode_Selection == 10) //后退距离设定
|
||
|
|
{
|
||
|
|
CurrentSetState = SetBackwardsDistance;
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
CurrentSetState = OtherMode;
|
||
|
|
}
|
||
|
|
|
||
|
|
//运动部分 Move Region
|
||
|
|
if (GV.DH_CAN.Move_Forward == 1)
|
||
|
|
{
|
||
|
|
CurrentMoveState = Move_Forwards;
|
||
|
|
} else if (GV.DH_CAN.Move_Backward == 1) //后退左转右转
|
||
|
|
{
|
||
|
|
CurrentMoveState = Move_Backwards;
|
||
|
|
} else if (GV.DH_CAN.Move_Turn_Left == 1)
|
||
|
|
{
|
||
|
|
CurrentMoveState = Move_TurnLeft;
|
||
|
|
|
||
|
|
} else if (GV.DH_CAN.Move_Turn_Right == 1) //右转
|
||
|
|
{
|
||
|
|
CurrentMoveState = Move_TurnRight;
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
CurrentMoveState = Move_HALT;
|
||
|
|
// change the parameter under the conditon of Halt
|
||
|
|
action_perfrom(SetTransitions,sizeof(SetTransitions)/sizeof(transition_t), CurrentSetState);
|
||
|
|
}
|
||
|
|
|
||
|
|
action_perfrom(MoveTransitions,sizeof(MoveTransitions)/sizeof(transition_t), CurrentMoveState);
|
||
|
|
action_perfrom(SwingTransitions, sizeof(SwingTransitions)/sizeof(transition_t),CurrentSwingState);
|
||
|
|
|
||
|
|
}
|
||
|
|
//左补偿，右补偿 摆臂角度，摆臂速度
|
||
|
|
GV.Swing_Speed = CV.SwingSpeedRange * GV.DH_CAN.Swing_Speed_Value / 255;
|
||
|
|
GV.Swing_Angle = CV.SwingAngleRange * GV.DH_CAN.Swing_Angle_Value / 255;
|
||
|
|
GV.Left_Compensation = CV.LeftCompensationRange
|
||
|
|
* GV.DH_CAN.Left_Compensation_Value / 255;
|
||
|
|
GV.Right_Compensation = CV.RightCompensationRange
|
||
|
|
* GV.DH_CAN.Right_Compensation_Value / 255;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void action_perfrom(transition_t transitions[], int length ,int state)
|
||
|
|
{
|
||
|
|
|
||
|
|
for (index_counter = 0;
|
||
|
|
index_counter < length;
|
||
|
|
index_counter++)
|
||
|
|
{
|
||
|
|
if (transitions[index_counter].State == state)
|
||
|
|
{
|
||
|
|
if (transitions[index_counter].robotRun != NULL)
|
||
|
|
{
|
||
|
|
transitions[index_counter].robotRun();
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
//return;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|