写好

User/component/kinematics.c

@@ -18,14 +18,55 @@
  * @return 
  */
 int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
+    if (param == NULL || hip_angle == NULL || knee_angle == NULL || angle == NULL || height == NULL) {
+        return -1; // 参数错误
+    }
+    
     float L1 = param->thigh_length;
     float L2 = param->calf_length;
-    float q1 = *hip_angle;
-    float q2 = *knee_angle;
-    *height = sqrt(L1*L1 + L2*L2 - 2*L1*L2*cos(q1+q2));
-    *angle = acos((L1*L1 + (*height)*(*height) - L2*L2)/(2*L1*(*height))) + q1;
+    float q1 = *hip_angle;      // 髋关节角度
+    float q2 = *knee_angle;     // 膝关节角度
+    
+    float q4 = (M_PI - q1 - q2)/2;
+
+    // 使用余弦定理求解等效摆动杆长度
+    // L2*L2 = L1*L1 + L3*L3 - 2*L1*L3*cosf(q4);
+    // 整理得：L3*L3 - 2*L1*cosf(q4)*L3 + (L1*L1 - L2*L2) = 0
+    
+    float a = 1.0f;
+    float b = -2.0f * L1 * cosf(q4);
+    float c = L1*L1 - L2*L2;
+    
+    float discriminant = b*b - 4*a*c;
+    
+    // 检查判别式，确保有实数解
+    if (discriminant < 0) {
+        return -2; // 无实数解，配置不可达
+    }
+    
+    float sqrt_discriminant = sqrtf(discriminant);
+    float L3_1 = (-b + sqrt_discriminant) / (2*a);
+    float L3_2 = (-b - sqrt_discriminant) / (2*a);
+    
+    // 选择正的解（物理意义上的长度）
+    float L3;
+    if (L3_1 > 0 && L3_2 > 0) {
+        // 两个正解，选择较小的（通常对应机构的正常工作姿态）
+        L3 = (L3_1 < L3_2) ? L3_1 : L3_2;
+    } else if (L3_1 > 0) {
+        L3 = L3_1;
+    } else if (L3_2 > 0) {
+        L3 = L3_2;
+    } else {
+        return -3; // 无正解
+    }
+
+    *angle = q1 + q4;
+    *height = L3;
+
     return 0; // 成功
 }
+
 /*串联腿单腿转摆动杆逆运动学*/
 /*大腿小腿均按照水平为0点，向下为正方向*/
 /**
@@ -38,14 +79,35 @@ int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_ang
  * @return 
  */
 int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
+    if (param == NULL || angle == NULL || height == NULL || hip_angle == NULL || knee_angle == NULL) {
+        return -1; // 参数错误
+    }
     float L1 = param->thigh_length;
     float L2 = param->calf_length;
-    float q = *angle;
-    float h = *height;
-    if(h > (L1 + L2) || h < fabs(L1 - L2)) {
-        return -1; // 无解
+    float q = *angle;  // 摆动杆角度
+    float h = *height; // 摆动杆长度
+
+    // 由正解可知：q = q1 + q4，h = L3
+    // 由余弦定理：L2^2 = L1^2 + h^2 - 2*L1*h*cos(q4)
+    // 整理得：cos(q4) = (L1^2 + h^2 - L2^2) / (2*L1*h)
+    float cos_q4 = (L1*L1 + h*h - L2*L2) / (2.0f * L1 * h);
+
+    // 检查 cos_q4 是否在 [-1, 1] 范围内
+    if (cos_q4 < -1.0f || cos_q4 > 1.0f) {
+        return -2; // 不可达
     }
-    *hip_angle = acos((L1*L1 + h*h - L2*L2)/(2*L1*h)) + q;
-    *knee_angle = acos((L1*L1 + L2*L2 - h*h)/(2*L1*L2)) - M_PI;
+
+    float q4 = acosf(cos_q4);
+
+    // 由正解：q = q1 + q4，且 q4 = (PI - q1 - q2)/2
+    // 整理得：q1 = q - q4
+    float q1 = q - q4;
+
+    // 再由 q4 = (PI - q1 - q2)/2，整理得：q2 = PI - 2*q4 - q1
+    float q2 = M_PI - 2.0f * q4 - q1;
+
+    *hip_angle = q1;
+    *knee_angle = q2;
+
     return 0; // 成功
 }

User/device/motor.c

@@ -18,29 +18,19 @@
 /* Exported functions ------------------------------------------------------- */
 float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
   if (motor == NULL) return DEVICE_ERR_NULL;
-  if (motor->reverse) {
-    return -motor->feedback.rotor_abs_angle;
-  }else{
     return motor->feedback.rotor_abs_angle;
 }
-}
 
 float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
   if (motor == NULL) return DEVICE_ERR_NULL;
-  if (motor->reverse) {
-    return -motor->feedback.rotor_speed;
-  }else{
     return motor->feedback.rotor_speed;
-  }
+  
 }
 
 float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
   if (motor == NULL) return DEVICE_ERR_NULL;
-  if (motor->reverse) {
-    return -motor->feedback.torque_current;
-  }else{
     return motor->feedback.torque_current;
-  }
+  
 }
 
 float MOTOR_GetTemp(const MOTOR_t *motor) {

User/device/motor_lk.c

@@ -76,10 +76,6 @@ static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
 }
 
 static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
-    // 调试信息：打印接收到的数据
-    // printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n", 
-    //        motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2], 
-    //        msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
 
     // 检查命令字节是否为反馈命令
     if (msg->data[0] != LK_CMD_FEEDBACK) {
@@ -98,24 +94,24 @@ static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
     switch (motor->param.module) {
         case MOTOR_LK_MF9025:
         case MOTOR_LK_MF9035:
-            // MF/MG电机：转矩电流值
             motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
             break;
         default:
-            // MS电机：功率值（范围-1000~1000）
-            motor->motor.feedback.torque_current = (float)raw_current_or_power;  // 将功率存储在torque_current字段中
+            motor->motor.feedback.torque_current = (float)raw_current_or_power;
             break;
     }
 
     // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
-    motor->motor.feedback.rotor_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
 
     // 解析编码器值 (DATA[6], DATA[7])
     uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
     uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
 
     // 将编码器值转换为弧度 (0 ~ 2π)
-    motor->motor.feedback.rotor_abs_angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+    float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+    motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
 }
 
 /* Exported functions ------------------------------------------------------- */
@@ -222,23 +218,25 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
     MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
     if (motor == NULL) return DEVICE_ERR_NO_DEV;
 
-    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
-    int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE);
+    // 根据反转参数调整输出
+    float output = param->reverse ? -value : value;
 
-    // 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
+    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
+    int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
+
+    // 构建CAN帧
     BSP_CAN_StdDataFrame_t tx_frame;
     tx_frame.id = param->id;
     tx_frame.dlc = MOTOR_TX_BUF_SIZE;
 
-    // 设置转矩闭环控制命令数据
-    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;  // 命令字节
-    tx_frame.data[1] = 0x00;                // NULL
-    tx_frame.data[2] = 0x00;                // NULL
-    tx_frame.data[3] = 0x00;                // NULL
-    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);        // 转矩电流控制值低字节
-    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节
-    tx_frame.data[6] = 0x00;                // NULL
-    tx_frame.data[7] = 0x00;                // NULL
+    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
+    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;
 
     return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }

User/device/motor_lz.c

@@ -27,7 +27,7 @@
 #define LZ_RAW_VALUE_MAX            (65535)         /* 16位原始值最大值 */
 #define LZ_TEMP_SCALE               (10.0f)         /* 温度缩放因子 */
 
-#define LZ_MAX_RECOVER_DIFF_RAD    (0.25f)
+#define LZ_MAX_RECOVER_DIFF_RAD    (0.28f)
 #define MOTOR_TX_BUF_SIZE           (8)
 #define MOTOR_RX_BUF_SIZE           (8)
 
@@ -36,7 +36,7 @@
 MOTOR_LZ_MotionParam_t lz_recover_param = {
     .target_angle = 0.0f,
     .target_velocity = 0.0f,
-    .kp = 15.0f,
+    .kp = 30.0f,
     .kd = 1.0f,
     .torque = 0.0f,
 };

User/module/balance_chassis.c

@@ -1,6 +1,8 @@
 #include "module/balance_chassis.h"
 #include "bsp/time.h"
 #include "bsp/can.h"
+#include "component/user_math.h"
+#include "component/kinematics.h"
 #include <math.h>
 
 
@@ -8,12 +10,19 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
   if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
 
+  // 如果从轮子平衡模式切换出去，恢复PID参数的原始range设置
+  if (c->mode == CHASSIS_MODE_WHELL_BALANCE) {
+    ((KPID_Params_t*)c->pid.left_wheel.param)->range = 0.0f;
+    ((KPID_Params_t*)c->pid.right_wheel.param)->range = 0.0f;
+  }
+
   PID_Reset(&c->pid.left_wheel);
   PID_Reset(&c->pid.right_wheel);
   PID_Reset(&c->pid.follow);
   PID_Reset(&c->pid.balance);
+  
   c->mode = mode;
-    c->state = 0;
+  c->state = 0;  // 重置状态，确保每次切换模式时都重新初始化
 
   return CHASSIS_OK;
 }
@@ -42,6 +51,10 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
     PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->follow_pid_param);
     PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, &param->motor_pid_param);
 
+    // 初始化设定点
+    c->setpoint.left_wheel = 0.0f;
+    c->setpoint.right_wheel = 0.0f;
+
     return CHASSIS_OK;
 }
 
@@ -111,6 +124,24 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
             MOTOR_LZ_Relax(&c->param->joint_motors[3]);
             MOTOR_LK_Relax(&c->param->wheel_motors[0]);
             MOTOR_LK_Relax(&c->param->wheel_motors[1]);
+            // 定义腿部运动学参数（假设单位为mm转换为m）
+            KIN_SerialLeg_Param_t left_leg_param = {
+                .thigh_length = 0.215f, 
+                .calf_length = 0.258f    
+            };
+            KIN_SerialLeg_Param_t right_leg_param = {
+                .thigh_length = 0.215f, 
+                .calf_length = 0.258f    
+            };
+            for (int i = 0; i < 4; i++) {
+                MOTOR_LZ_RecoverToZero(&c->param->joint_motors[i]);
+            }
+            
+            KIN_SerialLeg_FK(&left_leg_param, 
+                                        &c->feedback.joint[0].rotor_abs_angle, 
+                                        &c->feedback.joint[1].rotor_abs_angle, 
+                                        &c->angle, 
+                                        &c->height);
             break;
 
         case CHASSIS_MODE_RECOVER:
@@ -139,56 +170,61 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
         
         case CHASSIS_MODE_WHELL_BALANCE:
         {   
-            // 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆
-            // 锁定关节电机到固定位置（比如直立状态）
-            // for (int i = 0; i < 4; i++) {
-            //     MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s
-            // }
+            // 定义腿部运动学参数（假设单位为mm转换为m）
+            KIN_SerialLeg_Param_t left_leg_param = {
+                .thigh_length = 0.215f, 
+                .calf_length = 0.258f    
+            };
+            KIN_SerialLeg_Param_t right_leg_param = {
+                .thigh_length = 0.215f, 
+                .calf_length = 0.258f    
+            };
 
-            // 双轮平衡控制
-            // 获取IMU pitch角度和角速度作为平衡反馈
-            float pitch_angle = c->feedback.imu.euler.pit;
-            float pitch_rate = c->feedback.imu.gyro.y;      // pitch角速度
+            float angle = 1.2f;
+            float height = 0.16f;
 
-            // 平衡PID计算 - 以直立为目标（0度）
-            float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt);
-            
-            // 前进后退控制（基于控制指令）
-            float forward_cmd = c_cmd->move_vec.vx;  // 前进速度指令
-            
-            // 转向控制（基于控制指令）
-            float turn_cmd = c_cmd->move_vec.wz;     // 转向速度指令
-            
-            // 计算左右轮速度设定值
-            c->setpoint.left_wheel = balance_output + forward_cmd - turn_cmd * 0.5f;
-            c->setpoint.right_wheel = balance_output + forward_cmd + turn_cmd * 0.5f;
-            
-            // 左轮速度PID控制
-            float left_wheel_speed = c->feedback.wheel[0].rotor_speed;
-            float left_output = PID_Calc(&c->pid.left_wheel, 
-                                       c->setpoint.left_wheel, 
-                                       left_wheel_speed, 
-                                       0.0f, 
-                                       c->dt);
-            
-            // 右轮速度PID控制
-            float right_wheel_speed = c->feedback.wheel[1].rotor_speed;
-            float right_output = PID_Calc(&c->pid.right_wheel, 
-                                        c->setpoint.right_wheel, 
-                                        right_wheel_speed, 
-                                        0.0f, 
-                                        c->dt);
-            
-            // 限制输出范围
-            left_output = fmaxf(-1.0f, fminf(1.0f, left_output));
-            right_output = fmaxf(-1.0f, fminf(1.0f, right_output));
-            
-            // 输出到电机
-            MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output);
-            MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output);
-            break;
+            for (int i = 0; i < 4; i++) {
+                c->output.joint[i].torque = 0.0f;
+                c->output.joint[i].target_velocity = 0.0f;
+                c->output.joint[i].kp = 50.0f;
+                c->output.joint[i].kd = 1.0f;
             }
 
+            KIN_SerialLeg_IK(&left_leg_param, &angle, &height, 
+                                        &c->output.joint[0].target_angle, 
+                                        &c->output.joint[1].target_angle);
+            c->output.joint[2].target_angle = c->output.joint[1].target_angle;
+            c->output.joint[3].target_angle = c->output.joint[0].target_angle;
+
+            for (int i = 0; i < 4; i++) {
+                MOTOR_LZ_MotionControl(&c->param->joint_motors[i], &c->output.joint[i]);
+            }
+
+            // 轮子控制：保持位置或进行平衡控制
+            float left_wheel_angle = c->feedback.wheel[0].rotor_abs_angle;
+            float right_wheel_angle = c->feedback.wheel[1].rotor_abs_angle;
+            
+            // 使用setpoint作为目标位置
+            if (c->state == 0) {
+                // 首次进入模式时，将当前位置设为目标位置
+                c->setpoint.left_wheel = left_wheel_angle;
+                c->setpoint.right_wheel = right_wheel_angle;
+                c->state = 1;
+            }
+            
+            float left_wheel_output = PID_Calc(&c->pid.left_wheel, c->setpoint.left_wheel, left_wheel_angle, 0.0f, c->dt);
+            float right_wheel_output = PID_Calc(&c->pid.right_wheel, c->setpoint.right_wheel, right_wheel_angle, 0.0f, c->dt);
+            
+            // 限制输出范围
+            left_wheel_output = AbsClip(left_wheel_output, 1.0f);
+            right_wheel_output = AbsClip(right_wheel_output, 1.0f);
+            
+            // 输出到轮毂电机
+            MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_wheel_output);
+            MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_wheel_output);
+            
+            break;
+        }        
         case CHASSIS_MODE_WHELL_LEG_BALANCE:
             // 轮子+腿平衡模式（暂时留空，后续实现）
             break;

User/module/balance_chassis.h

@@ -60,8 +60,8 @@ typedef struct {
 }Chassis_Feedback_t;
 
 typedef struct {
-  MOTOR_MIT_Output_t joint[4]; /* 四个关节电机输出 */
-  MOTOR_MIT_Output_t wheel[2]; /* 两个轮子电机输出 */
+  MOTOR_LZ_MotionParam_t joint[4]; /* 四个关节电机输出 */
+  float wheel[2]; /* 两个轮子电机输出 */
 }Chassis_Output_t;
 
 
@@ -102,6 +102,10 @@ typedef struct {
   Chassis_Output_t output;
 
   int8_t state;
+
+  float angle;
+  float height;
+
   float wz_multi; /* 小陀螺模式旋转方向 */
 
   /* PID计算的目标值 */

User/module/config.c

@@ -78,14 +78,14 @@ Config_RobotParam_t robot_config = {
             .range = 0.0f,
         },
         .motor_pid_param = {
-            .k = 1.0f,
-            .p = 0.5f,
-            .i = 0.0f,
-            .d = 0.0f,
-            .i_limit = 0.0f,
-            .out_limit = 12.0f,
-            .d_cutoff_freq = 30.0f,
-            .range = 0.0f,
+            .k = 0.1f,
+            .p = 10.0f,
+            .i = 0.2f,
+            .d = 0.2f,
+            .i_limit = 0.3f,
+            .out_limit = -1.0f,
+            .d_cutoff_freq = 0.0f,
+            .range = M_2PI,
         },
         .low_pass_cutoff_freq = {
             .in = 30.0f,
@@ -105,7 +105,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 125,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = false,
+                .reverse = true,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
             { // 右膝关节
@@ -121,7 +121,7 @@ Config_RobotParam_t robot_config = {
                 .motor_id = 127,
                 .host_id = 130,
                 .module = MOTOR_LZ_RSO3,
-                .reverse = false,
+                .reverse = true,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
         },