516 lines
25 KiB
C
516 lines
25 KiB
C
/*
|
||
* 底盘模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "chassis.h"
|
||
|
||
#include <stdlib.h>
|
||
|
||
#include "cmsis_os2.h"
|
||
#include "device/go.h"
|
||
#include "component/limiter.h"
|
||
#include "component/kinematics.h"
|
||
#include "component/path.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define CHASSIS_DEFAULT_HEIGHT (0.22f) /* 底盘默认高度，单位：米 */
|
||
|
||
#define CHASSIS_MAX_SPEED (0.02f) /*调试用速度限幅*/
|
||
#define CHASSIS_MAX_TORQUE (1.0f) /*调试用力矩限幅*/
|
||
/* Private macro ------------------------------------------------------------ */
|
||
|
||
|
||
/* Private variables -------------------------------------------------------- */
|
||
static Kinematics_JointCMD_t zero_torque = {
|
||
.T = 0.0f, /* 零力矩 */
|
||
.W = 0.0f, /* 零速度 */
|
||
.Pos = 0.0f, /* 零位置 */
|
||
.K_P = 0.0f, /* 零刚度系数 */
|
||
.K_W = 0.0f, /* 零速度系数 */
|
||
};
|
||
|
||
static Kinematics_JointCMD_t damping_torque = {
|
||
.T = 0.0f, /* 零力矩 */
|
||
.W = 0.0f, /* 阻尼速度 */
|
||
.Pos = 0.0f, /* 零位置 */
|
||
.K_P = 0.0f, /* 零刚度系数 */
|
||
.K_W = 0.1f, /* 阻尼速度系数 */
|
||
};
|
||
|
||
//位控模式
|
||
static Kinematics_JointCMD_t position_mode = {
|
||
.T = 0.0f, /* 零力矩 */
|
||
.W = 0.0f, /* 零速度 */
|
||
.Pos = 0.0f, /* 零位置 */
|
||
.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 设置底盘运行模式
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param mode 要设置的模式
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
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 */
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 初始化底盘
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param param 包含底盘参数的结构体指针
|
||
* \param target_freq 任务预期的运行频率
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
|
||
float target_freq) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->param = param; /* 初始化参数 */
|
||
c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
|
||
c->height = CHASSIS_DEFAULT_HEIGHT; /* 设置底盘默认高度为0.2米 */
|
||
|
||
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_NUM * sizeof(*c->filter.in));
|
||
if (c->filter.in == NULL) goto error;
|
||
|
||
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:
|
||
/* 动态内存分配错误时，释放已经分配的内存，返回错误值 */
|
||
BSP_Free(c->pid.motor_id);
|
||
BSP_Free(c->filter.in);
|
||
BSP_Free(c->filter.out);
|
||
return CHASSIS_ERR_NULL;
|
||
}
|
||
|
||
/**
|
||
* \brief 更新底盘的反馈信息
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param can CAN设备结构体
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c, const GO_ChassisFeedback_t *go){
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
Kinematics_RealFeedback(&c->feedback.id[i],&go->id[i],
|
||
c->param->mech_param.ratio.id[i],
|
||
c->param->mech_param.zero_point.id[i]);
|
||
}
|
||
}
|
||
|
||
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;
|
||
if (c_cmd == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->dt = (float)(now - c->lask_wakeup) / 1000.0f;
|
||
c->lask_wakeup = now;
|
||
c->time += c->dt; /* 更新用于生成动作的时间 */
|
||
// while (c->time > 1.0f) { c ->time -= 1.0f; } /* 保持时间在0-1秒之间 */
|
||
|
||
Chassis_SetMode(c, c_cmd); /* 设置底盘模式和动作 */
|
||
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */
|
||
c->output = (GO_ChassisCMD_t){0}; /* 清空输出 */
|
||
c->mode = CHASSIS_MODE_RELAX; /* 更新状态 */
|
||
c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
|
||
c->time = 0.0f; /* 重置时间 */
|
||
break;
|
||
case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
|
||
}
|
||
c->mode = CHASSIS_MODE_DAMP; /* 更新状态 */
|
||
c->action = CHASSIS_ACTION_NONE; /* 清除动作状态 */
|
||
c->time = 0.0f; /* 重置时间 */
|
||
break;
|
||
case CHASSIS_MODE_FOLLOW:{
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
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++) {
|
||
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:{
|
||
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 (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 {
|
||
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]; /* 右前腿小腿 */
|
||
}
|
||
}
|
||
}
|
||
// 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:{
|
||
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.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 -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-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];
|
||
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;
|
||
}
|
||
|
||
|
||
}
|
||
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, &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.ratio.id[i] * CHASSIS_MAX_TORQUE);
|
||
}
|
||
|
||
}
|
||
|
||
void Chassis_DumpOutput(const Chassis_t *c, GO_ChassisCMD_t *out){
|
||
if (c == NULL || out == NULL) return; /* 主结构体和输出结构体不能为空 */
|
||
|
||
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
||
Kinematics_RealOutput(&c->output.id[i], &out->id[i],
|
||
c->param->mech_param.ratio.id[i],
|
||
c->param->mech_param.zero_point.id[i]);
|
||
}
|
||
}
|