整理FSM

Core/Src/FSM.c

@@ -96,19 +96,8 @@ void Fsm_Init()
 //double Rocker_angle;
 
 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;
 	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,19 +193,14 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
 				CurrentMoveState = Move_HALT;  //停车
 			}
 		}
+	}
 	else
 	{
 		CurrentMoveState = Move_HALT;  //停车
 	}
 }
-	else
+void Front_End()
 {
-		CurrentMoveState = Move_HALT;  //停车
-	}
-
-
-	/****************************************/
-
 	//	 版本二：LU_Single 控制打磨电机和吸尘器（非自锁版本）
 	if (P_U7->LU_Single == 1000 && Polish_lock == 0 && process_state == 0)
 	{
@@ -340,9 +311,9 @@ void GF_Dispatch()//2ms调用一次 给车体速度等赋值
 		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)
 {
 

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();

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
+M1 竖直上换道
-CH9_SY_H
+R5 竖直下换道
-左上三
+M2 水平左换道
-右上三
+R6 水平右换道
-左上一
-右上一
-R1
-R2
-R3
-
-L1 竖直上换道
-R1 竖直下换道
-L2 水平左换道
-R2 水平右换道
 左上三键 前：前端上升 后：前端下降
 右上三键 前：自动前进 后：自动后退
 左上一键 打磨启停