231 lines
9.1 KiB
C
231 lines
9.1 KiB
C
/*
|
|
* 底盘模组
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "chassis.h"
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include "cmsis_os2.h"
|
|
// #include "component/limiter.h"
|
|
#include "device/go.h"
|
|
#include "component/limiter.h"
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/*底盘默认高度*/
|
|
#define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
|
|
#define CHASSIS_MAX_SPEED (0.03f)
|
|
/* 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.0f, /* 刚度系数 */
|
|
.K_W = 0.1f, /* 速度系数 */
|
|
};
|
|
|
|
static uint8_t chassis_mode_states = 0;
|
|
static uint8_t chassis_action_states = 0;
|
|
/* Private function -------------------------------------------------------- */
|
|
/**
|
|
* \brief 设置底盘模式
|
|
*
|
|
* \param c 包含底盘数据的结构体
|
|
* \param mode 要设置的模式
|
|
*
|
|
* \return 函数运行结果
|
|
*/
|
|
// static int8_t Chassis_SetMode(Chassis_t *c, CMD_ChassisMode_t mode,
|
|
// uint32_t now) {
|
|
// if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
|
|
// if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
|
|
|
|
// if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
|
|
// srand(now);
|
|
// c->wz_multi = (rand() % 2) ? -1 : 1;
|
|
// }
|
|
// /* 切换模式后重置PID和滤波器 */
|
|
// for (uint8_t i = 0; i < c->num_wheel; i++) {
|
|
// PID_Reset(c->pid.motor + i);
|
|
// LowPassFilter2p_Reset(c->filter.in + i, 0.0f);
|
|
// LowPassFilter2p_Reset(c->filter.out + i, 0.0f);
|
|
// }
|
|
// c->mode = mode;
|
|
|
|
// 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米 */
|
|
|
|
return CHASSIS_OK; /* 返回成功 */
|
|
}
|
|
|
|
/**
|
|
* \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_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->mode = c_cmd->mode; /* 设置底盘模式 */
|
|
|
|
|
|
|
|
switch (c->mode) {
|
|
case CHASSIS_MODE_RELAX: /* 放松模式，电机不输出 */
|
|
c->output = (GO_ChassisCMD_t){0}; /* 清空输出 */
|
|
chassis_mode_states = CHASSIS_MODE_RELAX; /* 更新状态 */
|
|
break;
|
|
case CHASSIS_MODE_DAMP: /* 阻尼模式，电机闭环控制保持阻尼，用于安全模式放松 */
|
|
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
|
c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
|
|
}
|
|
chassis_mode_states = CHASSIS_MODE_DAMP; /* 更新状态 */
|
|
break;
|
|
case CHASSIS_MODE_RECOVER: /* 恢复模式，电机闭环控制恢复到初始位置。用于机器人上电后恢复到初始位置 */
|
|
if (chassis_mode_states != CHASSIS_MODE_RECOVER){
|
|
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
|
if (i % 3 == 0) { /* 每三个电机一个组 */
|
|
c->output.id[i].K_P = 1.0;
|
|
c->output.id[i].K_W = 0.1; /* 设置刚度系数和速度系数 */
|
|
c->output.id[i].Pos = 0.0f; /* 设置位置为零 */
|
|
c->output.id[i].T = 0.0f; /* 设置力矩为零 */
|
|
c->output.id[i].W = 0.0f; /* 设置速度为零 */
|
|
}
|
|
else {
|
|
// c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
|
|
c->output.id[i] = position_mode; /* 设置位置模式 */
|
|
}
|
|
|
|
// c->output.named.lf_thigh.Pos = 1.0f; /* 左前腿大腿位置 */
|
|
// c->output.named.lf_calf.Pos = 2.0f; /* 左前腿小腿位置 */
|
|
// c->output.named.rf_thigh.Pos = -1.0f; /* 右前腿大腿位置 */
|
|
// c->output.named.rf_calf.Pos = -2.0f; /* 右前腿小腿位置 */
|
|
// c->output.named.lr_thigh.Pos = 1.0f; /* 左后腿大腿位置 */
|
|
// c->output.named.lr_calf.Pos = 2.0f; /* 左后腿小腿位置 */
|
|
// c->output.named.rr_thigh.Pos = -1.0f; /* 右后腿大腿位置 */
|
|
// c->output.named.rr_calf.Pos = -2.0f; /* 右后腿小腿位置 */
|
|
c->output.named.lf_thigh.Pos = c->height * 5;
|
|
c->output.named.lf_calf.Pos = c->height * 10;
|
|
c->output.named.rf_thigh.Pos = -c->height * 5;
|
|
c->output.named.rf_calf.Pos = -c->height * 10;
|
|
c->output.named.lr_thigh.Pos = c->height * 5;
|
|
c->output.named.lr_calf.Pos = c->height * 10;
|
|
c->output.named.rr_thigh.Pos = -c->height * 5;
|
|
c->output.named.rr_calf.Pos = -c->height * 10;
|
|
chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
|
|
}
|
|
break;
|
|
}
|
|
c->output.named.lf_hip.Pos = 0.0f; /* 左前腿髋关节位置 */
|
|
c->output.named.lr_hip.Pos = 0.0f; /* 左后腿髋关节位置 */
|
|
c->output.named.rf_hip.Pos = 0.0f; /* 右前腿髋关节位置 */
|
|
c->output.named.rr_hip.Pos = 0.0f; /* 右后腿髋关节位置 */
|
|
|
|
c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
|
|
c->output.named.lf_thigh.Pos = c->height * 5;
|
|
c->output.named.lf_calf.Pos = c->height * 10;
|
|
c->output.named.rf_thigh.Pos = -c->height * 5;
|
|
c->output.named.rf_calf.Pos = -c->height * 10;
|
|
c->output.named.lr_thigh.Pos = c->height * 5;
|
|
c->output.named.lr_calf.Pos = c->height * 10;
|
|
c->output.named.rr_thigh.Pos = -c->height * 5;
|
|
c->output.named.rr_calf.Pos = -c->height * 10;
|
|
chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
|
|
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
float target_pose[3] = {0.0,0.0861,-c->height}; /* 目标位置 */
|
|
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]; /* 小腿位置 */
|
|
}
|
|
|
|
// float target_pose[3] = {c_cmd->ctrl_vec.vx/10.0f, c_cmd->ctrl_vec.vy/10.0f, -0.2f}; /* 目标位置 */
|
|
// float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
|
|
// float angle_pose[3];
|
|
// Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[0]); /* 逆运动学计算 */
|
|
// c->output.named.lf_hip.Pos = angle_pose[0]; /* 左前腿髋关节位置 */
|
|
// c->output.named.lf_thigh.Pos = angle_pose[1]; /* 左前腿大腿位置 */
|
|
// c->output.named.lf_calf.Pos = angle_pose[2]; /* 左前腿小腿位置 */
|
|
break;
|
|
}
|
|
|
|
// Limit_ChassicOutput(&c->feedback, &c->output, 0.5, 0.0, 0.0);
|
|
for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
|
|
/* 限制输出 */
|
|
Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
|
|
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]);
|
|
}
|
|
|
|
}
|
|
|
|
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]);
|
|
}
|
|
}
|