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

平衡控制有了，但是一坨狗屎

Browse Source
This commit is contained in:
Robofish 2025-06-29 11:43:14 +08:00
parent 07fd423f20
commit 318287223c
30 changed files with 5594 additions and 4817 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@ -8,6 +8,8 @@
"kinematics.h": "c",
"bezier_curve.h": "c",
"queue": "c",
"stack": "c"
"stack": "c",
"buzzer.h": "c",
"user_task.h": "c"
}
}

18
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -158,32 +158,22 @@
<Ww>
<count>0</count>
<WinNumber>1</WinNumber>
<ItemText>dr16</ItemText>
<ItemText>chassis</ItemText>
</Ww>
<Ww>
<count>1</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
<ItemText>go_feedback</ItemText>
</Ww>
<Ww>
<count>2</count>
<WinNumber>1</WinNumber>
<ItemText>n100</ItemText>
<ItemText>go</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>go_feedback</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>param_default</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>chassis_cmd</ItemText>
<ItemText>n100_cali</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

9351
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

20
User/bsp/buzzer.c
View File

@ -3,6 +3,7 @@
#include <main.h>
#include <tim.h>
#include "bsp/delay.h"
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
@ -10,6 +11,9 @@
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Buzzer_Start(void) {
if (HAL_TIM_PWM_Start(&htim12, TIM_CHANNEL_2) == HAL_OK) return BSP_OK;
return BSP_ERR;
@ -30,6 +34,22 @@ int8_t BSP_Buzzer_Set(float freq, float duty_cycle) {
return BSP_OK;
}
int8_t BSP_Buzzer_Set_Note(BSP_Buzzer_Note_t note, float delay_sec) {
if (note <= 0 || delay_sec < 0.0f) return BSP_ERR;
uint32_t timer_clk = HAL_RCC_GetPCLK1Freq(); // 具体时钟频率请根据你的芯片和配置调整
uint32_t prescaler = htim12.Init.Prescaler + 1;
uint32_t period = (uint32_t)((float)timer_clk / (prescaler * note)) - 1;
__HAL_TIM_SET_AUTORELOAD(&htim12, period);
uint32_t pulse = (uint32_t)(0.5f * (float)(period + 1)); // 设置50%的占空比
__HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, pulse);
BSP_Delay((uint32_t)(delay_sec * 1000.0f)); // 延时指定秒数
return BSP_OK;
}
int8_t BSP_Buzzer_Stop(void) {
if (HAL_TIM_PWM_Stop(&htim12, TIM_CHANNEL_2) == HAL_OK) return BSP_OK;
return BSP_ERR;

35
User/bsp/buzzer.h
View File

@ -13,8 +13,43 @@ extern "C" {
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
/*预设音符列表*/
typedef enum {
BSP_BUZZER_NOTE_C4 = 261, // C4音符频率
BSP_BUZZER_NOTE_D4 = 294, // D4音符频率
BSP_BUZZER_NOTE_E4 = 329, // E4音符频率
BSP_BUZZER_NOTE_F4 = 349, // F4音符频率
BSP_BUZZER_NOTE_G4 = 392, // G4音符频率
BSP_BUZZER_NOTE_A4 = 440, // A4音符频率
BSP_BUZZER_NOTE_B4 = 494, // B4音符频率
BSP_BUZZER_NOTE_C5 = 523, // C5音符频率
BSP_BUZZER_NOTE_D5 = 587, // D5音符频率
BSP_BUZZER_NOTE_E5 = 659, // E5音符频率
BSP_BUZZER_NOTE_F5 = 698, // F5音符频率
BSP_BUZZER_NOTE_G5 = 784, // G5音符频率
BSP_BUZZER_NOTE_A5 = 880, // A5音符频率
BSP_BUZZER_NOTE_B5 = 988, // B5音符频率
BSP_BUZZER_NOTE_C6 = 1047, // C6音符频率
BSP_BUZZER_NOTE_D6 = 1175, // D6音符频率
BSP_BUZZER_NOTE_E6 = 1319, // E6音符频率
BSP_BUZZER_NOTE_F6 = 1397, // F6音符频率
BSP_BUZZER_NOTE_G6 = 1568, // G6音符频率
BSP_BUZZER_NOTE_A6 = 1760, // A6音符频率
BSP_BUZZER_NOTE_B6 = 1976, // B6音符频率
BSP_BUZZER_NOTE_C7 = 2093, // C7音符频率
BSP_BUZZER_NOTE_D7 = 2349, // D7音符频率
BSP_BUZZER_NOTE_E7 = 2637, // E7音符频率
BSP_BUZZER_NOTE_F7 = 2794, // F7音符频率
BSP_BUZZER_NOTE_G7 = 3136, // G7音符频率
BSP_BUZZER_NOTE_A7 = 3520, // A7音符频率
BSP_BUZZER_NOTE_B7 = 3951, // B7音符频率
BSP_BUZZER_NOTE_C8 = 4186 // C8音符频率
} BSP_Buzzer_Note_t;
int8_t BSP_Buzzer_Start(void);
int8_t BSP_Buzzer_Set(float freq, float duty_cycle);
int8_t BSP_Buzzer_Set_Note(BSP_Buzzer_Note_t note, float delay_sec);
int8_t BSP_Buzzer_Stop(void);
#ifdef __cplusplus

2
User/component/cmd.c
View File

@ -99,7 +99,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
cmd->chassis.delta_hight = rc->ch_res;
cmd->chassis.delta_eulr.pit = rc->ch_r_y;
}
// /**

1
User/component/cmd.h
View File

@ -38,6 +38,7 @@ typedef struct {
CMD_ChassisAction_t action; /* 底盘动作 */
float delta_hight; /* 底盘高度，单位：米 */
MoveVector_t ctrl_vec; /* 底盘控制向量 */
AHRS_Eulr_t delta_eulr; /* 云台欧拉角变化量 */
} CMD_ChassisCmd_t;
typedef struct {

60
User/component/kinematics.c
View File

@ -49,31 +49,6 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
return 0;
}
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign) {
float l1 = leg_params->hip * sign->hip;
float l2 = -leg_params->thigh;
float l3 = -leg_params->calf;
float q1 = theta_in[0];
float q2 = theta_in[1] * sign->thigh;
float q3 = theta_in[2] * sign->calf;
float s1 = sinf(q1);
float s2 = sinf(q2);
float s3 = sinf(q3);
float c1 = cosf(q1);
float c2 = cosf(q2);
float c3 = cosf(q3);
float c23 = c2 * c3 - s2 * s3;
float s23 = s2 * c3 + c2 * s3;
foot_out[0] = l3 * s23 + l2 * s2;
foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
foot_out[2] = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
}
/**
* @brief 单腿逆运动学
* @param foot_in 足端目标位置 [x, y, z]（单位：米）
@ -110,3 +85,38 @@ int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_Lin
theta_out[2] = q3 * sign->calf;
return 0;
}
void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3], Kinematics_Sign_t *sign) {
float l1 = leg_params->hip * sign->hip;
float l2 = -leg_params->thigh;
float l3 = -leg_params->calf;
float q1 = theta_in[0];
float q2 = theta_in[1] * sign->thigh;
float q3 = theta_in[2] * sign->calf;
float s1 = sinf(q1);
float s2 = sinf(q2);
float s3 = sinf(q3);
float c1 = cosf(q1);
float c2 = cosf(q2);
float c3 = cosf(q3);
float c23 = c2 * c3 - s2 * s3;
float s23 = s2 * c3 + c2 * s3;
float px = l3 * s23 + l2 * s2;
float py = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
float pz = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
// 配合逆运动学的符号处理
if (sign->hip != sign->thigh) {
foot_out[1] = py;
} else {
foot_out[1] = -py;
}
foot_out[0] = px;
foot_out[2] = pz;
}

32
User/component/limiter.c
View File

@ -9,19 +9,45 @@
#include "component/user_math.h"
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float max_speed, float max_angle, float min_angle){
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos,float *out_troque, float max_speed, float max_angle, float min_angle, float max_torque) {
// 限制位置变化速度
if (*out_pos - feedback_pos > max_speed) {
// if (*out_pos - feedback_pos > max_speed) {
// *out_pos = feedback_pos + max_speed;
// } else if (*out_pos - feedback_pos < -max_speed) {
// *out_pos = feedback_pos - max_speed;
// }
if (fabsf(*out_pos - feedback_pos) > max_speed) {
if (*out_pos > feedback_pos) {
*out_pos = feedback_pos + max_speed;
} else if (*out_pos - feedback_pos < -max_speed) {
} else {
*out_pos = feedback_pos - max_speed;
}
}
// 限制角度范围
if (*out_pos > max_angle) {
*out_pos = max_angle;
} else if (*out_pos < min_angle) {
*out_pos = min_angle;
}
/*限制前馈力矩*/
// if (fabsf(*out_pos) > max_torque) {
// if (*out_pos > 0) {
// *out_pos = max_torque;
// } else {
// *out_pos = -max_torque;
// }
// }
if (fabsf(*out_troque) > max_torque) {
if (*out_troque > 0) {
*out_troque = max_torque;
} else {
*out_troque = -max_torque;
}
}
return 0; // 成功
}

4
User/component/limiter.h
View File

@ -11,8 +11,8 @@ extern "C" {
#include <stdbool.h>
#include <stdint.h>
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos,
float max_speed, float max_angle, float min_angle);
int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float *out_torque,
float max_speed, float max_angle, float min_angle, float max_torque);

4
User/component/path.c
View File

@ -4,6 +4,10 @@
#include "path.h"
float Path_Straight1d(float start, float end, float t) {
return start + (end - start) * t;
}
/**
* @brief 生成一条直线段的路径点
* @param start 起始点

3
User/component/path.h
View File

@ -11,6 +11,9 @@ extern "C" {
#include <stdbool.h>
#include <stdint.h>
float Path_Straight1d(float start, float end, float t);
/**
* @brief 生成一条直线段的路径点
* @param start 起始点

6
User/device/n100.c
View File

@ -133,7 +133,7 @@ bool N100_WaitDmaCplt(void) {
SIGNAL_N100_NEW_DATA);
}
int8_t N100_ParseData (N100_t *n100) {
int8_t N100_ParseData (N100_t *n100, N100_Cali_t *cali) {
if (n100 == NULL) return DEVICE_ERR_NULL;
// 先校验原始数据
if (!N100_Verify(rxbuf)) {
@ -144,7 +144,9 @@ int8_t N100_ParseData (N100_t *n100) {
n100->eulr = n100->rx_raw.data.eulr;
n100->gyro = n100->rx_raw.data.gyro;
n100->quat = n100->rx_raw.data.quat;
n100->eulr.rol += cali->offset_x; /* 偏航角校准 */
n100->eulr.pit += cali->offset_y; /* 俯仰角校准 */
n100->eulr.yaw += cali->offset_z; /* 翻滚角校准 */
return DEVICE_OK;
}

7
User/device/n100.h
View File

@ -37,6 +37,11 @@ typedef struct {
uint8_t end;
} N100_AHRS_t;
typedef struct {
float offset_x;
float offset_y;
float offset_z;
} N100_Cali_t;
typedef struct {
osThreadId_t thread_alert;
@ -52,7 +57,7 @@ int8_t N100_Restart(void);
int8_t N100_StartReceiving(N100_t *n100);
bool N100_WaitDmaCplt(void);
int8_t N100_ParseData (N100_t *n100);
int8_t N100_ParseData (N100_t *n100, N100_Cali_t *cali);
int8_t N100_HandleOffline(N100_t *n100);
#ifdef __cplusplus

380
User/module/chassis.c
View File

@ -15,9 +15,10 @@
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
#define CHASSIS_DEFAULT_HEIGHT (0.22f) /* 底盘默认高度，单位：米 */
#define CHASSIS_MAX_SPEED (0.03f) /*调试用速度限幅*/
#define CHASSIS_MAX_SPEED (0.02f) /*调试用速度限幅*/
#define CHASSIS_MAX_TORQUE (1.0f) /*调试用力矩限幅*/
/* Private macro ------------------------------------------------------------ */
@ -43,13 +44,14 @@ static Kinematics_JointCMD_t position_mode = {
.T = 0.0f, /* 零力矩 */
.W = 0.0f, /* 零速度 */
.Pos = 0.0f, /* 零位置 */
.K_P = 3.0f, /* 刚度系数 */
.K_W = 0.2f, /* 速度系数 */
.K_P = 1.2f, /* 刚度系数 */
.K_W = 0.05f, /* 速度系数 */
};
// static uint8_t chassis_mode_states = 0;
// static uint8_t chassis_action_states = 0;
static uint8_t stage = 0;
static float T = 0.0f; /* 记录当前时间 */
/* Private function -------------------------------------------------------- */
/**
* \brief 设置底盘运行模式
@ -62,19 +64,26 @@ static uint8_t stage = 0;
static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisCmd_t *c_cmd) {
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (c_cmd == NULL) return CHASSIS_ERR_NULL; /* 控制指令不能为空 */
if (c->mode == c_cmd->mode && c->action == c_cmd->action) {
return CHASSIS_OK; /* 如果当前模式和动作与要设置的相同，直接返回 */
}
stage = 0;
c->mode = c_cmd->mode;
c->action = c_cmd->action; /* 更新底盘模式和动作 */
c->time = 0.0f; /* 重置时间 */
c->eulr_setpoint.pit = 0;
c->eulr_setpoint.rol = 0.0f; /* 重置期望的俯仰角和翻滚角 */
c->eulr_setpoint.yaw = c->eulr_imu.yaw; /* 重置期望的偏航角为当前IMU偏航角 */
if (c->mode != CHASSIS_MODE_POSITION) {
c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
}
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
PID_Reset(c->pid.motor_id + i);
LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
}
PID_Reset(&c->pid.blance); /* 重置平衡PID */
if (c->mode != c_cmd->mode) { /* 如果当前模式和要设置的模式不同 */
c->mode = c_cmd->mode; /* 更新底盘模式 */
stage = 0; /* 重置阶段 */
c->time = 0.0f; /* 重置时间 */
c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
}
if (c->action != c_cmd->action) { /* 如果当前动作和要设置的动作不同 */
c->action = c_cmd->action; /* 更新底盘动作 */
stage = 0; /* 重置阶段 */
c->time = 0.0f; /* 重置时间 */
c->height = CHASSIS_DEFAULT_HEIGHT; /* 重置底盘高度 */
}
return CHASSIS_OK;
}
@ -97,15 +106,27 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
c->height = CHASSIS_DEFAULT_HEIGHT; /* 设置底盘默认高度为0.2米 */
c->pid.motor_id = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->pid.motor_id));
c->pid.motor_id = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->pid.motor_id));
if (c->pid.motor_id == NULL) goto error;
c->filter.in = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.in));
c->filter.in = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->filter.in));
if (c->filter.in == NULL) goto error;
c->filter.out = BSP_Malloc(GO_MOTOR_MODE_NUM * sizeof(*c->filter.out));
c->filter.out = BSP_Malloc(GO_MOTOR_NUM * sizeof(*c->filter.out));
if (c->filter.out == NULL) goto error;
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
PID_Init(c->pid.motor_id + i, KPID_MODE_NO_D, target_freq,
&(c->param->torque_pid_param));
LowPassFilter2p_Init(c->filter.in + i, target_freq,
c->param->low_pass_cutoff_freq.in);
LowPassFilter2p_Init(c->filter.out + i, target_freq,
c->param->low_pass_cutoff_freq.out);
}
PID_Init(&c->pid.blance, KPID_MODE_NO_D, target_freq,
&(c->param->blance_pid_param));
return CHASSIS_OK; /* 返回成功 */
error:
@ -132,6 +153,15 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go){
}
}
int8_t Chassis_UpdateImu(Chassis_t *c, const AHRS_Eulr_t *eulr){
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
if (eulr == NULL) return CHASSIS_ERR_NULL; /* IMU数据不能为空 */
c->eulr_imu.yaw = eulr->yaw;
c->eulr_imu.pit = eulr->pit;
c->eulr_imu.rol = eulr->rol;
}
int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now){
/* 底盘数据和控制指令结构体不能为空 */
if (c == NULL) return CHASSIS_ERR_NULL;
@ -159,10 +189,21 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
c->time = 0.0f; /* 重置时间 */
break;
case CHASSIS_MODE_POSITION:{
case CHASSIS_MODE_FOLLOW:{
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
c->output.id[i] = position_mode; /* 设置位置模式 */}
c->output.id[i] = position_mode; /* 设置位置模式 */
}
c->delta_eulr.pit = PID_Calc(&c->pid.blance, 0.0f, c->eulr_imu.pit, 0.0f, c->dt);
c->eulr_setpoint.pit += c->delta_eulr.pit; /* 更新期望的俯仰角 */
//限制c->eulr_setpoint.pit在-0.1f到0.1f之间
if (c->eulr_setpoint.pit > 0.05f) {
c->eulr_setpoint.pit = 0.05f;
}
else if (c->eulr_setpoint.pit < -0.05f) {
c->eulr_setpoint.pit = -0.05f;
}
// c->delta_eulr.pit = 0;
/* 计算俯仰角变化量 */
switch (c->action) {
case CHASSIS_ACTION_NONE:{
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
@ -175,16 +216,62 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
case CHASSIS_ACTION_STAND:{
c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
float pitch_correction = PID_Calc(&c->pid.blance, 0.0f, c->eulr_imu.pit, 0.0f, c->dt);
for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
if (i % 2 == 0) { /* 左前腿和右后腿 */
float target_pose[3] = {0.0, -0.0861, -c->height};
if (c->eulr_setpoint.pit > 0.0f) {
if (i == 0) { /* 左前腿 */
float target_pose[3] = {-0.015, -0.0861, -c->height - c->eulr_setpoint.pit};
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
} else if (i == 1) { /* 右前腿 */
float target_pose[3] = {-0.015, 0.0861, -c->height - c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
} else if (i == 2) { /* 左后腿 */
float target_pose[3] = {-0.015, -0.0861, -c->height + c->eulr_setpoint.pit}; /* 左前腿和右后腿 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
} else if (i == 3) { /* 右后腿 */
float target_pose[3] = {-0.015, 0.0861, -c->height + c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
}
} else {
float target_pose[3] = {0.0, 0.0861, -c->height}; /* 右前腿和左后腿 */
if (i == 0) { /* 左前腿 */
float target_pose[3] = {-0.015, -0.0861, -c->height - c->eulr_setpoint.pit};
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
} else if (i == 1) { /* 右前腿 */
float target_pose[3] = {-0.015, 0.0861, -c->height - c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 右前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
} else if (i == 2) { /* 左后腿 */
float target_pose[3] = {-0.015, -0.0861, -c->height + c->eulr_setpoint.pit};
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 左前腿髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 左前腿大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 左前腿小腿 */
} else if (i == 3) { /* 右后腿 */
float target_pose[3] = {-0.015, 0.0861, -c->height + c->eulr_setpoint.pit}; /* 右前腿和左后腿 */
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 右前腿髋关节 */
@ -192,93 +279,44 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 右前腿小腿 */
}
}
}
// break;
if (c_cmd->ctrl_vec.vx == 0.0f && c_cmd->ctrl_vec.vy == 0.0f && c_cmd->ctrl_vec.wz == 0.0f) {
break;
}
} /* 站立动作 */
case CHASSIS_ACTION_WALK:{
c->height = c->height + c_cmd->delta_hight * c->dt;
float T = 2.0f; // 步态周期(s)
float swing_height = 0.15f; // 摆动腿抬高高度
float stride_x = c_cmd->ctrl_vec.vx * T; // x方向步幅
float stride_y = c_cmd->ctrl_vec.vy * T; // y方向步幅
float wz = c_cmd->ctrl_vec.wz; // 旋转速度
uint8_t swing_leg = (uint8_t)(c->time / (T / 4)) % 4; // 当前摆动腿编号
float t_phase = fmodf(c->time, T / 4) / (T / 4); // 当前腿相内归一化时间
for (uint8_t leg = 0; leg < 4; leg++) {
float target_pose[3];
float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
float base_x = 0.0f;
if (leg == swing_leg) {
// 摆动腿：贝塞尔轨迹，抬高并前移
float start[3] = {base_x - stride_x/2, base_y - stride_y/2, -c->height};
float mid1[3] = {base_x, base_y, -c->height + swing_height};
float mid2[3] = {base_x, base_y, -c->height + swing_height};
float end[3] = {base_x + stride_x/2, base_y + stride_y/2, -c->height};
Path_Bezier3d(start, mid1, mid2, end, t_phase, target_pose);
} else {
// 支撑腿：3/4周期内走完整步幅的1/3
// 计算支撑腿在支撑相内的归一化时间
uint8_t support_leg_index = (leg + 4 - swing_leg) % 4;
float support_phase = fmodf(c->time + (T / 4) * support_leg_index, T) / (T * 3.0f / 4.0f);
if (support_phase > 1.0f) support_phase = 1.0f;
// 支撑腿分三段，每段1/3步幅
float seg = support_phase * 3.0f; // [0,3)
int seg_idx = (int)seg; // 0,1,2
float seg_phase = seg - seg_idx; // [0,1)
// 每段起止点
float seg_start_x = base_x + stride_x/2 - stride_x * seg_idx / 3.0f;
float seg_start_y = base_y + stride_y/2 - stride_y * seg_idx / 3.0f;
float seg_end_x = base_x + stride_x/2 - stride_x * (seg_idx+1) / 3.0f;
float seg_end_y = base_y + stride_y/2 - stride_y * (seg_idx+1) / 3.0f;
Path_straight3d(
(float[3]){seg_start_x, seg_start_y, -c->height},
(float[3]){seg_end_x, seg_end_y, -c->height},
seg_phase, target_pose
);
}
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
c->output.id[leg * 3 + 0].Pos = angle_pose[0];
c->output.id[leg * 3 + 1].Pos = angle_pose[1];
c->output.id[leg * 3 + 2].Pos = angle_pose[2];
}
break;
}
case CHASSIS_ACTION_TROT:{
c->height = c->height + c_cmd->delta_hight * c->dt;
float T = 0.6f; // 步态周期(s)
if (c->time > T) {
c->time -= T; // 保持时间在0-T之间
}
float stride_x = c_cmd->ctrl_vec.vx / 10.0f;
float stride_y = c_cmd->ctrl_vec.vy / 10.0f;
float swing_height = 0.15f; // 摆动腿抬高高度
float swing_height = 0.12f; // 摆动腿抬高高度
// 对角腿分组：0-3一组，1-2一组
for (uint8_t leg = 0; leg < 4; leg++) {
float t_leg = fmodf(c->time + ((leg == 0 || leg == 3) ? 0.0f : T/2), T) / T; // 对角腿相差半周期
float base_y = (leg % 2 == 0) ? -0.0861f : 0.0861f;
float wz = (leg < 2) ? c_cmd->ctrl_vec.wz/5 : -c_cmd->ctrl_vec.wz/5; // 前两条腿正向，后两条腿反向
float target_pose[3];
if (t_leg < 0.5f) {
// 摆动相，贝塞尔插值
float t_bezier = t_leg / 0.5f;
float start[3] = {-stride_x, base_y - stride_y, -c->height};
float mid1[3] = {0, base_y, -c->height + swing_height};
float mid2[3] = {0, base_y, -c->height + swing_height};
float end[3] = {stride_x, base_y + stride_y, -c->height};
float start[3] = {-stride_x -0.015, base_y - stride_y - wz, -c->height};
float mid1[3] = {-0.015 - stride_x/2, base_y, -c->height + swing_height};
float mid2[3] = {-0.015 + stride_x/2, base_y, -c->height + swing_height};
float end[3] = {stride_x -0.015, base_y + stride_y + wz, -c->height};
Path_Bezier3d(start, mid1, mid2, end, t_bezier, target_pose);
} else {
// 支撑相，首尾相连，起点为上一个摆动相终点，终点为下一个摆动相起点
float t_line = (t_leg - 0.5f) / 0.5f;
float start[3] = {stride_x, base_y + stride_y, -c->height}; // 摆动相终点
float end[3] = {-stride_x, base_y - stride_y, -c->height}; // 下一个摆动相起点
float start[3] = {stride_x-0.015, base_y + stride_y + wz, -c->height}; // 摆动相终点
float end[3] = {-stride_x-0.015, base_y - stride_y - wz, -c->height}; // 下一个摆动相起点
Path_straight3d(start, end, t_line, target_pose);
}
float angle_pose[3];
@ -292,16 +330,176 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
}
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
/*PID计算力矩*/
c->output.id[i].T = PID_Calc(c->pid.motor_id, c->output.id[i].Pos, c->feedback.id[i].Pos, 0.0f, c->dt);
}
break;
}
case CHASSIS_MODE_POSITION:{
/*设置pd参数为位控参数，获取遥控器控制命令*/
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
c->output.id[i] = position_mode;} /* 设置位置模式 */
c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
c->eulr_setpoint.pit += c_cmd->delta_eulr.pit* c->dt; /* 更新期望的俯仰角 */
c->eulr_setpoint.rol = 0.0f; /* 期望的俯仰角和翻滚角 */
c->eulr_setpoint.yaw += c_cmd->delta_eulr.yaw * c->dt; /* 期望的偏航角 */
/*pitch轴软件限位*/
if (c->eulr_setpoint.pit > 0.2f) {
c->eulr_setpoint.pit = 0.2f; /* 限制俯仰角在-0.05到0.05之间 */
} else if (c->eulr_setpoint.pit < -0.2f) {
c->eulr_setpoint.pit = -0.2f; /* 限制俯仰角在-0.05到0.05之间 */
}
switch (c->action)
{
case CHASSIS_ACTION_NONE:{
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
c->output.id[i].Pos = c->feedback.id[i].Pos;}
c->mode = CHASSIS_MODE_POSITION;
c->action = CHASSIS_ACTION_NONE;
c->time = 0.0f; /* 重置时间 */
break;
}
case CHASSIS_ACTION_STAND:{
switch (stage) {
/* 重置时间记录当前位子 */
case 0:{
c->time = 0.0f; /* 重置时间 */
T = 0.2f; /* 设置站立动作的时间周期 */
for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
c->leg_pos.start_pos.leg[i].x = c->feedback.id[i * 3].Pos; /* 四条腿基坐标x轴位置 */
c->leg_pos.start_pos.leg[i].y = c->feedback.id[i * 3 + 1].Pos;
c->leg_pos.start_pos.leg[i].z = c->feedback.id[i * 3 + 2].Pos;
c->leg_pos.end_pos.leg[i].x = -0.015f; /* 四条腿基坐标x轴位置 */
c->leg_pos.end_pos.leg[i].y = (i % 2 == 0) ? -c->param->mech_param.length.hip : c->param->mech_param.length.hip; /* 左前腿和右后腿y轴位置，右前腿和左后腿y轴位置 */
c->leg_pos.end_pos.leg[i].z = -c->height; /* 四条腿基坐标z轴位置 */
float angle_pose[3];
Kinematics_InverseKinematics(&c->leg_pos.end_pos.leg[i].x, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->leg_pos.end_pos.leg[i].x = angle_pose[0]; /* 髋关节 */
c->leg_pos.end_pos.leg[i].y = angle_pose[1]; /* 大腿 */
c->leg_pos.end_pos.leg[i].z = angle_pose[2]; /* 小腿 */
/*设置四腿基坐标*/
c->foot_base[i].x = -0.015f; /* 四条腿基坐标x轴位置 */
c->foot_base[i].y = (i % 2 == 0) ? -c->param->mech_param.length.hip : c->param->mech_param.length.hip; /* 左前腿和右后腿y轴位置，右前腿和左后腿y轴位置 */
c->foot_base[i].z = -c->height;
}
stage ++;
break;
}
case 1:{
/*在周期内对12个关节完成一次直线插补*/
for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
float t = c->time / T; /* 计算当前时间在周期中的比例 */
c->output.id[i * 3].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].x, c->leg_pos.end_pos.leg[i].x, t); /* 髋关节 */
c->output.id[i * 3 + 1].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].y, c->leg_pos.end_pos.leg[i].y, t); /* 大腿 */
c->output.id[i * 3 + 2].Pos = Path_Straight1d(c->leg_pos.start_pos.leg[i].z, c->leg_pos.end_pos.leg[i].z, t); /* 小腿 */
}
if (c->time >= T) {
stage ++;
}
break;
}
case 2:{
/*计算pit和rol的平衡pid*/
float pitch_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.pit, c->eulr_imu.pit, 0.0f, c->dt);
float roll_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.rol, c->eulr_imu.rol, 0.0f, c->dt);
/*计算为了补偿平衡的腿长*/
c->foot_base[0].z = -c->height - pitch_correction - roll_correction + c->delta_eulr.pit; /* 左前腿 */
c->foot_base[1].z = -c->height - pitch_correction + roll_correction + c->delta_eulr.pit; /* 右前腿 */
c->foot_base[2].z = -c->height + pitch_correction - roll_correction - c->delta_eulr.pit; /* 左后腿 */
c->foot_base[3].z = -c->height + pitch_correction + roll_correction - c->delta_eulr.pit; /* 右后腿 */
float target_pose[3];
float angle_pose[3];
/*将足端坐标转换为关节角度*/
for (uint8_t i = 0; i < GO_MOTOR_NUM/3; i++) {
target_pose[0] = c->foot_base[i].x; /* 髋关节 */
target_pose[1] = c->foot_base[i].y; /* 大腿 */
target_pose[2] = c->foot_base[i].z;
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]);
c->output.id[i * 3].Pos = angle_pose[0]; /* 髋关节 */
c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿 */
c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿 */
}
break;
}
}
if (c_cmd->ctrl_vec.vx == 0.0f && c_cmd->ctrl_vec.vy == 0.0f && c_cmd->ctrl_vec.wz == 0.0f){
break;
}
} /* 站立动作 */
case CHASSIS_ACTION_TROT:{
if( c->action == CHASSIS_ACTION_TROT){
float pitch_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.pit, c->eulr_imu.pit, 0.0f, c->dt);
float roll_correction = PID_Calc(&c->pid.blance, c->eulr_setpoint.rol, c->eulr_imu.rol, 0.0f, c->dt);
/*计算为了补偿平衡的腿长*/
c->foot_base[0].z = -c->height - pitch_correction - roll_correction; /* 左前腿 */
c->foot_base[1].z = -c->height - pitch_correction + roll_correction; /* 右前腿 */
c->foot_base[2].z = -c->height + pitch_correction - roll_correction; /* 左后腿 */
c->foot_base[3].z = -c->height + pitch_correction + roll_correction; /* 右后腿 */
}
T = 0.6f; /* 设置trot动作的时间周期 */
if (c->time > T) {
c->time -= T; // 保持时间在0-T之间
}
float stride_x = c_cmd->ctrl_vec.vx / 10.0f; /* 步幅x轴 */
float stride_y = c_cmd->ctrl_vec.vy / 10.0f; /* 步幅y轴 */
float swing_height = 0.12f; // 摆动腿默认抬高高度
// 对角腿分组：0-3一组，1-2一组
for (uint8_t leg = 0; leg < 4; leg++) {
float t_leg = fmodf(c->time + ((leg == 0 || leg == 3) ? 0.0f : T/2), T) / T; // 对角腿相差半周期
float wz = (leg < 2) ? c_cmd->ctrl_vec.wz/5 : -c_cmd->ctrl_vec.wz/5; // 前两条腿正向，后两条腿反向
float target_pose[3];
if (t_leg < 0.5f) {
// 摆动相，贝塞尔插值
float t_bezier = t_leg / 0.5f;
float start[3] = {c->foot_base[leg].x -stride_x, c->foot_base[leg].y - stride_y - wz, c->foot_base[leg].z};
float mid1[3] = {c->foot_base[leg].x - stride_x/2, c->foot_base[leg].y, c->foot_base[leg].z + swing_height};
float mid2[3] = {c->foot_base[leg].x + stride_x/2, c->foot_base[leg].y, c->foot_base[leg].z + swing_height};
float end[3] = {c->foot_base[leg].x +stride_x , c->foot_base[leg].y + stride_y + wz, c->foot_base[leg].z};
Path_Bezier3d(start, mid1, mid2, end, t_bezier, target_pose);
} else {
// 支撑相，首尾相连，起点为上一个摆动相终点，终点为下一个摆动相起点
float t_line = (t_leg - 0.5f) / 0.5f;
float start[3] = {stride_x+c->foot_base[leg].x, c->foot_base[leg].y + stride_y + wz, c->foot_base[leg].z}; // 摆动相终点
float end[3] = {-stride_x+c->foot_base[leg].x, c->foot_base[leg].y - stride_y - wz, c->foot_base[leg].z}; // 下一个摆动相起点
Path_straight3d(start, end, t_line, target_pose);
}
float angle_pose[3];
Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[leg]);
c->output.id[leg * 3 + 0].Pos = angle_pose[0];
c->output.id[leg * 3 + 1].Pos = angle_pose[1];
c->output.id[leg * 3 + 2].Pos = angle_pose[2];
}
break;
} /* trot动作 */
}
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
/*PID计算力矩*/
c->output.id[i].T = PID_Calc(c->pid.motor_id, c->output.id[i].Pos, c->feedback.id[i].Pos, 0.0f, c->dt);
}
}
}
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
/* 限制输出 */
Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos, &c->output.id[i].T,
c->param->mech_param.ratio.id[i] * CHASSIS_MAX_SPEED,
c->param->mech_param.limit.max.id[i],
c->param->mech_param.limit.min.id[i]);
c->param->mech_param.limit.min.id[i],
c->param->mech_param.ratio.id[i] * CHASSIS_MAX_TORQUE);
}
}

67
User/module/chassis.h
View File

@ -38,24 +38,6 @@ typedef enum {
IMU_ERROR,
} Chassis_ImuType_t;
// typedef union {
// float id[GO_MOTOR_NUM]; /* 零点角度，单位：弧度 */
// struct {
// float lf_hip; /* 左前腿髋关节零点 */
// float lf_thigh; /* 左前腿大腿零点 */
// float lf_calf; /* 左前腿小腿零点 */
// float rf_hip; /* 右前腿髋关节零点 */
// float rf_thigh; /* 右前腿大腿零点 */
// float rf_calf; /* 右前腿小腿零点 */
// float lr_hip; /* 左后腿髋关节零点 */
// float lr_thigh; /* 左后腿大腿零点 */
// float lr_calf; /* 左后腿小腿零点 */
// float rr_hip; /* 右后腿髋关节零点 */
// float rr_thigh; /* 右后腿大腿零点 */
// float rr_calf; /* 右后腿小腿零点 */
// } named;
// } Chassis_MotorZeroPoint_t;
typedef union {
float id[GO_MOTOR_NUM];
struct{
@ -74,9 +56,14 @@ typedef union {
} named;
} joint_params;
typedef struct {
float x;
float y;
float z;
} joint_pos;
typedef struct {
joint_params min;
joint_params max; /* 关节的最小和最大角度，单位：弧度 */
joint_params max;
} joint_limits;
typedef union {
@ -97,6 +84,7 @@ typedef struct{
joint_params ratio; /* 关节减速比 */
joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
Kinematics_LegOffset_t leg_offset; /* 关节偏移，单位：米 */
Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
@ -110,7 +98,7 @@ typedef struct {
Chassis_Mech_Params_t mech_param; /* 机械参数 */
KPID_Params_t torque_pid_param; /* 力矩矩控制PID的参数 */
KPID_Params_t blance_pid_param; /* 平衡PID的参数 */
/* 低通滤波器截止频率 */
struct {
float in; /* 输入 */
@ -133,18 +121,45 @@ typedef struct {
GO_ChassisFeedback_t feedback; /* 底盘反馈信息 */
GO_ChassisCMD_t output;
float height; /* 底盘高度，单位：米 */
struct {
float x;
float y;
float z;
} foot_base[GO_MOTOR_NUM/3]; /* 四个足端的位置，单位：米 */
struct{
struct{
joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
}start_pos;
struct{
joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
}mid1_pos;
struct{
joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
}mid2_pos;
struct{
joint_pos leg[GO_MOTOR_NUM/3]; /* 四条腿的关节位置，单位：米 */
}end_pos;
} leg_pos; /* 四条腿的关节位置，单位：米 */
/* 模块通用 */
CMD_ChassisMode_t mode; /* 底盘模式 */
CMD_ChassisAction_t action; /* 底盘模式 */
/* 底盘设计 */
int8_t num_joint; /* 关节数量 */
AHRS_Eulr_t eulr_imu; /* 欧拉角，单位：弧度 */
AHRS_Eulr_t delta_eulr; /* 欧拉角变化量，单位：弧度 */
AHRS_Eulr_t eulr_setpoint; /* 期望的欧拉角，单位：弧度 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */
/* 反馈控制用的PID */
struct {
KPID_t *motor_id; /* 控制轮子电机用的PID的动态数组 */
KPID_t blance; /* 平衡PID */
} pid;
/* 滤波器 */
@ -178,6 +193,14 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,float target_fre
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go);
/**
* @brief 更新底盘的IMU信息
* @param c 包含底盘数据的结构体
* @param eulr 包含IMU欧拉角的结构体
* @return
*/
int8_t Chassis_UpdateImu(Chassis_t *c, const AHRS_Eulr_t *eulr);
/**
* \brief 运行底盘控制逻辑
*

39
User/module/config.c
View File

@ -30,13 +30,24 @@ Config_t param_default = {
.type = CHASSIS_TYPE_QUADRUPED,
.torque_pid_param = {
.k = 1.0f, /* 控制器增益 */
.p = 1.0f, /* 比例项增益 */
.i = 0.0f, /* 积分项增益 */
.k = 5.0f, /* 控制器增益 */
.p = 20.0f, /* 比例项增益 */
.i = 1.0f, /* 积分项增益 */
.d = 0.0f, /* 微分项增益 */
.i_limit = 0.0f, /* 积分项上限 */
.out_limit = 0.0f, /* 输出绝对值限制 */
.d_cutoff_freq = 10.0f, /* D项低通截止频率 */
.i_limit = 100.0f, /* 积分项上限 */
.out_limit = 100.0f, /* 输出绝对值限制 */
.d_cutoff_freq = -1.0f, /* D项低通截止频率 */
.range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
},
.blance_pid_param = {
.k = 1.00f, /* 控制器增益 */
.p = 0.08f, /* 比例项增益 */
.i = 0.08f, /* 积分项增益 */
.d = 0.0f, /* 微分项增益 */
.i_limit = 0.05f, /* 积分项上限 */
.out_limit = 0.1f, /* 输出绝对值限制 */
.d_cutoff_freq = -1.0f, /* D项低通截止频率 */
.range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
},
@ -91,19 +102,19 @@ Config_t param_default = {
.named = {
.lf_hip = 0.02f, /* 左前腿髋关节零点角度，单位：弧度 */
.lf_thigh = -3.17f, /* 左前腿大腿零点角度，单位：弧度 */
.lf_calf = 2.84f - JOINT_CALF_OFFSET, /* 左前腿小腿零点角度，单位：弧度 */
.lf_calf = 0.72f - JOINT_CALF_OFFSET, /* 左前腿小腿零点角度，单位：弧度 */
.rf_hip = 5.37f, /* 右前腿髋关节零点角度，单位：弧度 */
.rf_thigh = 9.38f, /* 右前腿大腿零点角度，单位：弧度 */
.rf_calf = 4.45f + JOINT_CALF_OFFSET, /* 右前腿小腿零点角度，单位：弧度 */
.rf_calf = 4.96f + JOINT_CALF_OFFSET, /* 右前腿小腿零点角度，单位：弧度 */
.lr_hip = 4.70f, /* 左后腿髋关节零点角度，单位：弧度 */
.lr_thigh = -4.11f, /* 左后腿大腿零点角度，单位：弧度 */
.lr_calf = 2.56f - JOINT_CALF_OFFSET, /* 左后腿小腿零点角度，单位：弧度 */
.lr_hip = 4.5f, /* 左后腿髋关节零点角度，单位：弧度 */
.lr_thigh = -3.2f, /* 左后腿大腿零点角度，单位：弧度 */
.lr_calf = 1.73f - JOINT_CALF_OFFSET, /* 左后腿小腿零点角度，单位：弧度 */
.rr_hip = 2.85f, /* 右后腿髋关节零点角度，单位：弧度 */
.rr_thigh = 10.81f, /* 右后腿大腿零点角度，单位：弧度 */
.rr_calf = 3.59f + JOINT_CALF_OFFSET, /* 右后腿小腿零点角度，单位：弧度 */
.rr_hip = 2.7f, /* 右后腿髋关节零点角度，单位：弧度 */
.rr_thigh = 10.58f, /* 右后腿大腿零点角度，单位：弧度 */
.rr_calf = 3.58f + JOINT_CALF_OFFSET, /* 右后腿小腿零点角度，单位：弧度 */
}
},

12
User/task/atti_esti.c
View File

@ -26,7 +26,12 @@ N100_t n100;
BMI088_Cali_t bmi088_cali = {
.gyro_offset = {0.0f, 0.0f, 0.0f},
}; /* BMI088校准数据 */
AHRS_Magn_t mage = {0.0f, 0.0f, 0.0f}; /* 磁力计数据，未使用 */
N100_Cali_t n100_cali = {
.offset_x = 0.0f,
.offset_y = 0.0f,
.offset_z = 0.0f,
};
AHRS_Magn_t mage = {-0.01f, 0.025f, 0.0f}; /* 磁力计数据，未使用 */
AHRS_t gimbal_ahrs;
AHRS_Eulr_t eulr_to_send;
@ -109,14 +114,15 @@ void Task_atti_esti(void *argument) {
N100_StartReceiving(&n100);
if (N100_WaitDmaCplt()) {
osKernelLock();
N100_ParseData(&n100);
N100_ParseData(&n100, &n100_cali); /* 解析N100接收到的数据 */
osKernelUnlock();
} else {
N100_HandleOffline(&n100);
}
osMessageQueueReset(task_runtime.msgq.body.eulr_imu); /* 重置消息队列 */
osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
osMessageQueueReset(task_runtime.msgq.chassis.eulr_imu); /* 重置消息队列 */
osMessageQueuePut(task_runtime.msgq.chassis.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
break;
default:

4
User/task/ctrl_chassis.c
View File

@ -16,6 +16,7 @@
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN*/
Chassis_t chassis;
AHRS_Eulr_t chassis_eulr; /* 底盘IMU欧拉角数据 */
GO_ChassisFeedback_t chassis_feedback; /* 底盘反馈数据 */
GO_ChassisCMD_t chassis_output; /* 底盘输出数据 */
CMD_ChassisCmd_t chassis_cmd; /* 底盘控制命令 */
@ -43,6 +44,9 @@ void Task_ctrl_chassis(void *argument) {
if(osMessageQueueGet(task_runtime.msgq.chassis.feefback, &chassis_feedback, NULL, 0) == osOK) { /* 从消息队列中获取底盘反馈数据 */
Chassis_UpdateFeedback(&chassis, &chassis_feedback); /* 更新底盘反馈数据 */
}
if(osMessageQueueGet(task_runtime.msgq.chassis.eulr_imu, &chassis_eulr, NULL, 0) == osOK) { /* 从消息队列中获取IMU欧拉角数据 */
Chassis_UpdateImu(&chassis, &chassis_eulr); /* 更新底盘IMU数据 */
}
osMessageQueueGet(task_runtime.msgq.cmd.chassis, &chassis_cmd, NULL, 0);
osKernelLock();

1
User/task/init.c
View File

@ -46,6 +46,7 @@ void Task_Init(void *argument) {
task_runtime.msgq.chassis.feefback = osMessageQueueNew(2u, sizeof(GO_ChassisFeedback_t), NULL);
task_runtime.msgq.chassis.output = osMessageQueueNew(2u, sizeof(GO_ChassisCMD_t), NULL);
task_runtime.msgq.body.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
task_runtime.msgq.chassis.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

33
User/task/test.c
View File

@ -6,8 +6,7 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN*/
#include "bsp/uart.h"
#include "gom_protocol.h"
#include "bsp/buzzer.h"
/* USER INCLUDE END*/
/* Private typedef ---------------------------------------------------------- */
@ -15,10 +14,8 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN*/
MotorCmd_t mcmd = {0};
MotorData_t data = {0};
// const BSP_Buzzer_Note_t music_notes[] = { BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_D4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_C4, 0, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_C7, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_A6, 0, BSP_BUZZER_NOTE_C5, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_G4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_F6, BSP_BUZZER_NOTE_C4, 0 };
// uint32_t index = 0; /* 音符索引 */
/* USER STRUCT END*/
/* Private function --------------------------------------------------------- */
@ -33,28 +30,20 @@ void Task_test(void *argument) {
osDelay(TEST_INIT_DELAY); /* 延时一段时间再开启任务 */
/* USER CODE INIT BEGIN*/
mcmd.id = 0;
mcmd.mode = 1;
mcmd.K_P = 0;
mcmd.K_W = 0;
mcmd.Pos = 0;
mcmd.W = 0;
mcmd.T = 0;
BSP_Buzzer_Start(); /* 启动蜂鸣器 */
BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_C4, 0.5f); /* 设置蜂鸣器音符为C4，延时0.5秒 */
BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_E4, 0.5f); /* 设置蜂鸣器音符为E4，延时0.5秒 */
BSP_Buzzer_Set_Note(BSP_BUZZER_NOTE_G4, 0.5f); /* 设置蜂鸣器音符为G4，延时0.5秒 */
BSP_Buzzer_Stop(); /* 停止蜂鸣器 */
// BSP_Buzzer_Set(2000,0.5f);
/* USER CODE INIT END*/
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// modify_data(&mcmd);
// HAL_UART_Transmit(BSP_UART_GetHandle(BSP_UART_LEFT_LEG), (uint8_t *)&mcmd.motor_send_data, sizeof(mcmd.motor_send_data), 1);
// HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG), (uint8_t *)&data.motor_recv_data, sizeof(data.motor_recv_data), 1);
// extract_data(&data);
// BSP_Buzzer_Set_Note(music_notes[index], 0.02f); /* 设置蜂鸣器音符 */
// index++;
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

3
User/task/user_task.h
View File

@ -13,7 +13,7 @@ extern "C" {
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* 任务运行频率 */
#define TEST_FREQ (1000)
#define TEST_FREQ (50)
#define MONITOR_FREQ (100)
#define CMD_FREQ (500)
#define CTRL_LEG_FREQ (800)
@ -67,6 +67,7 @@ typedef struct {
struct {
osMessageQueueId_t feefback;
osMessageQueueId_t output;
osMessageQueueId_t eulr_imu;
}chassis; /* 底盘控制消息队列 */
} msgq;
/* USER MESSAGE END */

BIN
Utils/1111.mp3 Normal file
View File

Binary file not shown.

4
Utils/222.c Normal file
View File

@ -0,0 +1,4 @@
// 自动生成的音符数组，每0.2秒检测一次主旋律
const BSP_Buzzer_Note_t music_notes[] = { BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_D4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_C4, 0, BSP_BUZZER_NOTE_F5, BSP_BUZZER_NOTE_G5, BSP_BUZZER_NOTE_C7, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_G6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_F4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_A6, 0, BSP_BUZZER_NOTE_C5, BSP_BUZZER_NOTE_D6, BSP_BUZZER_NOTE_C6, 0, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_C6, BSP_BUZZER_NOTE_B6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E6, BSP_BUZZER_NOTE_A6, BSP_BUZZER_NOTE_E4, BSP_BUZZER_NOTE_G4, BSP_BUZZER_NOTE_C4, BSP_BUZZER_NOTE_F6, BSP_BUZZER_NOTE_C4, 0 };
const float music_notes_durations[] = { 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.80f, 0.20f, 0.20f, 0.20f, 0.60f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.40f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.20f, 0.40f, 0.80f };
const unsigned int music_notes_len = 43;

BIN
Utils/222.mp3 Normal file
View File

Binary file not shown.

193
Utils/Kinematics.py
View File

@ -1,30 +1,63 @@
# test_kinematics.py
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def forward_kinematics(theta, link, side_sign):
l1 = link['hip'] * side_sign
l2 = -link['thigh']
l3 = -link['calf']
# 定义机械参数
leg_params = {
'hip': 0.1, # 髋关节长度
'thigh': 0.3, # 大腿长度
'calf': 0.3 # 小腿长度
}
# 定义关节方向符号 (与C代码中一致)
signs = {
'left_front': {'hip': 1, 'thigh': 1, 'calf': -1},
'right_front': {'hip': -1, 'thigh': -1, 'calf': 1},
'left_rear': {'hip': -1, 'thigh': 1, 'calf': -1},
'right_rear': {'hip': 1, 'thigh': -1, 'calf': 1}
}
def forward_kinematics(theta, leg_params, sign):
"""正运动学"""
q1, q2, q3 = theta
q2 = q2 * sign['thigh']
q3 = q3 * sign['calf']
s1, s2, s3 = np.sin([q1, q2, q3])
c1, c2, c3 = np.cos([q1, q2, q3])
l1 = leg_params['hip'] * sign['hip']
l2 = leg_params['thigh']
l3 = leg_params['calf']
c23 = c2 * c3 - s2 * s3
s23 = s2 * c3 + c2 * s3
# 计算足端位置
x = l2 * np.cos(q2) + l3 * np.cos(q2 + q3)
y = l1 * np.cos(q1) + np.sin(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
z = l1 * np.sin(q1) - np.cos(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
x = l3 * s23 + l2 * s2
y = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1
z = l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2
return np.array([x, y, z])
foot_pos = np.array([x, -y if sign['hip'] == sign['thigh'] else y, z])
return foot_pos
def inverse_kinematics(foot_pos, leg_params, sign):
"""逆运动学"""
px = foot_pos[0]
py = -foot_pos[1] if sign['hip'] == sign['thigh'] else foot_pos[1]
pz = foot_pos[2]
b2y = leg_params['hip'] * sign['hip']
b3z = -leg_params['thigh']
b4z = -leg_params['calf']
a = leg_params['hip']
c = np.sqrt(px**2 + py**2 + pz**2)
b = np.sqrt(c**2 - a**2)
# 检查可达性
if np.isnan(b) or b > abs(b3z) + abs(b4z):
return None
def q1_ik(py, pz, l1):
L = np.sqrt(py*py + pz*pz - l1*l1)
L = np.sqrt(py**2 + pz**2 - l1**2)
return np.arctan2(pz*l1 + py*L, py*l1 - pz*L)
def q3_ik(b3z, b4z, b):
temp = (b3z**2 + b4z**2 - b**2) / (2.0 * np.abs(b3z * b4z))
temp = (b3z**2 + b4z**2 - b**2) / (2.0 * abs(b3z * b4z))
temp = np.clip(temp, -1.0, 1.0)
q3 = np.arccos(temp)
return -(np.pi - q3)
@ -36,43 +69,109 @@ def q2_ik(q1, q3, px, py, pz, b3z, b4z):
m2 = b3z + b4z * np.cos(q3)
return np.arctan2(m1*a1 + m2*a2, m1*a2 - m2*a1)
def inverse_kinematics(foot, link, side_sign):
px, py, pz = foot
b2y = link['hip'] * side_sign
b3z = -link['thigh']
b4z = -link['calf']
a = link['hip']
c = np.sqrt(px*px + py*py + pz*pz)
b = np.sqrt(c*c - a*a)
if np.isnan(b) or b > abs(b3z) + abs(b4z):
return None
q1 = q1_ik(py, pz, b2y)
q3 = q3_ik(b3z, b4z, b)
q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z)
return np.array([q1, q2, q3])
# ...existing code...
return np.array([q1, q2 * sign['thigh'], q3 * sign['calf']])
def test():
link = {'hip': 0.05, 'thigh': 0.2, 'calf': 0.2}
side_sign = 1 # 左前腿
def test_kinematics(leg_sign, test_name):
print(f"\nTesting {test_name} leg...")
# 输入一个目标点
foot_target = np.array([0.0, 0.06, -0.2])
print("目标足端位置:", foot_target)
# 生成随机关节角度
np.random.seed(42)
test_angles = np.random.uniform(-np.pi/2, np.pi/2, (10, 3))
# 逆运动学求解关节角度
theta = inverse_kinematics(foot_target, link, side_sign)
if theta is None:
print("目标点不可达！")
return
print("逆运动学解（关节角度，单位：弧度）:", theta)
print("逆运动学解（关节角度，单位：度）:", np.degrees(theta))
errors = []
# 正运动学验算
foot_check = forward_kinematics(theta, link, side_sign)
print("正运动学验算足端位置:", foot_check)
print("位置误差:", np.abs(foot_target - foot_check))
for angles in test_angles:
# 正运动学计算足端位置
foot_pos = forward_kinematics(angles, leg_params, leg_sign)
if __name__ == "__main__":
test()
# 逆运动学计算关节角度
calculated_angles = inverse_kinematics(foot_pos, leg_params, leg_sign)
if calculated_angles is None:
print(f"Unreachable position for angles: {angles}")
continue
# 计算误差
error = np.max(np.abs(angles - calculated_angles))
errors.append(error)
print(f"Original angles: {angles}")
print(f"Calculated angles: {calculated_angles}")
print(f"Error: {error:.6f} rad\n")
avg_error = np.mean(errors)
max_error = np.max(errors)
print(f"Average error: {avg_error:.6f} rad")
print(f"Maximum error: {max_error:.6f} rad")
return avg_error, max_error
# 测试所有四种腿型
results = {}
for leg_name, leg_sign in signs.items():
avg_err, max_err = test_kinematics(leg_sign, leg_name)
results[leg_name] = (avg_err, max_err)
# 打印汇总结果
print("\nSummary of results:")
for leg_name, (avg_err, max_err) in results.items():
print(f"{leg_name}: avg error={avg_err:.6f} rad, max error={max_err:.6f} rad")
# 可视化一个例子
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
# 选择左前腿作为示例
leg_sign = signs['left_front']
angles = np.array([0.5, -0.3, 0.7]) # 髋关节、大腿、小腿角度
# 计算正运动学
foot_pos = forward_kinematics(angles, leg_params, leg_sign)
# 绘制
l1 = leg_params['hip'] * leg_sign['hip']
l2 = leg_params['thigh']
l3 = leg_params['calf']
q1, q2, q3 = angles
q2 = q2 * leg_sign['thigh']
q3 = q3 * leg_sign['calf']
# 计算各关节位置
hip_pos = np.array([0, 0, 0])
knee_pos = np.array([
l2 * np.cos(q2),
l1 * np.cos(q1) + np.sin(q1) * l2 * np.sin(q2),
l1 * np.sin(q1) - np.cos(q1) * l2 * np.sin(q2)
])
foot_pos = np.array([
l2 * np.cos(q2) + l3 * np.cos(q2 + q3),
l1 * np.cos(q1) + np.sin(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3)),
l1 * np.sin(q1) - np.cos(q1) * (l2 * np.sin(q2) + l3 * np.sin(q2 + q3))
])
# 调整y坐标符号
if leg_sign['hip'] != leg_sign['thigh']:
knee_pos[1] = -knee_pos[1]
foot_pos[1] = -foot_pos[1]
# 绘制连杆
ax.plot([hip_pos[0], knee_pos[0]], [hip_pos[1], knee_pos[1]], [hip_pos[2], knee_pos[2]], 'b-', linewidth=3)
ax.plot([knee_pos[0], foot_pos[0]], [knee_pos[1], foot_pos[1]], [knee_pos[2], foot_pos[2]], 'r-', linewidth=3)
# 绘制关节
ax.scatter(hip_pos[0], hip_pos[1], hip_pos[2], c='k', s=100, label='Hip')
ax.scatter(knee_pos[0], knee_pos[1], knee_pos[2], c='k', s=100, label='Knee')
ax.scatter(foot_pos[0], foot_pos[1], foot_pos[2], c='g', s=100, label='Foot')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Leg Kinematics Example (Left Front Leg)')
ax.legend()
plt.tight_layout()
plt.show()

65
Utils/music.py Normal file
View File

@ -0,0 +1,65 @@
from pydub import AudioSegment
import numpy as np
import os
import librosa
# 音符频率表（与buzzer.h一致）
NOTE_FREQS = [
261, 294, 329, 349, 392, 440, 494, 523, 587, 659, 698, 784, 880, 988, 1047, 1175, 1319, 1397, 1568, 1760, 1976, 2093, 2349, 2637, 2794, 3136, 3520, 3951, 4186
]
NOTE_NAMES = [
"BSP_BUZZER_NOTE_C4", "BSP_BUZZER_NOTE_D4", "BSP_BUZZER_NOTE_E4", "BSP_BUZZER_NOTE_F4", "BSP_BUZZER_NOTE_G4", "BSP_BUZZER_NOTE_A4", "BSP_BUZZER_NOTE_B4",
"BSP_BUZZER_NOTE_C5", "BSP_BUZZER_NOTE_D5", "BSP_BUZZER_NOTE_E5", "BSP_BUZZER_NOTE_F5", "BSP_BUZZER_NOTE_G5", "BSP_BUZZER_NOTE_A5", "BSP_BUZZER_NOTE_B5",
"BSP_BUZZER_NOTE_C6", "BSP_BUZZER_NOTE_D6", "BSP_BUZZER_NOTE_E6", "BSP_BUZZER_NOTE_F6", "BSP_BUZZER_NOTE_G6", "BSP_BUZZER_NOTE_A6", "BSP_BUZZER_NOTE_B6",
"BSP_BUZZER_NOTE_C7", "BSP_BUZZER_NOTE_D7", "BSP_BUZZER_NOTE_E7", "BSP_BUZZER_NOTE_F7", "BSP_BUZZER_NOTE_G7", "BSP_BUZZER_NOTE_A7", "BSP_BUZZER_NOTE_B7",
"BSP_BUZZER_NOTE_C8"
]
def freq_to_note_name(freq):
# 找到最接近的音符
idx = np.argmin(np.abs(np.array(NOTE_FREQS) - freq))
return NOTE_NAMES[idx], NOTE_FREQS[idx]
def mp3_to_note_array(mp3_path, c_array_name="music_notes", out_path=None, interval_sec=0.02):
y, sr = librosa.load(mp3_path, sr=None, mono=True)
hop_length = int(sr * interval_sec)
pitches, magnitudes = librosa.piptrack(y=y, sr=sr, hop_length=hop_length)
notes = []
durations = []
last_note = None
for t in range(pitches.shape[1]):
index = magnitudes[:, t].argmax()
freq = pitches[index, t]
if freq > 0:
note_name, note_freq = freq_to_note_name(freq)
else:
note_name = "0"
if note_name == last_note and notes:
durations[-1] += interval_sec
else:
notes.append(note_name)
durations.append(interval_sec)
last_note = note_name
# 生成C数组字符串
notes_str = ", ".join(notes)
durations_str = ", ".join([f"{d:.2f}f" for d in durations])
c_code = f"// 自动生成的音符数组，每{interval_sec}秒检测一次主旋律\n"
c_code += f"const BSP_Buzzer_Note_t {c_array_name}[] = {{ {notes_str} }};\n"
c_code += f"const float {c_array_name}_durations[] = {{ {durations_str} }};\n"
c_code += f"const unsigned int {c_array_name}_len = {len(notes)};\n"
# 自动生成输出路径
if out_path is None:
base, _ = os.path.splitext(mp3_path)
out_path = base + ".c"
# 写入C文件
with open(out_path, "w") as f:
f.write(c_code)
print(f"音符数组已写入: {out_path}")
if __name__ == "__main__":
mp3_path = "/Users/lvzucheng/Documents/R/CM_DOG/Utils/222.mp3" # 你的mp3文件路径
c_array_name = "music_notes" # C数组名
mp3_to_note_array(mp3_path, c_array_name, interval_sec=0.2)

3
Utils/tldhc.c Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Utils/tldhc.mp3 Normal file
View File

Binary file not shown.