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.
484 lines
12 KiB
484 lines
12 KiB
/*
|
|
* fsm.c
|
|
*
|
|
* Created on: Oct 18, 2024
|
|
* Author: akeguo
|
|
*/
|
|
|
|
#include "fsm.h"
|
|
|
|
#include "BHBF_ROBOT.h"
|
|
#include "bsp_include.h"
|
|
|
|
#include "robot_state.h"
|
|
int32_t *RobotSpeed;
|
|
//int32_t JontSwingSpeed;
|
|
//int32_t JontTiltSpeed;
|
|
int32_t JontSwingSpeed=400;
|
|
int32_t JontTiltSpeed=400;
|
|
void action_perfrom(transition_t transitions[], int length, int state);
|
|
void GF_Dispatch();
|
|
void action_perfrom();
|
|
MoveSTATE_t CurrentMoveState;
|
|
SwingSTATE_t CurrentSwingState;
|
|
TiltSTATE_t CurrentTiltState; //点头，抬头
|
|
SetSTATE_t CurrentSetState; //设置
|
|
int index_counter = 0;
|
|
int polish_Flag=0;//打磨标志
|
|
int Polish_Count = 0;//打磨启停上升沿计数
|
|
int Out_Lift_Count = 0;
|
|
int Spray_Flag=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 },
|
|
{ Move_AutoForward, Auto},
|
|
{ Move_AutoBackward, Auto},
|
|
{ Move_ChgLeft, ChgLeft},
|
|
{ Move_ChgRight, ChgRight},
|
|
{ Move_ChgUp, ChgUp},
|
|
{ Move_ChgDown, ChgDown},
|
|
{ Move_ChgFinish, ChgFinish},
|
|
{ Move_ChgFinish, HALT_State_Do},
|
|
|
|
};
|
|
|
|
transition_t SwingTransitions[] =
|
|
{
|
|
|
|
{ SwingHALT, SwingHALT_State_Do },
|
|
{ SwingLeft, SwingLeft_State_Do },
|
|
{ SwingRight, SwingRight_State_Do },
|
|
{ SwingHome, SwingHome_Do }, };
|
|
|
|
transition_t SetTransitions[] =
|
|
{
|
|
|
|
{ OtherMode, OtherMode_State_Do },
|
|
{ SetLaneChangeDistance, LaneChangeDistance_Setting_State_Do },
|
|
{ SetBackwardsDistance, BackWardsDistance_Setting_State_Do },
|
|
|
|
};
|
|
|
|
transition_t TiltTransitions[] =
|
|
{
|
|
|
|
{ TiltHALT, TiltHalt_Do },
|
|
{ TiltUP, TiltUp_Do },
|
|
{ TiltDown, TiltDown_Do },
|
|
{ TiltHome, TiltHome_Do },
|
|
{ TiltCurrentModeDown, TiltCurrentModeDown_Do },
|
|
};
|
|
|
|
void Fsm_Init()
|
|
{
|
|
GF_BSP_Interrupt_Add_CallBack(
|
|
DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, GF_Dispatch);
|
|
}
|
|
//状态迁移
|
|
|
|
//double Y_Value_1;
|
|
//double X_Value_1;
|
|
//double Rocker_angle;
|
|
|
|
int JJ_Flag;
|
|
|
|
double Speed_Judge()
|
|
{
|
|
if(DHRougheningContrller.Vehicle_Speed1 == 1)
|
|
{
|
|
return 0.1;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed2 == 1)
|
|
{
|
|
return 0.3;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed3 == 1)
|
|
{
|
|
return 0.5;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed4 == 1)
|
|
{
|
|
return 0.8;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed5 == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed6 == 1)
|
|
{
|
|
|
|
return 3;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed7 == 1)
|
|
{
|
|
|
|
return 5;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed8 == 1)
|
|
{
|
|
return 8;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed9 == 1)
|
|
{
|
|
return 10;
|
|
}
|
|
if(DHRougheningContrller.Vehicle_Speed10 == 1)
|
|
{
|
|
return 15;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void Robot_Halt(MoveSTATE_t STATE)
|
|
{
|
|
|
|
if(DHRougheningContrller.Horizontal_Task == 1
|
|
|| DHRougheningContrller.Vertical_Task == 1)
|
|
{
|
|
GV.AuTo_Flag = 99;
|
|
}
|
|
else
|
|
{
|
|
GV.AuTo_Flag = 0;
|
|
}
|
|
|
|
|
|
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|
|
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
|
|
{
|
|
GV.Chg_Flag = 1;
|
|
Chg_Pa.change_road_finish_flag = 1;
|
|
}
|
|
else
|
|
{
|
|
GV.Chg_Flag = 0;
|
|
Chg_Pa.change_road_finish_flag = 0;
|
|
}
|
|
|
|
CurrentMoveState = STATE; //停车
|
|
}
|
|
|
|
void Robot_ResetJudge()
|
|
{
|
|
if(DHRougheningContrller.Wireless_State == 0)
|
|
{
|
|
return;
|
|
}
|
|
if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
|
|
&& DHRougheningContrller.Vertical_Task == 0
|
|
&& DHRougheningContrller.Horizontal_Task ==0)
|
|
&& DHRougheningContrller.Forwards == 0
|
|
&& DHRougheningContrller.Backwards == 0
|
|
&& DHRougheningContrller.Left_Turn == 0
|
|
&& DHRougheningContrller.Right_Turn == 0
|
|
&& DHRougheningContrller.Roughening_Start == 0
|
|
&& DHRougheningContrller.End_Down == 0
|
|
&& DHRougheningContrller.End_Up == 0)
|
|
{
|
|
GV.Robot_Move_Step = 1;
|
|
}
|
|
}
|
|
|
|
void Robot_Move()
|
|
{
|
|
// 急停版本一
|
|
if(DHRougheningContrller.Emergency_Stop == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
|
|
if(Polish_Count == 100 && polish_Flag == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish,1);
|
|
polish_Flag = 99;
|
|
Polish_Count = 0;
|
|
}
|
|
else if(polish_Flag == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish,0);
|
|
Polish_Count++;
|
|
}
|
|
Robot_Halt(Move_Emergency);
|
|
|
|
return;
|
|
}
|
|
else if(DHRougheningContrller.Emergency_Stop == 0)
|
|
{
|
|
if(DHRougheningContrller.Roughening_Stop && polish_Flag == 99)
|
|
{
|
|
polish_Flag = 0;
|
|
}
|
|
}
|
|
|
|
|
|
//运动部分 Move Region
|
|
if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|
|
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Up == 1
|
|
|| DHRougheningContrller.Vertical_Task == 1
|
|
|| DHRougheningContrller.Horizontal_Task ==1)
|
|
&& DHRougheningContrller.Forwards == 0
|
|
&& DHRougheningContrller.Backwards == 0
|
|
&& DHRougheningContrller.Left_Turn == 0
|
|
&& DHRougheningContrller.Right_Turn == 0)
|
|
{
|
|
if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
|
|
&& (DHRougheningContrller.Vertical_Task == 1
|
|
|| DHRougheningContrller.Horizontal_Task == 1)) //自动作业与换道互斥
|
|
{
|
|
if(DHRougheningContrller.Horizontal_Task == 1)
|
|
{
|
|
CurrentMoveState = Move_AutoForward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=1;
|
|
}
|
|
}
|
|
else if(DHRougheningContrller.Vertical_Task == 1)
|
|
{
|
|
CurrentMoveState = Move_AutoBackward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=3;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 1
|
|
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Up == 1)
|
|
&& (DHRougheningContrller.Vertical_Task == 0
|
|
&& DHRougheningContrller.Horizontal_Task == 0))
|
|
{
|
|
if(GV.Chg_Flag == 1
|
|
&& (DHRougheningContrller.LaneChange_Horizontal_Left == 1
|
|
|| DHRougheningContrller.LaneChange_Horizontal_Right == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Down == 1
|
|
|| DHRougheningContrller.LaneChange_Vertical_Up == 1))
|
|
{
|
|
CurrentMoveState = Move_ChgFinish;
|
|
}
|
|
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 1 )
|
|
{
|
|
CurrentMoveState = Move_ChgUp;
|
|
}
|
|
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 1
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
|
|
{
|
|
CurrentMoveState = Move_ChgDown;
|
|
}
|
|
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 1
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
|
|
{
|
|
CurrentMoveState = Move_ChgLeft;
|
|
}
|
|
else if(DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 1
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0 )
|
|
{
|
|
CurrentMoveState = Move_ChgRight;
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if((DHRougheningContrller.LaneChange_Horizontal_Left == 0
|
|
&& DHRougheningContrller.LaneChange_Horizontal_Right == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Down == 0
|
|
&& DHRougheningContrller.LaneChange_Vertical_Up == 0
|
|
&& DHRougheningContrller.Vertical_Task == 0
|
|
&& DHRougheningContrller.Horizontal_Task == 0)
|
|
|
|
&& (DHRougheningContrller.Forwards == 1
|
|
|| DHRougheningContrller.Backwards == 1
|
|
|| DHRougheningContrller.Left_Turn == 1
|
|
|| DHRougheningContrller.Right_Turn == 1))
|
|
{
|
|
if (DHRougheningContrller.Left_Turn == 0
|
|
&& DHRougheningContrller.Right_Turn == 0)//前进周围300范围内才继续检测前进是否按下
|
|
{
|
|
if (DHRougheningContrller.Forwards == 1)//前进
|
|
{
|
|
CurrentMoveState = Move_Forwards;
|
|
}
|
|
else if (DHRougheningContrller.Backwards == 1)
|
|
{
|
|
CurrentMoveState = Move_Backwards; //后退
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if (DHRougheningContrller.Forwards == 0
|
|
&& DHRougheningContrller.Backwards == 0)
|
|
{
|
|
if (DHRougheningContrller.Right_Turn == 1)
|
|
{
|
|
CurrentMoveState = Move_TurnRight; //右转
|
|
}
|
|
else if (DHRougheningContrller.Left_Turn == 1)
|
|
{
|
|
CurrentMoveState = Move_TurnLeft; //左转
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Robot_Halt(Move_HALT); //停车
|
|
}
|
|
/****************************************/
|
|
//LU_Single 控制打磨电机
|
|
if(DHRougheningContrller.Roughening_Start && polish_Flag == 0)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Vacucm,0);
|
|
GF_BSP_GPIO_SetIO(Out_Polish,0);
|
|
if(Polish_Count == 100)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish,1);
|
|
polish_Flag = 1;
|
|
}
|
|
else
|
|
{
|
|
Polish_Count++;
|
|
}
|
|
}
|
|
else if(DHRougheningContrller.Roughening_Stop && polish_Flag == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Vacucm,1);
|
|
GF_BSP_GPIO_SetIO(Out_Polish,0);
|
|
if(Polish_Count == 100)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish,1);
|
|
polish_Flag = 0;
|
|
}
|
|
else
|
|
{
|
|
Polish_Count++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Polish_Count=0;
|
|
}
|
|
|
|
//LU_Three控制打磨升降
|
|
if(DHRougheningContrller.End_Up)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
}
|
|
else if(DHRougheningContrller.End_Down)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
|
|
}
|
|
else
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
}
|
|
}
|
|
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
|
|
{
|
|
switch(GV.Robot_Move_Step)
|
|
{
|
|
case 0:
|
|
Robot_ResetJudge();
|
|
break;
|
|
case 1:
|
|
Robot_Move();
|
|
default:
|
|
break;
|
|
}
|
|
|
|
IV.Robot_CurrentState = CurrentMoveState;
|
|
|
|
//Robot Speed 指向的是GV.movespeed
|
|
GV.Robot_Speed_Mpm = Speed_Judge();
|
|
GV.Robot_ManualSpeed = abs(GV.Robot_Speed_Mpm * 186.9 );//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
CV.TurnLeftRightSpeed = abs(2 * 186.9 );//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
|
|
|
|
GV.Robot_AutoSpeed = GV.Robot_Speed_Mpm * 186.9;
|
|
// magic data：65，控制换道距离，对应10cm左右
|
|
GV.Left_Compensation = DHRougheningContrller.Left_Compensation / 13056.0;
|
|
GV.Right_Compensation = 65 - DHRougheningContrller.Right_Compensation / 10043.0 * 10;
|
|
|
|
|
|
action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
|
|
// action_perfrom(SwingTransitions,sizeof(SwingTransitions) / sizeof(transition_t), CurrentSwingState);
|
|
// action_perfrom(TiltTransitions, sizeof(TiltTransitions) / sizeof(transition_t), CurrentTiltState);
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|