From 85075c7c07ce57efed6baaf181a779dc3022f65b Mon Sep 17 00:00:00 2001
From: L1ng <429616286@qq.com>
Date: Thu, 21 May 2026 09:25:46 +0800
Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E4=BA=86=E6=9C=89=E5=85=B3CV?=
 =?UTF-8?q?=E7=9A=84=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Core/BASE/Src/MSP/msp_TL720D.c        |  3 +++
 Core/FSM/Src/Handset_Status_Setting.c | 12 +++++++++
 Core/FSM/Src/robot_move_actions.c     | 10 ++++++--
 Core/Src/main.c                       | 35 ++++++++++++++++++++++++---
 4 files changed, 54 insertions(+), 6 deletions(-)

diff --git a/Core/BASE/Src/MSP/msp_TL720D.c b/Core/BASE/Src/MSP/msp_TL720D.c
index 235fcf5..fbf3dca 100644
--- a/Core/BASE/Src/MSP/msp_TL720D.c
+++ b/Core/BASE/Src/MSP/msp_TL720D.c
@@ -33,9 +33,12 @@ void TL720D_intialize(struct UARTHandler *Handler)
 	//log_info("TL720D_intialize");
 	LOGFF(DL_ERROR,"TL720D_intialize");
 }
+
+int watch;
 void decode_TL720D(uint8_t *buffer, uint16_t length)
 {
 
+	watch=length;
 	if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00
 			&& buffer[3] == 0x84)
 	{
diff --git a/Core/FSM/Src/Handset_Status_Setting.c b/Core/FSM/Src/Handset_Status_Setting.c
index 62a0e0b..0dbb53f 100644
--- a/Core/FSM/Src/Handset_Status_Setting.c
+++ b/Core/FSM/Src/Handset_Status_Setting.c
@@ -53,6 +53,7 @@ static InputEvent GetVerticalLeftModeEvents(void);
 static InputEvent GetVerticalRightModeEvents(void);
 static InputEvent GetRegionalFlatTaskEvents(void);
 static InputEvent GetRegionalHorizontalTaskEvents(void);
+static InputEvent GetEmergencyModeEvents(void);
 // 辅助功能函数
 static InputEvent CheckCommonKeys(void);
 static InputEvent CheckCommonKeys_to_manual(void);
@@ -74,6 +75,7 @@ static const ModeEventHandler modeEventHandlers[MODE_COUNT] = {
     [Vertical_Mode_Right]             = GetVerticalRightModeEvents,
     [Regional_Horizontal_Automatic_Task] = GetRegionalHorizontalTaskEvents,
     [Regional_Flat_Automatic_Task]    = GetRegionalFlatTaskEvents,
+    [Emergency_Mode]                  = GetEmergencyModeEvents,
 };
 
 /*=========================== 函数实现 ===========================*/
@@ -350,6 +352,16 @@ static InputEvent GetRegionalFlatTaskEvents(void)
     return CalculateRockerEvent();
 }
 
+
+static InputEvent GetEmergencyModeEvents(void)
+{
+    InputEvent key = CheckEmergencyStop();
+    if (key != INPUT_NONE) return key;
+
+    key = CheckCommonKeys_to_manual();
+    return (key != INPUT_NONE) ? key : CalculateRockerEvent_manual();
+}
+
 /**
  * @brief  IO状态检测（升降控制）
  * @param  无
diff --git a/Core/FSM/Src/robot_move_actions.c b/Core/FSM/Src/robot_move_actions.c
index 22b4702..2999ed1 100644
--- a/Core/FSM/Src/robot_move_actions.c
+++ b/Core/FSM/Src/robot_move_actions.c
@@ -462,7 +462,7 @@ static void handleAttitudeJudge_vertical(void)
 //            GV.Robot_Angle_Desire -= 360.0f;
 //        }
 //    }
-    s_fsmState[s_activeFsm] = STATE_ATTITUDE_ADJUST;
+    s_fsmState[s_activeFsm] = STATE_WORK_WAY_OR_DIRECT_MOVE;//STATE_ATTITUDE_ADJUST;
 }
 
 
@@ -952,13 +952,18 @@ static void auto_drive_pid_weld(void)
 
 }
 
+float speed_w[2];
 float PID_factor=1;
 static void auto_drive_pid_vertical(void)
 {
     float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
     float speeds[2];
+    float Robot_Desired_Speed_w=GV.Robot_Desired_Speed*0.1;
 
-    TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, GV.Robot_Desired_Speed,PID_factor, speeds);
+    TwoWheel_AngleControl(robotRoll, GV.Robot_Angle_Desire, Robot_Desired_Speed_w,PID_factor, speeds);
+
+    speed_w[0]=speeds[0];
+    speed_w[1]=speeds[1];
 
     GV.Left_Speed_M_min  = factor_1*speeds[0];
     GV.Right_Speed_M_min = factor_2*speeds[1];   /* 根据电机方向可能需要调整符号 */
@@ -1150,6 +1155,7 @@ void flag_reset(void)
 
 void Emergency_Stop_Action(void)
 {
+
 	GV.GroundManagementValue.MaualControlPower=1;
 	GV.GroundManagementValue.MaualPowerState=0;
 
diff --git a/Core/Src/main.c b/Core/Src/main.c
index a554a07..4baf168 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -37,6 +37,7 @@
 #include "msp_ground_management.h"
 #include "msp_strain_gauge.h"
 #include <bsp_tempature.h>
+#include "B03_Para_01_100.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -72,6 +73,7 @@ int can2_sendListPeriod = 10;
 int can2_DispacherPeriod = 20;
 int SAVE_To_CV = 0;
 int dog_flag=0;
+//extern const B03_Para_t g_B03_param_table[100];
 /* USER CODE END PD */
 
 /* Private macro -------------------------------------------------------------*/
@@ -161,6 +163,8 @@ int main(void)
   MX_LPUART1_UART_Init();
   MX_ADC2_Init();
   MX_LWIP_Init();
+
+
   /* USER CODE BEGIN 2 */
 
 
@@ -190,14 +194,37 @@ int main(void)
 				//HAL_IWDG_Refresh(&hiwdg1);   // 喂狗
 			}
 
+
 			if (SAVE_To_CV == 1)
 			{
+				//将0号机器人的参数传入CV中
+				memcpy(&CV, &g_B03_param_table[0], sizeof(CV));
+				//恢复SAVE_To_CV值防止重复设置
 				SAVE_To_CV = 0;
-				//CV写入falsh�????????????????????
-				GF_BSP_EEPROM_Set_CV(CV);
-				CV = GF_BSP_EEPROM_Get_CV();
-				//FS_SetZero();
 			}
+
+			//CV写入EEPROM
+			else if (SAVE_To_CV == 2)
+			{
+			GF_BSP_EEPROM_Set_CV(CV);
+			SAVE_To_CV = 0;
+			}
+			//从EEPROM读取参数
+			else if (SAVE_To_CV == 3)
+			{
+			CV = GF_BSP_EEPROM_Get_CV();
+			SAVE_To_CV = 0;
+			}
+
+
+//			if (SAVE_To_CV == 1)
+//			{
+//				SAVE_To_CV = 0;
+//				//CV写入falsh�????????????????????
+//				GF_BSP_EEPROM_Set_CV(CV);
+//				CV = GF_BSP_EEPROM_Get_CV();
+//				//FS_SetZero();
+//			}
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */