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.
479 lines
12 KiB
479 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 Polish_lock = 0; // 打磨锁定标志（非自锁）
|
|
int Spray_Flag=0;//喷漆标志
|
|
int Spray_Count=0;
|
|
int Spray_lock = 0;// 喷漆打磨锁定标志（非自锁）
|
|
//int Vacuum_Flag = 0;
|
|
//int Vacuum_Count = 0;
|
|
int Vacuum_Delay_Count = 0; // Polish和Vacuum之间的延迟计数器
|
|
int emergency_lock_count = 0;// 急停：锁定计数器
|
|
int Operation_lock = 0; // 通用锁定标志
|
|
|
|
int process_state = 0; // 后台处理状态（0:空闲 1:开启中 2:关闭中）
|
|
|
|
//手动下 机器人运动
|
|
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},
|
|
|
|
};
|
|
|
|
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;
|
|
|
|
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
|
|
{
|
|
// //运动部分 Move Region
|
|
Lcompensation_control();
|
|
Rcompensation_control();
|
|
|
|
int b=Get_BIT(SystemErrorCode, u7_sbus) ;
|
|
if (Get_BIT(SystemErrorCode, u7_sbus) == 1)
|
|
{
|
|
|
|
}
|
|
|
|
CurrentMoveState = Move_HALT;
|
|
if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
|
|
&& P_U7->M6 == -1000 && P_U7->M5 == -1000
|
|
&& P_U7->RU_Three == 0)
|
|
&& (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
|
|
&& (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
|
|
{
|
|
if(P_U7->RU_Three != 0
|
|
&& P_U7->M2 == -1000 && P_U7->M1 == -1000
|
|
&& P_U7->M6 == -1000 && P_U7->M5 == -1000) //自动作业与换道互斥
|
|
{
|
|
if(P_U7->RU_Three == 1000)
|
|
{
|
|
CurrentMoveState = Move_AutoForward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=1;
|
|
}
|
|
|
|
}
|
|
else if(P_U7->RU_Three == -1000)
|
|
{
|
|
CurrentMoveState = Move_AutoBackward;
|
|
if(GV.AuTo_Flag == 0)
|
|
{
|
|
GV.AuTo_Flag=3;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
|
|
&& P_U7->M6 == -1000 && P_U7->M5 == -1000)
|
|
&& P_U7->RU_Three == 0)
|
|
{
|
|
if(GV.Chg_Flag == 1 && (P_U7->M1 == 1000 || P_U7->M2 == 1000
|
|
|| P_U7->M5 == 1000 || P_U7->M6 == 1000))
|
|
{
|
|
CurrentMoveState = Move_ChgFinish;
|
|
}
|
|
else if(P_U7->M1 == 1000
|
|
&& P_U7->M2 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
|
|
{
|
|
CurrentMoveState = Move_ChgUp;
|
|
}
|
|
else if(P_U7->M2 == 1000
|
|
&& P_U7->M1 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000)
|
|
{
|
|
CurrentMoveState = Move_ChgDown;
|
|
}
|
|
else if(P_U7->M5 == 1000
|
|
&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M6 == -1000)
|
|
{
|
|
CurrentMoveState = Move_ChgLeft;
|
|
}
|
|
else if(P_U7->M6 == 1000
|
|
&& P_U7->M1 == -1000 && P_U7->M2 == -1000 && P_U7->M5 == -1000)
|
|
{
|
|
CurrentMoveState = Move_ChgRight;
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if( !((P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
|
|
&&(P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300))
|
|
&& P_U7->M2 == -1000 && P_U7->M1 == -1000
|
|
&& P_U7->M6 == -1000 && P_U7->M5 == -1000
|
|
&& P_U7->RU_Three == 0)
|
|
{
|
|
if (P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)//前进周围300范围内才继续检测前进是否按下
|
|
{
|
|
if (P_U7->CH0_LYB_V > 300)//前进
|
|
{
|
|
CurrentMoveState = Move_Forwards;
|
|
}
|
|
else if (P_U7->CH0_LYB_V < -300)
|
|
{
|
|
CurrentMoveState = Move_Backwards; //后退
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT;//停车
|
|
}
|
|
}
|
|
else if (P_U7->CH0_LYB_V >= -300 && P_U7->CH0_LYB_V <= 300)
|
|
{
|
|
if (P_U7->CH1_LYB_H > 300)
|
|
{
|
|
CurrentMoveState = Move_TurnRight; //右转
|
|
}
|
|
else if (P_U7->CH1_LYB_H < -300)
|
|
{
|
|
CurrentMoveState = Move_TurnLeft; //左转
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
}
|
|
else
|
|
{
|
|
CurrentMoveState = Move_HALT; //停车
|
|
}
|
|
|
|
|
|
/****************************************/
|
|
|
|
// 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
|
|
if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
|
|
{
|
|
Polish_lock = 1; // 立即锁定，防止重复触发
|
|
|
|
if (Polish_Flag == 0)
|
|
{
|
|
process_state = 1; // 准备开俩设备
|
|
Vacuum_Delay_Count = 0; // 开吸尘器前，提前重置上一轮计数
|
|
}
|
|
else
|
|
{
|
|
process_state = 2; // 准备关闭俩设备
|
|
Polish_Count = 100; // 从100开始倒计时
|
|
}
|
|
}
|
|
else if (P_U7->LU_Single == -1000)
|
|
{
|
|
Polish_lock = 0;
|
|
}
|
|
|
|
if (process_state == 1)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Vacuum, 0);
|
|
if (Vacuum_Delay_Count < 200)
|
|
{
|
|
Vacuum_Delay_Count++;
|
|
}
|
|
else
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish, 0);
|
|
|
|
if (Polish_Count >= 100)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish, 1);
|
|
Polish_Flag = 1;
|
|
process_state = 0; // 跑完耗时操作就退回初始状态
|
|
Polish_Count = 0; // 重置计数
|
|
}
|
|
else
|
|
{
|
|
Polish_Count++;
|
|
}
|
|
}
|
|
}
|
|
|
|
else if (process_state == 2)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
|
|
GF_BSP_GPIO_SetIO(Out_Polish, 0);
|
|
|
|
if (Polish_Count <= 0)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Polish, 1);
|
|
Polish_Flag = 0;
|
|
// GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
|
|
process_state = 0; // 跑完退回
|
|
}
|
|
else
|
|
{
|
|
Polish_Count--;
|
|
}
|
|
}
|
|
|
|
//LU_Three控制打磨推杆升降
|
|
if(P_U7->LU_Three==1000)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,0);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
}
|
|
else if(P_U7->LU_Three==-1000)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,0);
|
|
}
|
|
else
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
}
|
|
|
|
|
|
|
|
// 控制电磁阀喷枪（非自锁版本）
|
|
// 功能：按一下开启，再按一下关闭，长按不会反复切换
|
|
// Flag管当前喷枪状态、Lock管长短按问题
|
|
if (P_U7->RU_Single == 1000)
|
|
{
|
|
if (Spray_lock == 0) // 未锁定时才处理
|
|
{
|
|
if (Spray_Flag == 0)// 当前关闭，切换为开启
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray, 0);
|
|
Spray_Flag = 1;
|
|
Spray_lock = 1; // 上锁。一旦按下实现状态切换就锁定。
|
|
}
|
|
else // 当前开启，切换为关闭
|
|
{
|
|
GF_BSP_GPIO_SetIO(Out_Spray, 1);
|
|
Spray_Flag = 0;
|
|
Spray_lock = 1; // 上锁关闭后锁定，防止长按再次触发
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 松开时复位锁定，允许下次按键触发
|
|
Spray_lock = 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
/*急停版本2：手动复位按键版
|
|
* 急停：同时按下S3、S4则触发急停；
|
|
* 退出急停：极短的一段时间后，按下S3、S4任意键则解除
|
|
*
|
|
* 急停一旦触发即锁定，并关闭全部设备；退出急停后，需手动复位所有被按下按键才能恢复控制
|
|
*
|
|
* 优先级：
|
|
* 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定
|
|
* */
|
|
|
|
|
|
if(P_U7->S3 == 1000 && P_U7->S4 == 1000)
|
|
{
|
|
GV.Emergency = 1;
|
|
Operation_lock = 1; // 急停时自动锁定
|
|
}
|
|
|
|
// 2急停锁定计时（仅在急停状态下计数，解决松手不同时问题）
|
|
if (GV.Emergency == 1)
|
|
{
|
|
if (emergency_lock_count < 500)
|
|
{
|
|
emergency_lock_count++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
emergency_lock_count = 0; // 非急停状态重置
|
|
}
|
|
|
|
if (GV.Emergency == 1 && emergency_lock_count >= 500)
|
|
{
|
|
if ((P_U7->S3 == 1000 && P_U7->S4 == -1000) ||
|
|
(P_U7->S3 == -1000 && P_U7->S4 == 1000))
|
|
{
|
|
GV.Emergency = 0;
|
|
emergency_lock_count = 0;
|
|
// 退出后保持锁定（Operation_lock仍为1），需手动解锁
|
|
}
|
|
}
|
|
|
|
// 解锁条件：按键全部复位：即，三键的推杆回中（0）且两键恢复默认状态（-1000）
|
|
// 仅在已退出急停（GV.Emergency==0）但仍锁定时生效
|
|
if (GV.Emergency == 0 && Operation_lock)
|
|
{
|
|
// if (P_U7->LU_Three == 0 && P_U7->LU_Single == -1000 && P_U7->RU_Single == -1000)
|
|
if (P_U7->LU_Three == 0)
|
|
{
|
|
Operation_lock = 0; // 满足条件，解锁
|
|
}
|
|
GF_BSP_GPIO_SetIO(0,0);
|
|
}
|
|
|
|
// 急停
|
|
if(GV.Emergency == 1
|
|
|| (GV.Emergency == 0 && Operation_lock == 1))
|
|
{
|
|
Polish_Flag = 0;
|
|
Spray_Flag = 0;
|
|
GF_BSP_GPIO_SetIO(Out_Spray,1);
|
|
// GF_BSP_GPIO_SetIO(Out_Polish,0);
|
|
// if(Polish_Count >= 100)
|
|
// {
|
|
// GF_BSP_GPIO_SetIO(Out_Polish,1);
|
|
// Polish_Flag = 0;
|
|
// // 重置相关状态变量
|
|
// Polish_Count = 0;
|
|
// }
|
|
// else
|
|
// {
|
|
// Polish_Count++;
|
|
// }
|
|
GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_2,1);
|
|
GF_BSP_GPIO_SetIO(Out_Lift_1,1);
|
|
GF_BSP_GPIO_SetIO(0,1);
|
|
|
|
|
|
CurrentMoveState = Move_Emergency;
|
|
}
|
|
|
|
|
|
|
|
|
|
//Robot Speed 指向的是GV.movespeed
|
|
GV.Robot_ManualSpeed = abs((GV.PV.Robot_ManualSpeedBase* 186.9 * (P_U7->CH0_LYB_V))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
|
|
|
|
GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9;
|
|
//
|
|
|
|
|
|
|
|
IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll;
|
|
IV.Robot_ForceValue = GV.Robot_ForceValue;
|
|
IV.Robot_DynamometerValue = GV.Robot_DynamometerValue;
|
|
IV.Robot_Current_Left = GV.LeftFrontMotor.Current;
|
|
IV.Robot_Current_Right = GV.RightFrontMotor.Current;
|
|
|
|
IV.Robot_Compensation_Left = GV.Left_Compensation;
|
|
IV.Robot_Compensation_Right = GV.Right_Compensation;
|
|
|
|
IV.Robot_Error_Left = GV.LeftFrontMotor.ERROR_Flag;
|
|
IV.Robot_Error_Right = GV.RightFrontMotor.ERROR_Flag;
|
|
IV.Robot_CurrentState = CurrentMoveState;
|
|
|
|
|
|
action_perfrom(MoveTransitions, sizeof(MoveTransitions) / sizeof(transition_t), CurrentMoveState);//结构体数组/大小/当前状态
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
|