From 50c3cd6f3b5d3a5cb1f649e868f97d570e982f4e Mon Sep 17 00:00:00 2001 From: "LIN\\54376" <543769318@qq.com> Date: Sun, 28 Sep 2025 09:51:57 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B4=E7=90=86FSM?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/FSM.c | 213 +++++++++++++++++++----------------------------- Core/Src/main.c | 5 +- readme.txt | 27 ++---- 3 files changed, 98 insertions(+), 147 deletions(-) diff --git a/Core/Src/FSM.c b/Core/Src/FSM.c index 560c67f..9c1f108 100644 --- a/Core/Src/FSM.c +++ b/Core/Src/FSM.c @@ -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) { diff --git a/Core/Src/main.c b/Core/Src/main.c index 191a6a7..cbabd3c 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -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(); diff --git a/readme.txt b/readme.txt index 6eecb79..0c8121b 100644 --- a/readme.txt +++ b/readme.txt @@ -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 水平右换道 左上三键 前：前端上升 后：前端下降 右上三键 前：自动前进 后：自动后退 左上一键 打磨启停