354 lines
10 KiB
C
354 lines
10 KiB
C
/*
|
||
底盘模组
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "cmsis_os2.h"
|
||
#include <stdlib.h>
|
||
#include "bsp/mm.h"
|
||
#include "bsp/can.h"
|
||
#include "component/ahrs.h"
|
||
#include "device/motor_rm.h"
|
||
#include "device/motor.h"
|
||
#include "module/chassis.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下限 */
|
||
#define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上限 */
|
||
#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) /* 35°对应的弧度值 */
|
||
#define CHASSIS_ROTOR_OMEGA 0.001
|
||
|
||
|
||
|
||
static int8_t Chassis_SetMode(Chassis_t *c, 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;
|
||
}
|
||
|
||
/**
|
||
* @brief 产生小陀螺wz随机速度
|
||
*
|
||
* @param min wz产生最小速度
|
||
* @param max wz产生最大速度
|
||
* @param now ctrl_chassis的tick数
|
||
* @return float
|
||
*/
|
||
static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
|
||
/* wz在min和max之间,上限0.6f */
|
||
float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
|
||
return (wz_vary > max) ? max : wz_vary;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 底盘初始化
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param param 包含底盘参数的结构体指针
|
||
* \param target_freq 任务预期的运行频率
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
|
||
AHRS_Eulr_t *mech_zero, float target_freq) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
c->param = param; /* 初始化参数 */
|
||
c->mode = CHASSIS_MODE_RELAX; /* 设置上电后底盘默认模式 */
|
||
c->mech_zero = mech_zero; /* 设置底盘机械零点 */
|
||
|
||
/* 如果电机反装重新计算机械零点 */
|
||
if (param->reverse.yaw) CircleReverse(&(c->mech_zero->yaw));
|
||
|
||
/* 根据参数(param)中的底盘型号初始化Mixer */
|
||
Mixer_Mode_t mixer_mode;
|
||
switch (c->param->type) {
|
||
case CHASSIS_TYPE_MECANUM:
|
||
c->num_wheel = 4;
|
||
mixer_mode = MIXER_MECANUM;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_PARLFIX4:
|
||
c->num_wheel = 4;
|
||
mixer_mode = MIXER_PARLFIX4;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_PARLFIX2:
|
||
c->num_wheel = 2;
|
||
mixer_mode = MIXER_PARLFIX2;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_OMNI_CROSS:
|
||
c->num_wheel = 4;
|
||
mixer_mode = MIXER_OMNICROSS;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_OMNI_PLUS:
|
||
c->num_wheel = 4;
|
||
mixer_mode = MIXER_OMNIPLUS;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_SINGLE:
|
||
c->num_wheel = 1;
|
||
mixer_mode = MIXER_SINGLE;
|
||
break;
|
||
|
||
case CHASSIS_TYPE_DRONE:
|
||
/* onboard sdk. */
|
||
return CHASSIS_ERR_TYPE;
|
||
}
|
||
|
||
/* 根据底盘型号动态分配控制时的变量 */
|
||
c->feedback.motor_rpm =
|
||
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->feedback.motor_rpm));
|
||
if (c->feedback.motor_rpm == NULL) goto error; /* 变量未分配，返回错误 */
|
||
|
||
c->feedback.motor_current =
|
||
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->feedback.motor_current));
|
||
if (c->feedback.motor_current == NULL) goto error;
|
||
|
||
c->setpoint.motor_rpm =
|
||
BSP_Malloc((size_t)c->num_wheel * sizeof(*c->setpoint.motor_rpm));
|
||
if (c->setpoint.motor_rpm == NULL) goto error;
|
||
|
||
c->pid.motor = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->pid.motor));
|
||
if (c->pid.motor == NULL) goto error;
|
||
|
||
c->out = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->out));
|
||
if (c->out == NULL) goto error;
|
||
|
||
c->filter.in = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->filter.in));
|
||
if (c->filter.in == NULL) goto error;
|
||
|
||
c->filter.out = BSP_Malloc((size_t)c->num_wheel * sizeof(*c->filter.out));
|
||
if (c->filter.out == NULL) goto error;
|
||
|
||
/* 初始化轮子电机控制PID和LPF */
|
||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||
PID_Init(c->pid.motor + i, KPID_MODE_NO_D, target_freq,
|
||
&(c->param->motor_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 */
|
||
PID_Init(&(c->pid.follow), KPID_MODE_NO_D, target_freq,
|
||
&(c->param->follow_pid_param));
|
||
|
||
Mixer_Init(&(c->mixer), mixer_mode); /* 初始化混合器 */
|
||
return CHASSIS_OK;
|
||
|
||
error:
|
||
/* 动态内存分配错误时，释放已经分配的内存，返回错误值 */
|
||
BSP_Free(c->feedback.motor_rpm);
|
||
BSP_Free(c->setpoint.motor_rpm);
|
||
BSP_Free(c->pid.motor);
|
||
BSP_Free(c->out);
|
||
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 Chassis_CAN_t *can) {
|
||
/*底盘数据和CAN结构体不能为空*/
|
||
if (c == NULL) return CHASSIS_ERR_NULL;
|
||
if (can == NULL) return CHASSIS_ERR_NULL;
|
||
|
||
/* 如果电机反装重新计算正确的反馈值 */
|
||
if (c->param->reverse.yaw) {
|
||
c->feedback.gimbal_yaw_encoder =
|
||
-MOTOR_GetRotorAbsAngle(&can->gimbal.named.yaw.motor) + M_2PI;
|
||
} else {
|
||
c->feedback.gimbal_yaw_encoder =
|
||
MOTOR_GetRotorAbsAngle(&can->gimbal.named.yaw.motor);
|
||
}
|
||
|
||
/* 将CAN中的反馈数据写入到feedback中 */
|
||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||
const MOTOR_RM_t *m = &can->chassis.as_array[i];
|
||
if (m != NULL) {
|
||
c->feedback.motor_rpm[i] = m->feedback.rotor_speed;
|
||
c->feedback.motor_current[i] = m->feedback.torque_current;
|
||
} else {
|
||
c->feedback.motor_rpm[i] = 0.0f;
|
||
c->feedback.motor_current[i] = 0.0f;
|
||
}
|
||
}
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
/**
|
||
* \brief 运行底盘控制逻辑
|
||
*
|
||
* \param c 包含底盘数据的结构体
|
||
* \param c_cmd 底盘控制指令
|
||
* \param dt_sec 两次调用的时间间隔
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Chassis_Control(Chassis_t *c, const 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->last_wakeup) / 1000.0f;
|
||
c->last_wakeup = now;
|
||
|
||
/* 根据遥控器命令更改底盘模式 */
|
||
Chassis_SetMode(c, c_cmd->mode, now);
|
||
|
||
/* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */
|
||
/*计算vx和vy */
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_BREAK: /* 刹车模式电机停止 */
|
||
c->move_vec.vx = 0.0f;
|
||
c->move_vec.vy = 0.0f;
|
||
break;
|
||
|
||
case CHASSIS_MODE_INDEPENDENT: /* 独立模式控制向量与运动向量相等 */
|
||
c->move_vec.vx = c_cmd->ctrl_vec.vx;
|
||
c->move_vec.vy = c_cmd->ctrl_vec.vy;
|
||
break;
|
||
|
||
case CHASSIS_MODE_OPEN:
|
||
case CHASSIS_MODE_RELAX:
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL: /* 按照云台方向换算运动向量 */
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
|
||
case CHASSIS_MODE_ROTOR: {
|
||
float beta = c->feedback.gimbal_yaw_encoder - c->mech_zero->yaw;
|
||
float cos_beta = cosf(beta);
|
||
float sin_beta = sinf(beta);
|
||
c->move_vec.vx =
|
||
cos_beta * c_cmd->ctrl_vec.vx - sin_beta * c_cmd->ctrl_vec.vy;
|
||
c->move_vec.vy =
|
||
sin_beta * c_cmd->ctrl_vec.vx + cos_beta * c_cmd->ctrl_vec.vy;
|
||
}
|
||
}
|
||
|
||
/* 计算wz */
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX:
|
||
case CHASSIS_MODE_BREAK:
|
||
case CHASSIS_MODE_INDEPENDENT: /* 独立模式wz是0 */
|
||
c->move_vec.wz = 0.0f;
|
||
break;
|
||
|
||
case CHASSIS_MODE_OPEN:
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台 */
|
||
c->move_vec.wz = PID_Calc(&(c->pid.follow), c->mech_zero->yaw,
|
||
c->feedback.gimbal_yaw_encoder, 0.0f, c->dt);
|
||
break;
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
|
||
c->move_vec.wz =
|
||
PID_Calc(&(c->pid.follow), c->mech_zero->yaw + M_7OVER72PI,
|
||
c->feedback.gimbal_yaw_encoder, 0.0f, c->dt);
|
||
break;
|
||
case CHASSIS_MODE_ROTOR: { /* 小陀螺模式使得底盘以一定速度旋转 */
|
||
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,
|
||
CHASSIS_ROTOR_WZ_MAX, now);
|
||
}
|
||
}
|
||
|
||
/* move_vec -> motor_rpm_set. 通过运动向量计算轮子转速目标值 */
|
||
Mixer_Apply(&(c->mixer), &(c->move_vec), c->setpoint.motor_rpm, c->num_wheel,
|
||
7000.0f);
|
||
|
||
/* 根据轮子转速目标值，利用PID计算电机输出值 */
|
||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||
/* 输入滤波 */
|
||
c->feedback.motor_rpm[i] =
|
||
LowPassFilter2p_Apply(c->filter.in + i, c->feedback.motor_rpm[i]);
|
||
|
||
/* 根据底盘模式计算输出值 */
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_BREAK:
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL:
|
||
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
|
||
case CHASSIS_MODE_ROTOR:
|
||
case CHASSIS_MODE_INDEPENDENT: /* 独立模式,受PID控制 */
|
||
c->out[i] = PID_Calc(c->pid.motor + i, c->setpoint.motor_rpm[i],
|
||
c->feedback.motor_rpm[i], 0.0f, c->dt);
|
||
break;
|
||
|
||
case CHASSIS_MODE_OPEN: /* 开环模式,不受PID控制 */
|
||
c->out[i] = c->setpoint.motor_rpm[i] / 9000.0f;
|
||
break;
|
||
|
||
case CHASSIS_MODE_RELAX: /* 放松模式,不输出 */
|
||
c->out[i] = 0;
|
||
break;
|
||
}
|
||
/* 输出滤波 */
|
||
c->out[i] = LowPassFilter2p_Apply(c->filter.out + i, c->out[i]);
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
/**
|
||
* \brief 复制底盘输出值
|
||
*
|
||
* \param s 包含底盘数据的结构体
|
||
* \param out CAN设备底盘输出结构体
|
||
*/
|
||
void Chassis_DumpOutput(Chassis_t *c, Chassis_CAN_t *can)
|
||
{
|
||
if (c == NULL || can == NULL) return;
|
||
|
||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||
MOTOR_RM_t *m = &can->chassis.as_array[i];
|
||
MOTOR_RM_SetOutput(&m->param, c->out[i]);
|
||
}
|
||
}
|
||
|
||
|
||
/**
|
||
* \brief Chassis数据resetoutput
|
||
*
|
||
*
|
||
*/
|
||
void Chassis_ResetOutput(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
for (uint8_t i = 0; i < c->num_wheel; i++) {
|
||
MOTOR_RM_Param_t *m_param = c->motors_param[i];
|
||
if (m_param) {
|
||
MOTOR_RM_Relax(m_param);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|