站不起来了草

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -165,6 +165,11 @@
           <WinNumber>1</WinNumber>
           <ItemText>shoot</ItemText>
         </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>dr16</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>

User/component/vmc.c

@@ -80,7 +80,7 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
     if (vmc == NULL || !vmc->initialized) {
         return -1;
     }
-    body_pitch = -body_pitch; // 机体俯仰角取反
+    // body_pitch = body_pitch; // 机体俯仰角取反
     
     VMC_Leg_t *leg = &vmc->leg;
     

User/component/vmc.h

@@ -8,7 +8,6 @@ extern "C" {
 #include <stdint.h>
 #include <stdbool.h>
 #include <math.h>
-
 /* Exported types ----------------------------------------------------------- */
 
 /**

User/module/balance_chassis.c

@@ -3,6 +3,7 @@
 #include "bsp/time.h"
 #include "component/filter.h"
 #include "component/user_math.h"
+#include "device/motor_lk.h"
 #include "device/motor_lz.h"
 #include <math.h>
 #include <stddef.h>
@@ -248,6 +249,15 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
         }
     }
 
+    for (int i = 0; i < 4; i++) {
+        // 机械零点调整
+        if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
+            c->feedback.joint[i].rotor_abs_angle -= M_2PI;
+        }
+        c->feedback.joint[i].rotor_abs_angle =
+            -c->feedback.joint[i].rotor_abs_angle; // 机械零点调整
+    }
+    
   // for (int i = 0; i < 4; i++) {
   //   c->feedback.joint[i] = virtual_chassis.data.joint[i];
   //   if (c->feedback.joint[i].rotor_abs_angle > M_PI) {
@@ -394,6 +404,7 @@ void Chassis_Output(Chassis_t *c) {
   // float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
   //                 c->output.joint[3]};
   // Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
+
   MOTOR_LZ_MotionParam_t motion_param = {
     .torque=0.0f,
     .target_angle=0.0f,
@@ -402,12 +413,15 @@ void Chassis_Output(Chassis_t *c) {
     .kd=0.0f,
   };
   for (int i = 0; i < 4; i++) {
+    motion_param.torque = c->output.joint[i];
     MOTOR_LZ_MotionControl(&c->param->joint_param[i],&motion_param);
   }
   c->output.wheel[0] =
       LowPassFilter2p_Apply(&c->filter.wheel_out[0], c->output.wheel[0]);
   c->output.wheel[1] =
       LowPassFilter2p_Apply(&c->filter.wheel_out[1], c->output.wheel[1]);
+  MOTOR_LK_SetOutput(&c->param->wheel_param[0], c->output.wheel[0]);
+  MOTOR_LK_SetOutput(&c->param->wheel_param[1], c->output.wheel[1]);
   // Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
                                   //  c->output.wheel[1]);
   // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f,

User/module/config.c

@@ -98,7 +98,7 @@ Config_RobotParam_t robot_config = {
 
     .chassis_param = {
         .yaw={
-            .k=1.0f,
+            .k=0.0f,
             .p=1.0f,
             .i=0.0f,
             .d=0.3f,
@@ -109,7 +109,7 @@ Config_RobotParam_t robot_config = {
         },
 
         .roll={
-            .k=1.0f,
+            .k=0.2f,
             .p=0.5f,           /* 横滚角比例系数 */
             .i=0.01f,           /* 横滚角积分系数 */
             .d=0.01f,           /* 横滚角微分系数 */
@@ -203,13 +203,13 @@ Config_RobotParam_t robot_config = {
         .wheel_param = {
             { // 左轮电机
                 .can = BSP_CAN_3,
-                .id = 0x141,
+                .id = 0x142,
                 .module = MOTOR_LK_MF9025,
                 .reverse = false,
             },
             { // 右轮电机
                 .can = BSP_CAN_3,
-                .id = 0x142,
+                .id = 0x141,
                 .module = MOTOR_LK_MF9025,
                 .reverse = true,
             },

User/task/atti_esit.c

@@ -82,9 +82,18 @@ void Task_atti_esit(void *argument) {
 
     /* 根据解析出来的四元数计算欧拉角 */
     AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
-    imu_for_chassis.accl = bmi088.accl;
+    /* 加速度计数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
-    imu_for_chassis.gyro = bmi088.gyro;
+    imu_for_chassis.accl.x = bmi088.accl.y;
-    imu_for_chassis.euler = eulr_to_send;
+    imu_for_chassis.accl.y = -bmi088.accl.x;
+    imu_for_chassis.accl.z = bmi088.accl.z;
+    /* 陀螺仪数据绕z轴旋转270度: (x,y,z) -> (y,-x,z) */
+    imu_for_chassis.gyro.x = bmi088.gyro.y;
+    imu_for_chassis.gyro.y = -bmi088.gyro.x;
+    imu_for_chassis.gyro.z = bmi088.gyro.z;
+    /* 欧拉角绕z轴旋转270度 */
+    imu_for_chassis.euler.rol = eulr_to_send.pit;
+    imu_for_chassis.euler.pit = -eulr_to_send.rol;
+    imu_for_chassis.euler.yaw = eulr_to_send.yaw - 1.57079632679f; // -90度
     osMessageQueueReset(
         task_runtime.msgq.chassis.imu); // 重置消息队列，防止阻塞
     osMessageQueuePut(task_runtime.msgq.chassis.imu, &imu_for_chassis, 0,

User/task/rc.c

@@ -105,13 +105,14 @@ switch (dr16.data.sw_l) {
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
       break;
     case DR16_SW_MID:
-      // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
+      cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
       break;
     case DR16_SW_DOWN:
+    // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
       // cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-      cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
+      // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
       break;
     default:
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;