diff --git a/Core/BASE/Inc/BSP/BHBF_ROBOT.h b/Core/BASE/Inc/BSP/BHBF_ROBOT.h
index 28ed1ef..6cc8796 100644
--- a/Core/BASE/Inc/BSP/BHBF_ROBOT.h
+++ b/Core/BASE/Inc/BSP/BHBF_ROBOT.h
@@ -31,7 +31,7 @@
 #include "BSP/bsp_UART.h"
 #include "BSP/DLTuc.h"
 #include "BSP/bsp_DLT_Log.h"
-
+#include "bsp_devic_moniter.h"
 
 #include "BSP/bsp_cpu_flash.h"
 #include "BSP/bsp_qspi_w25q128.h"
diff --git a/Core/BASE/Inc/BSP/bsp_devic_moniter.h b/Core/BASE/Inc/BSP/bsp_devic_moniter.h
new file mode 100644
index 0000000..4a2d4a7
--- /dev/null
+++ b/Core/BASE/Inc/BSP/bsp_devic_moniter.h
@@ -0,0 +1,56 @@
+/*
+ * bsp_devic_moniter.h
+ *
+ *  Created on: 2026年6月1日
+ *      Author: bm673
+ */
+
+#ifndef BASE_INC_BSP_BSP_DEVIC_MONITER_H_
+#define BASE_INC_BSP_BSP_DEVIC_MONITER_H_
+
+#include <stdint.h>
+#include "stm32h7xx_hal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*==========================
+ * 设备ID
+ *==========================*/
+typedef enum {
+    //DEV_GYRO = 0,
+    //DEV_SBUS,
+    DEV_LEFT_MOTOR=0,
+    //DEV_RIGHT_MOTOR,
+    DEV_COUNT   // 总设备数
+} DeviceId;
+
+/*==========================
+ * 错误码，每位对应一个设备
+ *==========================*/
+extern uint32_t g_error_code;
+
+/*==========================
+ * 接口函数
+ *==========================*/
+
+/* 初始化链表及错误码 */
+void DevMon_Init();
+
+/* 心跳上报 */
+void DevMon_Feed(DeviceId id);
+
+/* 轮询检测超时，更新错误码 */
+void DevMon_Task();
+
+/* 判断设备是否在线 */
+uint8_t DevMon_IsOnline(DeviceId id);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif /* BASE_INC_BSP_BSP_DEVIC_MONITER_H_ */
diff --git a/Core/BASE/Src/BSP/bsp_UART.c b/Core/BASE/Src/BSP/bsp_UART.c
index 746310d..855fe74 100644
--- a/Core/BASE/Src/BSP/bsp_UART.c
+++ b/Core/BASE/Src/BSP/bsp_UART.c
@@ -391,6 +391,7 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
 //		GF_BSP_Interrupt_Run_CallBack(
 //				DF_BSP_InterCall_E22_Serial_RxCpltCallback);
 		UARTHandlerRX(&LTE_7S0_Serial_UART_Handler);
+
 	} else if (huart->Instance == UART5)		//**E28	// SBUS
 	{
 
diff --git a/Core/BASE/Src/BSP/bsp_devic_moniter.c b/Core/BASE/Src/BSP/bsp_devic_moniter.c
new file mode 100644
index 0000000..0eba249
--- /dev/null
+++ b/Core/BASE/Src/BSP/bsp_devic_moniter.c
@@ -0,0 +1,140 @@
+/*
+ * Devic_moniter.c
+ *
+ *  Created on: 2026年6月1日
+ *      Author: bm673
+ */
+/*
+ * bsp_devic_monitor.c
+ * 静态链表版设备心跳监测
+ */
+
+#include "bsp_devic_moniter.h"
+#include <string.h>
+#include "BSP/bsp_include.h"
+
+/* 注册设备到链表 */
+void DevMon_Register(DeviceId id,uint32_t timeout_ms, uint32_t first_delay_ms);
+/*==========================
+ * 节点结构
+ *==========================*/
+typedef struct {
+    DeviceId id;             // 设备ID
+    uint32_t timeout_ms;     // 正常心跳超时
+    uint32_t first_delay_ms; // 首次上报允许延迟
+    uint32_t last_tick;      // 上次心跳时间
+    uint8_t first_reported;  // 是否收到首次心跳
+    int next;                // 链表索引
+} DevNode_t;
+
+/*==========================
+ * 静态链表池
+ *==========================*/
+static DevNode_t s_node[DEV_COUNT];
+static int s_head = -1;       // 链表头
+
+/* 全局错误码 */
+uint32_t g_error_code = 0;
+
+/*==========================
+ * 初始化
+ *==========================*/
+void DevMon_Init()
+{
+	uint32_t tick=HAL_GetTick();
+    memset(s_node, 0, sizeof(s_node));//将节点数组都置位
+    s_head = -1;                      //设置链表为空
+    g_error_code = 0;                 //设置错误为空
+
+    for (int i = 0; i < DEV_COUNT; i++) {
+        s_node[i].last_tick = tick;   //更新系统失控
+        s_node[i].next = -1;          //这为啥都置位-1我是不理解的
+        s_node[i].id = (DeviceId)i;   // 初始化ID
+
+    }
+
+    //DevMon_Register(DEV_GYRO, 1000, 1000);
+    //DevMon_Register(DEV_SBUS, 1000, 3000);
+    DevMon_Register(DEV_LEFT_MOTOR, 1000, 30000);
+    //DevMon_Register(DEV_RIGHT_MOTOR, 200, 4000);
+
+	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_TIM8_2ms_PeriodElapsedCallback, DevMon_Task);
+}
+
+/*==========================
+ * 注册设备
+ *==========================*/
+void DevMon_Register(DeviceId id,
+                     uint32_t timeout_ms,
+                     uint32_t first_delay_ms)
+{
+    if (id >= DEV_COUNT) return;
+
+    s_node[id].timeout_ms = timeout_ms;
+    s_node[id].first_delay_ms = first_delay_ms;
+    s_node[id].first_reported = 0;
+    s_node[id].next = s_head;
+    s_head = id;
+
+
+    g_error_code &= ~(1U << id);     // 清除错误位
+}
+
+/*==========================
+ * 心跳上报
+ *==========================*/
+void DevMon_Feed(DeviceId id)
+{
+	uint32_t tick=HAL_GetTick();
+
+    if (id >= DEV_COUNT) return;  //
+
+    s_node[id].last_tick = tick;
+    s_node[id].first_reported = 1;
+
+    g_error_code &= ~(1U << id);     //把第id位清0，其他不变
+}
+
+/*==========================
+ * 周期检测
+ *==========================*/
+uint32_t elapsed;
+uint32_t timeout;
+uint32_t last_time;
+uint32_t now_time;
+void DevMon_Task()
+{
+	uint32_t tick=HAL_GetTick();
+    int curr = s_head;
+    while (curr != -1)
+    {
+        DevNode_t *node = &s_node[curr];              //这句话，我不太理解
+
+        elapsed = tick - node->last_tick;    //这句话我理解，就是在判断是否超时
+        last_time=node->last_tick;
+        now_time=tick;
+        timeout = node->first_reported ? node->timeout_ms : node->first_delay_ms;
+
+
+        if (elapsed > timeout)
+            g_error_code |= (1U << node->id);     //把第id位置1，其他不变
+        else
+            g_error_code &= ~(1U << node->id);    //把第id位清0，其他不变
+
+        curr = node->next;
+    }
+//    也可以用for替代
+}
+
+/*==========================
+ * 判断在线
+ *==========================*/
+uint8_t DevMon_IsOnline(DeviceId id)
+{
+    if (id >= DEV_COUNT) return 0;
+
+    return (g_error_code & (1U << id)) ? 0 : 1;
+}
+
+
+
diff --git a/Core/BASE/Src/MSP/msp_MK32_1.c b/Core/BASE/Src/MSP/msp_MK32_1.c
index 40277fe..a80c080 100644
--- a/Core/BASE/Src/MSP/msp_MK32_1.c
+++ b/Core/BASE/Src/MSP/msp_MK32_1.c
@@ -60,6 +60,8 @@ void decode_MK32Data(uint8_t *buffer, uint16_t length)
 	HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
 	        "mk32_sbus", 1);
 
+
+
 }
 
 void Sbus_Data_Count(uint8_t *buf, int32_t *But_Value)
diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c
index e645e1c..6c533a9 100644
--- a/Core/FSM/Src/Handset_Status_Setting.c
+++ b/Core/FSM/Src/Handset_Status_Setting.c
@@ -396,10 +396,10 @@ void PV_control(void)
     }
 
     //摆臂速度不允许为1，若设置了1，则强制置为2
-    if(GV.Robot_Swing_Speed==1)
-    {
-    	GV.Robot_Swing_Speed=2;
-    }
+//    if(GV.Robot_Swing_Speed==1)
+//    {
+//    	GV.Robot_Swing_Speed=2;
+//    }
 }
 
 /**
@@ -435,10 +435,10 @@ void IV_control(void)
     }
 
     //摆臂速度不允许为1，若设置了1，则强制置为2
-    if(GV.Robot_Swing_Speed==1)
-    {
-    	GV.Robot_Swing_Speed=2;
-    }
+//    if(GV.Robot_Swing_Speed==1)
+//    {
+//    	GV.Robot_Swing_Speed=2;
+//    }
 
     if((P_MK32->CH5_SB!=0)||(P_MK32->CH7_SD!=0))
     {
diff --git a/Core/FSM/Src/fsm_state_control.c b/Core/FSM/Src/fsm_state_control.c
index 1126bb5..389ad02 100644
--- a/Core/FSM/Src/fsm_state_control.c
+++ b/Core/FSM/Src/fsm_state_control.c
@@ -350,10 +350,11 @@ void GF_Dispatch(void)
         g_debug_current_action_func = prev_action_func;
     }
 
-    if(robot_start_flag==0) //保证上电之后摆臂就能动,不加这句的话，就要先动其它摇杆，摆臂才能动
+    if(robot_start_flag==0) //保证上电之后摆臂和推杆就能动,不加这句的话，就要先动其它摇杆，摆臂和推杆才能动
     {
     	Robot_Swing_Operation_Function();
     	get_swing_mode();
+    	PaintGun_Contronl();
     }
 
     flag_reset();
@@ -366,6 +367,8 @@ void GF_Dispatch(void)
 
     //tuigan_auto();
 
+    watch_dog=DevMon_IsOnline(DEV_LEFT_MOTOR);
+
 }
 
 /*-------------------------------------------------------------------
@@ -426,7 +429,7 @@ int AbnormalDetect(void)
 	cnt++;
 
 	// ====================== 第三步：需要延迟触发的错误 ======================
-	if (is_error == 1 || is_error == 2 || is_error == 3 ||is_error == 6||is_error == 8)
+	if (is_error == 1 || is_error == 3 ||is_error == 6||is_error == 8)
 	{
 		if (cnt > 1000)
 		{
@@ -437,7 +440,7 @@ int AbnormalDetect(void)
 
 		}
 		if (is_error == 1) return 1;
-		if (is_error == 2) return 2;
+		//if (is_error == 2) return 2;
 		if (is_error == 3) return 3;
 		if (is_error == 6) return 6;
 		if (is_error == 8) return 8;
diff --git a/Core/FSM/Src/motor.c b/Core/FSM/Src/motor.c
index e621f4a..c3a3c04 100644
--- a/Core/FSM/Src/motor.c
+++ b/Core/FSM/Src/motor.c
@@ -15,7 +15,7 @@ DispacherController *Roughening_DispacherController_CAN2;
 
 
 char TT_Motor_Need_To_Activate = 1;
-char TT_Motor_Need_To_Activate_1 = 1;
+int TT_Motor_Need_To_Activate_1 = 1;
 
 float Move_Base_Speed_Count_m_Min=201.7*7.64;//天太电机1m/min
 float Swing_Speed_Deg_Sencond_motor=201.7;//HJ32-121
@@ -74,12 +74,7 @@ void Motor_Controller_intialize_CAN2(FDCANHandler *Handler)
 
 void MotorCommandsLoop()
 {
-//
-//	if(start_time<15000)
-//	{
-//		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
-//		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID2_RightMotor", 1);
-//	}
+
 	GV.LeftMotor.Target_Velcity=(int)(GV.Left_Speed_M_min*Move_Base_Speed_Count_m_Min);
 	GV.RightMotor.Target_Velcity=(int)(GV.Right_Speed_M_min*Move_Base_Speed_Count_m_Min);
 	GV.robot_real_speed=((float)GV.LeftMotor.Real_Velcity)*10/Move_Base_Speed_Count_m_Min;
@@ -97,8 +92,6 @@ void MotorCommandsLoop()
 		ActivateMotor(2, Roughening_Motor_Controller, 2000);
 
 
-
-
 		Enable_NMT(000,Roughening_Motor_Controller,01,1000);
 		Enable_NMT(000,Roughening_Motor_Controller,02,1000);
 
@@ -136,16 +129,17 @@ void MotorCommandsLoop()
 }
 
 
-
+int32_t LAST_POSITON=0;
+int32_t LAST_SPEED=0;
+int32_t delta_pos=0;
 void MotorCommandsLoop_2_Position()
 {
-//	if(start_time<15000)
-//	{
-//	    HardWareErrorController->Set_PCOMHardWare(HardWareErrorController, "ZQ_CAN_ID3_SwingMotor", 1);
-//	}
+
 	GV.SwingMotor.Tar_Position_count=(int32_t)(middle_position_motor-GV.Tar_Position_angle*TT_One_Deg_Count_motor);
 	GV.SwingMotor.Tar_Position_Velcity_RPM=GV.Tar_Position_Velcity_Degree_S*Swing_Speed_Deg_Sencond_motor;
 
+
+	delta_pos=GV.SwingMotor.Real_Position-GV.SwingMotor.Last_Real_Position;
 	if (TT_Motor_Need_To_Activate_1 == 1)
 	{
 		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 6000);
@@ -153,7 +147,7 @@ void MotorCommandsLoop_2_Position()
 		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
 		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
 		ActivateMotor(3, Roughening_Motor_Controller_CAN2, 2000);
-		Postion_Velcocity_Run_SetParameter(                   3,                      0,                    0,             500,             500,  Roughening_Motor_Controller_CAN2,    100   );
+		Postion_Velcocity_Run_SetParameter(                   3,                      0,                    0,             100,             100,  Roughening_Motor_Controller_CAN2,    100   );
 		TT_Motor_Need_To_Activate_1 = 0;
 	}
 	else
@@ -168,7 +162,12 @@ void MotorCommandsLoop_2_Position()
 		switch(GV.SwingMotor.Position_immediately1_Lag2)
 		{
 			case 1:
+
 				Position_Immediately_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
+
+				LAST_POSITON=GV.SwingMotor.Tar_Position_count;
+				LAST_SPEED=GV.SwingMotor.Tar_Position_Velcity_RPM;
+
 				break;
 			case 2:
 				Position_Lag_Setting(3,Roughening_Motor_Controller_CAN2, GV.SwingMotor.Tar_Position_count,GV.SwingMotor.Tar_Position_Velcity_RPM,30);
@@ -201,7 +200,8 @@ void MotorCommandsLoop_2()
 			TT_Request_Current(3, Roughening_Motor_Controller_CAN2, 6);
 			TT_Request_Fault(3, Roughening_Motor_Controller_CAN2, 6);
 		}
-//		TT_SpeedMode_Set_TargetSpeed(3, Roughening_Motor_Controller_CAN2, 6,GV.SwingMotor.Target_Velcity);
+		GV.SwingMotor.Target_Velcity=(P_MK32->CH10_LD1+1000);//400-6051
+		TT_SpeedMode_Set_TargetSpeed(3, Roughening_Motor_Controller_CAN2, 6,GV.SwingMotor.Target_Velcity);
 		Heartbeat_Flag++;
 		if(Heartbeat_Flag%15==0)
 		{
@@ -226,6 +226,7 @@ void Roughening_MotorDecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
 		case 1:
 		{
 			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"ZQ_CAN_ID1_LeftMotor", 1);
+			DevMon_Feed(DEV_LEFT_MOTOR);
 			TT_Analytic_Fun(1, buffer);
 		}
 			break;
diff --git a/Core/FSM/Src/paint_gun_action.c b/Core/FSM/Src/paint_gun_action.c
index 8205412..b07c377 100644
--- a/Core/FSM/Src/paint_gun_action.c
+++ b/Core/FSM/Src/paint_gun_action.c
@@ -47,7 +47,7 @@ void PaintGun_OFF_Exit()
 }
 
 IO_State   curr_io_state;
-//无压力传感器数值下控制推杆函数
+//手动控制下控制推杆函数
 void PaintGun_Contronl()
 {
 	GV.Now_press=strainGaugeValue->Pressure;
@@ -98,7 +98,7 @@ void PaintGun_Contronl()
 }
 
 
-//有压力传感器数值下控制推杆函数
+//自动作业状态下控制推杆函数
 void PaintGun_Contronl_Press()
 {
 	GV.Now_press=strainGaugeValue->Pressure;
diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c
index d8f9b35..8a7733e 100644
--- a/Core/FSM/Src/robot_move_actions.c
+++ b/Core/FSM/Src/robot_move_actions.c
@@ -354,6 +354,8 @@ static void check_and_reset_fsm(FsmType current) {
         }
         s_lastActiveFsm = current;   // 更新上次类型
         Change_stop_flag=1;
+
+
     }
     // 如果相同，不做任何操作
 }
diff --git a/Core/FSM/Src/swing_action.c b/Core/FSM/Src/swing_action.c
index ad76614..a00814c 100644
--- a/Core/FSM/Src/swing_action.c
+++ b/Core/FSM/Src/swing_action.c
@@ -27,25 +27,47 @@ float right_compare_updata;
 int lef_positon=-569580;
 int Right_positon=-1230420;
 
+int swing_delay_count=0;
+int com_delay=0;
+int pos_flag=0;   //控制延时停摆臂只读一次角度
+int pos_direction=0; //获取延时停摆臂的方向
 //手动摆臂函数
 void Robot_Swing_Operation_Function()
 {
+	com_delay=(P_MK32->CH10_LD1+1000)/2;
 	if(P_MK32->CH0_RY_H>500)
 	{
-		GV.SwingMotor.Position_immediately1_Lag2=1;
-		GV.Tar_Position_angle=90;
-		GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		swing_delay_count++;
+		if(swing_delay_count>com_delay)
+		{
+			pos_flag=0;
+			pos_direction=1;
+			swing_delay_count=0;
+			GV.SwingMotor.Position_immediately1_Lag2=1;
+			GV.Tar_Position_angle=90;
+			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		}
+
 
 	}
 	else if(P_MK32->CH0_RY_H<-500)
 	{
-		GV.SwingMotor.Position_immediately1_Lag2=1;
-		GV.Tar_Position_angle=-90;
-		GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		swing_delay_count++;
+		if(swing_delay_count>com_delay)
+		{
+			pos_flag=0;
+			pos_direction=2;
+			swing_delay_count=0;
+			GV.SwingMotor.Position_immediately1_Lag2=1;
+			GV.Tar_Position_angle=-90;
+			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		}
 	}
 	else
 	{
 		Move_Swing_Halt_Func_Do();
+		swing_delay_count=0;
+		pos_direction=0;
 	}
 }
 
@@ -72,6 +94,7 @@ void Move_Swing_Right_Func_Do_lay(void)
 void Move_Swing_Left_Func_Do_imm(void)
 {
 
+	//pos_flag=0;
 	GV.SwingMotor.Position_immediately1_Lag2=1;
 
 	GV.Tar_Position_angle=left_compare_value;
@@ -84,16 +107,59 @@ void Move_Swing_Left_Func_Do_imm(void)
 void Move_Swing_Right_Func_Do_imm(void)
 {
 
+	//pos_flag=0;
 	GV.SwingMotor.Position_immediately1_Lag2=1;
 	GV.Tar_Position_angle=right_compare_value;
 	GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
 
 }
 
+int32_t Position_angle;
+
 void Move_Swing_Halt_Func_Do(void)
 {
 	GV.SwingMotor.Position_immediately1_Lag2=1;
-	GV.SwingMotor.Tar_Position_count=0;
-	GV.Tar_Position_Velcity_Degree_S=0;
+	if(pos_flag==0)
+	{
+		pos_flag=1;
+		if(pos_direction==1)     //顺时针
+		{
+			//延几度再停
+			Position_angle= -(float)GV.SwingMotor.Real_Position/11014
+					+GV.Robot_Swing_Speed*0.5;
+			if(Position_angle>90)
+			{
+				Position_angle=90;
+			}
+			GV.Tar_Position_angle=Position_angle;
+			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		}
+		else if(pos_direction==2)   //逆时针
+		{
+			//延几度再停
+			Position_angle= -(float)GV.SwingMotor.Real_Position/11014
+					-GV.Robot_Swing_Speed*0.5;
+			if(Position_angle<-90)
+			{
+				Position_angle=-90;
+			}
+			GV.Tar_Position_angle=Position_angle;
+			GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
+		}
+		else
+		{
+			GV.Tar_Position_Velcity_Degree_S=0;
+		}
+
+    }
+	else
+	{
+		//GV.Tar_Position_Velcity_Degree_S=0;
+	}
+
+
+
+	//GV.SwingMotor.Tar_Position_count=0;
+    //GV.Tar_Position_Velcity_Degree_S=0;
 
 }
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 6b5f02d..95bdf1f 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -171,6 +171,7 @@ int main(void)
   GF_Robot_Init();
   GV.Robot_Move_Speed = 1;
   TCP_Client_Init();
+  DevMon_Init();
 
 //	GV.Robot_Move_Speed = 1000;
 //	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"ComError_Ultrasonic_Sensor",0,ComError_Ultrasonic_Sensor);