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
Split 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;
int JJ_Flag;
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
void Robot_Move()
{
// //运动部分 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
@ -127,7 +116,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
GV.AuTo_Flag=1;
}
}
else if(P_U7->RU_Three == -1000)
{
@ -137,10 +125,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
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)
@ -171,14 +155,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
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)
@ -217,82 +193,77 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
CurrentMoveState = Move_HALT; //停车
}
}
else
{
CurrentMoveState = Move_HALT; //停车
}
}
else
{
CurrentMoveState = Move_HALT; //停车
}
}
void Front_End()
{
// 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
{
Polish_lock = 1; // 立即锁定，防止重复触发
/****************************************/
// 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
if (Polish_Flag == 0)
{
Polish_lock = 1; // 立即锁定，防止重复触发
if (Polish_Flag == 0)
{
process_state = 1; // 准备开俩设备
Vacuum_Delay_Count = 0; // 开吸尘器前，提前重置上一轮计数
}
else
{
process_state = 2; // 准备关闭俩设备
Polish_Count = 100; // 从100开始倒计时
}
process_state = 1; // 准备开俩设备
Vacuum_Delay_Count = 0; // 开吸尘器前，提前重置上一轮计数
}
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);
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++;
}
}
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);
else if (process_state == 2)
if (Polish_Count <= 0)
{
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--;
}
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)
@ -318,31 +289,31 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
// 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; // 上锁关闭后锁定，防止长按再次触发
}
}
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;
}
}
void Emergency()
{
/*急停版本2：手动复位按键版
* 急停：同时按下S3、S4则触发急停；
* 退出急停：极短的一段时间后，按下S3、S4任意键则解除
@ -353,7 +324,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
* 急停且自动锁定、急停退出但且自动锁定、急停退出且解除自动锁定
* */
if(P_U7->S3 == 1000 && P_U7->S4 == 1000)
{
GV.Emergency = 1;
@ -403,39 +373,33 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
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;
}
}
void GF_Dispatch()//2ms调用一次 给车体速度等赋值
{
// //运动部分 Move Region
//左补偿、右补偿
Lcompensation_control();
Rcompensation_control();
Robot_Move();
Front_End();
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;
@ -454,11 +418,6 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
}
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();
/* USER CODE BEGIN Init */
// HAL_Delay(10000); ////////////////////////////延时10s
/* USER CODE END Init */
/* Configure the system clock */
@ -150,6 +150,9 @@ int main(void)
SystemTimer_Intialize(); //加数数定时器
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
Error_Detect_Intialzie(500);//every 1 seconds
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
CH9_SY_H
左上三
右上三
左上一
右上一
R1
R2
R3
L1 竖直上换道
R1 竖直下换道
L2 水平左换道
R2 水平右换道
左上滑轮 左补偿
右上滑轮 右补偿
M1 竖直上换道
R5 竖直下换道
M2 水平左换道
R6 水平右换道
左上三键 前：前端上升 后：前端下降
右上三键 前：自动前进 后：自动后退
左上一键 打磨启停

Write Preview
Loading…
Cancel
Save