robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

写好

Browse Source
This commit is contained in:
Robofish 2025-09-02 20:25:52 +08:00
parent 6bb0e96aa8
commit b7876f7eab
7 changed files with 207 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
User/component/kinematics.c
View File

@ -18,14 +18,55 @@
* @return * @return
*/ */
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) { 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 L1 = param->thigh_length;
float L2 = param->calf_length; float L2 = param->calf_length;
float q1 = *hip_angle; float q1 = *hip_angle; // 髋关节角度
float q2 = *knee_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 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; // 成功 return 0; // 成功
} }
/*串联腿单腿转摆动杆逆运动学*/ /*串联腿单腿转摆动杆逆运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/ /*大腿小腿均按照水平为0点，向下为正方向*/
/** /**
@ -38,14 +79,35 @@ int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_ang
* @return * @return
*/ */
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) { 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 L1 = param->thigh_length;
float L2 = param->calf_length; float L2 = param->calf_length;
float q = *angle; float q = *angle; // 摆动杆角度
float h = *height; float h = *height; // 摆动杆长度
if(h > (L1 + L2) || h < fabs(L1 - L2)) {
return -1; // 无解 // 由正解可知：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; // 成功 return 0; // 成功
} }

14
User/device/motor.c
View File

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

64
User/device/motor_lk.c
View File

@ -76,11 +76,7 @@ static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
} }
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { 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) { if (msg->data[0] != LK_CMD_FEEDBACK) {
// 如果不是标准反馈命令，可能是其他格式的数据 // 如果不是标准反馈命令，可能是其他格式的数据
@ -90,32 +86,32 @@ static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 解析温度 (DATA[1]) // 解析温度 (DATA[1])
motor->motor.feedback.temp = (int8_t)msg->data[1]; motor->motor.feedback.temp = (int8_t)msg->data[1];
// 解析转矩电流值或功率值 (DATA[2], DATA[3]) // 解析转矩电流值或功率值 (DATA[2], DATA[3])
int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
// 根据电机类型解析电流或功率 // 根据电机类型解析电流或功率
switch (motor->param.module) { switch (motor->param.module) {
case MOTOR_LK_MF9025: case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035: case MOTOR_LK_MF9035:
// MF/MG电机：转矩电流值
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
break; break;
default: default:
// MS电机：功率值（范围-1000~1000） motor->motor.feedback.torque_current = (float)raw_current_or_power;
motor->motor.feedback.torque_current = (float)raw_current_or_power; // 将功率存储在torque_current字段中
break; break;
} }
// 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB // 解析转速 (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]) // 解析编码器值 (DATA[6], DATA[7])
uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
// 将编码器值转换为弧度 (0 ~ 2π) // 将编码器值转换为弧度 (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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
@ -211,35 +207,37 @@ int8_t MOTOR_LK_UpdateAll(void) {
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL; if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV; if (manager == NULL) return DEVICE_ERR_NO_DEV;
// 限制输出值范围 // 限制输出值范围
if (value > 1.0f) value = 1.0f; if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f; if (value < -1.0f) value = -1.0f;
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV; if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据反转参数调整输出
float output = param->reverse ? -value : value;
// 转矩闭环控制命令 - 将输出值转换为转矩控制值 // 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE); int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
// 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID） // 构建CAN帧
BSP_CAN_StdDataFrame_t tx_frame; BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id; tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE; tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 设置转矩闭环控制命令数据 tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
tx_frame.data[0] = LK_CMD_TORQUE_CTRL; // 命令字节 tx_frame.data[1] = 0x00;
tx_frame.data[1] = 0x00; // NULL tx_frame.data[2] = 0x00;
tx_frame.data[2] = 0x00; // NULL tx_frame.data[3] = 0x00;
tx_frame.data[3] = 0x00; // NULL tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); // 转矩电流控制值低字节 tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节 tx_frame.data[6] = 0x00;
tx_frame.data[6] = 0x00; // NULL tx_frame.data[7] = 0x00;
tx_frame.data[7] = 0x00; // NULL
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
} }

4
User/device/motor_lz.c
View File

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

132
User/module/balance_chassis.c
View File

@ -1,6 +1,8 @@
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "bsp/time.h" #include "bsp/time.h"
#include "bsp/can.h" #include "bsp/can.h"
#include "component/user_math.h"
#include "component/kinematics.h"
#include <math.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 (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */ 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.left_wheel);
PID_Reset(&c->pid.right_wheel); PID_Reset(&c->pid.right_wheel);
PID_Reset(&c->pid.follow); PID_Reset(&c->pid.follow);
PID_Reset(&c->pid.balance); PID_Reset(&c->pid.balance);
c->mode = mode; c->mode = mode;
c->state = 0; c->state = 0; // 重置状态，确保每次切换模式时都重新初始化
return CHASSIS_OK; 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.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); 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; 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_LZ_Relax(&c->param->joint_motors[3]);
MOTOR_LK_Relax(&c->param->wheel_motors[0]); MOTOR_LK_Relax(&c->param->wheel_motors[0]);
MOTOR_LK_Relax(&c->param->wheel_motors[1]); 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; break;
case CHASSIS_MODE_RECOVER: case CHASSIS_MODE_RECOVER:
@ -138,57 +169,62 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
break; break;
case CHASSIS_MODE_WHELL_BALANCE: case CHASSIS_MODE_WHELL_BALANCE:
{ {
// 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆 // 定义腿部运动学参数（假设单位为mm转换为m）
// 锁定关节电机到固定位置（比如直立状态） KIN_SerialLeg_Param_t left_leg_param = {
// for (int i = 0; i < 4; i++) { .thigh_length = 0.215f,
// MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s .calf_length = 0.258f
// } };
KIN_SerialLeg_Param_t right_leg_param = {
.thigh_length = 0.215f,
.calf_length = 0.258f
};
float angle = 1.2f;
float height = 0.16f;
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作为目标位置
// 获取IMU pitch角度和角速度作为平衡反馈 if (c->state == 0) {
float pitch_angle = c->feedback.imu.euler.pit; // 首次进入模式时，将当前位置设为目标位置
float pitch_rate = c->feedback.imu.gyro.y; // pitch角速度 c->setpoint.left_wheel = left_wheel_angle;
c->setpoint.right_wheel = right_wheel_angle;
c->state = 1;
}
// 平衡PID计算 - 以直立为目标（0度） float left_wheel_output = PID_Calc(&c->pid.left_wheel, c->setpoint.left_wheel, left_wheel_angle, 0.0f, c->dt);
float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt); float right_wheel_output = PID_Calc(&c->pid.right_wheel, c->setpoint.right_wheel, right_wheel_angle, 0.0f, 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)); left_wheel_output = AbsClip(left_wheel_output, 1.0f);
right_output = fmaxf(-1.0f, fminf(1.0f, right_output)); 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);
// 输出到电机
MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output);
MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output);
break; break;
} }
case CHASSIS_MODE_WHELL_LEG_BALANCE: case CHASSIS_MODE_WHELL_LEG_BALANCE:
// 轮子+腿平衡模式（暂时留空，后续实现） // 轮子+腿平衡模式（暂时留空，后续实现）
break; break;

8
User/module/balance_chassis.h
View File

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

20
User/module/config.c
View File

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