YuLin_Zhen
/
FW3.0
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity
Browse Source

整理FSM

master
LIN\54376 1 month ago
parent
6bf8ab7f1f
commit
50c3cd6f3b
3 changed files with 98 additions and 147 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 213
      Core/Src/FSM.c
  2. 5
      Core/Src/main.c
  3. 27
      readme.txt

213
Core/Src/FSM.c
View File

@ -96,19 +96,8 @@ void Fsm_Init()
//double Rocker_angle; //double Rocker_angle;
int JJ_Flag; int JJ_Flag;
void Robot_Move()
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; CurrentMoveState = Move_HALT;
if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000 if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
&& P_U7->M6 == -1000 && P_U7->M5 == -1000 && P_U7->M6 == -1000 && P_U7->M5 == -1000
@ -127,7 +116,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{ {
GV.AuTo_Flag=1; GV.AuTo_Flag=1;
} }
} }
else if(P_U7->RU_Three == -1000) else if(P_U7->RU_Three == -1000)
{ {
@ -137,10 +125,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
GV.AuTo_Flag=3; GV.AuTo_Flag=3;
} }
} }
else
{
CurrentMoveState = Move_HALT;//停车
}
} }
else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000 else if(!(P_U7->M2 == -1000 && P_U7->M1 == -1000
&& P_U7->M6 == -1000 && P_U7->M5 == -1000) && P_U7->M6 == -1000 && P_U7->M5 == -1000)
@ -171,14 +155,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{ {
CurrentMoveState = Move_ChgRight; 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) else if( !((P_U7->CH1_LYB_H >= -300 && P_U7->CH1_LYB_H <= 300)
@ -217,82 +193,77 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
CurrentMoveState = Move_HALT; //停车 CurrentMoveState = Move_HALT; //停车
} }
} }
else
{
CurrentMoveState = Move_HALT; //停车
}
} }
else else
{ {
CurrentMoveState = Move_HALT; //停车 CurrentMoveState = Move_HALT; //停车
} }
}
void Front_End()
{
// 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
{
Polish_lock = 1; // 立即锁定，防止重复触发
if (Polish_Flag == 0)
/****************************************/
// 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
{ {
Polish_lock = 1; // 立即锁定，防止重复触发 process_state = 1; // 准备开俩设备
Vacuum_Delay_Count = 0; // 开吸尘器前，提前重置上一轮计数
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) else
{ {
Polish_lock = 0; process_state = 2; // 准备关闭俩设备
Polish_Count = 100; // 从100开始倒计时
} }
}
else if (P_U7->LU_Single == -1000)
{
Polish_lock = 0;
}
if (process_state == 1) 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_Vacuum, 0); GF_BSP_GPIO_SetIO(Out_Polish, 0);
if (Vacuum_Delay_Count < 200)
{ if (Polish_Count >= 100)
Vacuum_Delay_Count++; {
} GF_BSP_GPIO_SetIO(Out_Polish, 1);
else Polish_Flag = 1;
{ process_state = 0; // 跑完耗时操作就退回初始状态
GF_BSP_GPIO_SetIO(Out_Polish, 0); Polish_Count = 0; // 重置计数
}
if (Polish_Count >= 100) else
{ {
GF_BSP_GPIO_SetIO(Out_Polish, 1); Polish_Count++;
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);
else if (process_state == 2) if (Polish_Count <= 0)
{ {
GF_BSP_GPIO_SetIO(Out_Vacuum, 1); GF_BSP_GPIO_SetIO(Out_Polish, 1);
GF_BSP_GPIO_SetIO(Out_Polish, 0); Polish_Flag = 0;
// GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
if (Polish_Count <= 0) process_state = 0; // 跑完退回
{
GF_BSP_GPIO_SetIO(Out_Polish, 1);
Polish_Flag = 0;
// GF_BSP_GPIO_SetIO(Out_Vacuum, 1);
process_state = 0; // 跑完退回
}
else
{
Polish_Count--;
}
} }
else
{
Polish_Count--;
}
}
//LU_Three控制打磨推杆升降 //LU_Three控制打磨推杆升降
if(P_U7->LU_Three==1000) if(P_U7->LU_Three==1000)
@ -318,31 +289,31 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
// Flag管当前喷枪状态、Lock管长短按问题 // Flag管当前喷枪状态、Lock管长短按问题
if (P_U7->RU_Single == 1000) if (P_U7->RU_Single == 1000)
{ {
if (Spray_lock == 0) // 未锁定时才处理 if (Spray_lock == 0) // 未锁定时才处理
{ {
if (Spray_Flag == 0)// 当前关闭，切换为开启 if (Spray_Flag == 0)// 当前关闭，切换为开启
{ {
GF_BSP_GPIO_SetIO(Out_Spray, 0); GF_BSP_GPIO_SetIO(Out_Spray, 0);
Spray_Flag = 1; Spray_Flag = 1;
Spray_lock = 1; // 上锁。一旦按下实现状态切换就锁定。 Spray_lock = 1; // 上锁。一旦按下实现状态切换就锁定。
} }
else // 当前开启，切换为关闭 else // 当前开启，切换为关闭
{ {
GF_BSP_GPIO_SetIO(Out_Spray, 1); GF_BSP_GPIO_SetIO(Out_Spray, 1);
Spray_Flag = 0; Spray_Flag = 0;
Spray_lock = 1; // 上锁关闭后锁定，防止长按再次触发 Spray_lock = 1; // 上锁关闭后锁定，防止长按再次触发
} }
} }
} }
else else
{ {
// 松开时复位锁定，允许下次按键触发 // 松开时复位锁定，允许下次按键触发
Spray_lock = 0; Spray_lock = 0;
} }
}
void Emergency()
{
/*急停版本2：手动复位按键版 /*急停版本2：手动复位按键版
* 急停：同时按下S3、S4则触发急停； * 急停：同时按下S3、S4则触发急停；
* 退出急停：极短的一段时间后，按下S3、S4任意键则解除 * 退出急停：极短的一段时间后，按下S3、S4任意键则解除
@ -353,7 +324,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
* 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定 * 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定
* */ * */
if(P_U7->S3 == 1000 && P_U7->S4 == 1000) if(P_U7->S3 == 1000 && P_U7->S4 == 1000)
{ {
GV.Emergency = 1; GV.Emergency = 1;
@ -403,39 +373,33 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
Polish_Flag = 0; Polish_Flag = 0;
Spray_Flag = 0; Spray_Flag = 0;
GF_BSP_GPIO_SetIO(Out_Spray,1); 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_Vacuum, 1);
GF_BSP_GPIO_SetIO(Out_Lift_2,1); GF_BSP_GPIO_SetIO(Out_Lift_2,1);
GF_BSP_GPIO_SetIO(Out_Lift_1,1); GF_BSP_GPIO_SetIO(Out_Lift_1,1);
GF_BSP_GPIO_SetIO(0,1); GF_BSP_GPIO_SetIO(0,1);
CurrentMoveState = Move_Emergency; CurrentMoveState = Move_Emergency;
} }
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
// //运动部分 Move Region
//左补偿、右补偿
Lcompensation_control();
Rcompensation_control();
Robot_Move();
Front_End();
Emergency();
/****************************************/
//Robot Speed 指向的是GV.movespeed //Robot Speed 指向的是GV.movespeed
GV.Robot_ManualSpeed = abs((GV.PV.Robot_ManualSpeedBase* 186.9 * (P_U7->CH0_LYB_V))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的 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的移动车体速度 这是整体车速 不是单个轮的 CV.TurnLeftRightSpeed = abs((2 * 186.9 * (P_U7->CH1_LYB_H))/ 1000);//RobotSpeed指向GV的移动车体速度 这是整体车速 不是单个轮的
GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9; GV.Robot_AutoSpeed = GV.PV.Robot_AutoSpeedBase* 186.9;
// //
IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll; IV.Robot_AngleRoll = GV.Robot_Angle.RF_Angle_Roll;
IV.Robot_ForceValue = GV.Robot_ForceValue; IV.Robot_ForceValue = GV.Robot_ForceValue;
IV.Robot_DynamometerValue = GV.Robot_DynamometerValue; IV.Robot_DynamometerValue = GV.Robot_DynamometerValue;
@ -454,11 +418,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
} }
void action_perfrom(transition_t transitions[], int length, int state) void action_perfrom(transition_t transitions[], int length, int state)
{ {

5
Core/Src/main.c
View File

@ -117,7 +117,7 @@ int main(void)
HAL_Init(); HAL_Init();
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */
// HAL_Delay(10000); ////////////////////////////延时10s
/* USER CODE END Init */ /* USER CODE END Init */
/* Configure the system clock */ /* Configure the system clock */
@ -150,6 +150,9 @@ int main(void)
SystemTimer_Intialize(); //加数数定时器 SystemTimer_Intialize(); //加数数定时器
GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�??????????? GF_BSP_GPIO_SetIO(0,0);//0�??????????? 1�???????????
GF_BSP_GPIO_SetIO(1,1);
HAL_Delay(10000); ////////////////////////////延时10s
DLT_LOG_ENABLE_LEVEL=0;//7 send all information 0 send nothing DLT_LOG_ENABLE_LEVEL=0;//7 send all information 0 send nothing
Error_Detect_Intialzie(500);//every 1 seconds Error_Detect_Intialzie(500);//every 1 seconds
GF_Robot_Init(); GF_Robot_Init();

27
readme.txt
View File

@ -1,5 +1,4 @@
五轮嵌入式3.0一号 五轮嵌入式3.0 五轮3.0的3、4号
有打磨模式下使用左右转检测到碰撞 回退 有打磨模式下使用左右转检测到碰撞 回退
@ -22,23 +21,13 @@ M3 == CH4_SA 1111
mode 123
左上滑轮 左上滑轮 左补偿
右上滑轮 右上滑轮 右补偿
CH8_SY_V M1 竖直上换道
CH9_SY_H R5 竖直下换道
左上三 M2 水平左换道
右上三 R6 水平右换道
左上一
右上一
R1
R2
R3
L1 竖直上换道
R1 竖直下换道
L2 水平左换道
R2 水平右换道
左上三键 前：前端上升 后：前端下降 左上三键 前：前端上升 后：前端下降
右上三键 前：自动前进 后：自动后退 右上三键 前：自动前进 后：自动后退
左上一键 打磨启停 左上一键 打磨启停

Write Preview
Loading…
Cancel
Save