From db9d9f7db58d8262c8a456b66ba5b378184715b9 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sun, 5 Oct 2025 14:13:49 +0800
Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E5=BA=95=E7=9B=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/device/virtual_chassis.c | 57 +++++++++++++++++++++--------------
 User/device/virtual_chassis.h |  2 +-
 User/module/balance_chassis.c |  6 ++--
 User/task/ctrl_chassis.c      |  2 +-
 4 files changed, 39 insertions(+), 28 deletions(-)

diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c
index b2e6f9a..4e9dade 100644
--- a/User/device/virtual_chassis.c
+++ b/User/device/virtual_chassis.c
@@ -372,31 +372,42 @@ int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques
 /**
  * @brief 发送轮子电机控制命令
  */
-int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]) {
-    if (chassis == NULL || left_cmd == NULL || right_cmd == NULL) {
+int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out) {
+    if (chassis == NULL) {
         return DEVICE_ERR_NULL;
     }
-    
-    // 发送左轮控制命令
-    BSP_CAN_StdDataFrame_t left_frame = {
-        .id = chassis->param.motors.wheel_left_id,
-        .dlc = 8,
-    };
-    memcpy(left_frame.data, left_cmd, 8);
-    memcpy(chassis->output.wheel_left_cmd, left_cmd, 8);
-    
-    // 发送右轮控制命令
-    BSP_CAN_StdDataFrame_t right_frame = {
-        .id = chassis->param.motors.wheel_right_id,
-        .dlc = 8,
-    };
-    memcpy(right_frame.data, right_cmd, 8);
-    memcpy(chassis->output.wheel_right_cmd, right_cmd, 8);
-    
-    int8_t result1 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &left_frame);
-    int8_t result2 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &right_frame);
-    
-    return (result1 == BSP_OK && result2 == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+    int16_t torque_control = (int16_t)(left_out * 2048);
+        // 构建CAN帧
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = chassis->param.motors.wheel_left_id;
+    tx_frame.dlc = 8;
+
+    tx_frame.data[0] = 0xA1;
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
+        return DEVICE_ERR;
+    }
+    torque_control = (int16_t)(right_out * 2048);
+        // 构建CAN帧
+    tx_frame.id = chassis->param.motors.wheel_right_id;
+    tx_frame.dlc = 8;
+    tx_frame.data[0] = 0xA1;
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
+        return DEVICE_ERR;
+    }
 }
 
 
diff --git a/User/device/virtual_chassis.h b/User/device/virtual_chassis.h
index 51d8c25..fea5d28 100644
--- a/User/device/virtual_chassis.h
+++ b/User/device/virtual_chassis.h
@@ -111,7 +111,7 @@ int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques
  * @param right_cmd 右轮控制命令数据（8字节）
  * @return DEVICE_OK 成功，其他值失败
  */
-int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]);
+int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out);
 
 #ifdef __cplusplus
 }
diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index e149b3b..e4645de 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -35,9 +35,7 @@ static int8_t Chassis_MotorRelax(Chassis_t *c) {
     return CHASSIS_ERR_NULL;
   float relax_torque[4] = {0.0f, 0.0f, 0.0f, 0.0f};
   Virtual_Chassis_SendJointTorque(&virtual_chassis, relax_torque);
-  // for (int i = 0; i < 2; i++) {
-  //   MOTOR_LK_Relax(&c->param->wheel_motors[i]);
-  // }
+  Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.1f, 0.1f);
   return CHASSIS_OK;
 }
 
@@ -106,6 +104,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   }
   c->param = param;
 
+  c->mode = CHASSIS_MODE_RELAX;
+
   /*初始化can*/
   BSP_CAN_Init();
 
diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c
index d18cc04..4645b53 100644
--- a/User/task/ctrl_chassis.c
+++ b/User/task/ctrl_chassis.c
@@ -22,7 +22,7 @@
 /* USER STRUCT BEGIN */
 Chassis_t chassis;
 Chassis_CMD_t chassis_cmd = {
-    .mode = CHASSIS_MODE_RECOVER,  // 改为RECOVER模式，让电机先启动
+    .mode = CHASSIS_MODE_RELAX,  // 改为RECOVER模式，让电机先启动
     .move_vec = {
         .vx = 0.0f,
         .vy = 0.0f,