/* * 底盘模组 */ /* Includes ----------------------------------------------------------------- */ #include "chassis.h" #include #include "bsp\mm.h" #include "cmsis_os2.h" #include "component\limiter.h" #include "device\can.h" #include "module\cap.h" /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ #define _CAP_PERCENTAGE_WORK 100 /* 底盘不再限制功率的电容电量 */ #define _CAP_PERCENTAGE_CHARGE 30 /* 电容开始工作的电容电量 */ #define CHASSIS_MAX_CAP_POWER 100 /* 电容能够提供的最大功率 */ #define CHASSIS_ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */ #define CHASSIS_ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */ #define M_7OVER72PI (M_2PI * 7.0f / 72.0f) /* 三十五度对应弧度值 */ // #define CHASSIS_ROTOR_OMEGA 0.15f /* 小陀螺转动频率 */ // 随机产生小陀螺转动频率,范围0.001-0.05 //#define CHASSIS_ROTOR_OMEGA rand() % 50 / 10000.0f + 0.001f #define CHASSIS_ROTOR_OMEGA 0.001 /* Private macro ------------------------------------------------------------ */ /* 保证电容电量宏定义在正确范围内 */ #if ((_CAP_PERCENTAGE_WORK < 0) || (_CAP_PERCENTAGE_WORK > 100) || \ (_CAP_PERCENTAGE_CHARGE < 0) || (_CAP_PERCENTAGE_CHARGE > 100)) #error "Cap percentage should be in the range from 0 to 100." #endif /* 保证电容功率宏定义在正确范围内 */ #if ((CHASSIS_MAX_CAP_POWER < 60) || (CHASSIS_MAX_CAP_POWER > 200)) #error "The capacitor power should be in in the range from 60 to 200." #endif /* Private variables -------------------------------------------------------- */ static const float CAP_PERCENTAGE_WORK = (float)_CAP_PERCENTAGE_WORK / 100.0f; static const float CAP_PERCENTAGE_CHARGE = (float)_CAP_PERCENTAGE_CHARGE / 100.0f; /* 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; } /** * @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 = fabs(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min; return wz_vary > 0.8f ? 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 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 = -can->motor.gimbal.named.yaw.rotor_angle + M_2PI; } else { c->feedback.gimbal_yaw_encoder = can->motor.gimbal.named.yaw.rotor_angle; } /* 将CAN中的反馈数据写入到feedback中 */ for (uint8_t i = 0; i < c->num_wheel; i++) { c->feedback.motor_rpm[i] = can->motor.chassis.as_array[i].rotor_speed; c->feedback.motor_current[i] = can->motor.chassis.as_array[i].torque_current; } return CHASSIS_OK; } /** * \brief 运行底盘控制逻辑 * * \param c 包含底盘数据的结构体 * \param c_cmd 底盘控制指令 * \param dt_sec 两次调用的时间间隔 * * \return 函数运行结果 */ 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; /* 根据遥控器命令更改底盘模式 */ 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_INDENPENDENT: /* 独立模式控制向量与运动向量相等 */ 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_INDENPENDENT: /* 独立模式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_INDENPENDENT: /* 独立模式,受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 c 底盘数据 * @param cap 电容数据 * @param ref 裁判系统数据 * @return 函数运行结果 */ int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap, const Referee_ForChassis_t *ref) { float power_limit = 0.0f; if (ref->ref_status != REF_STATUS_RUNNING) { /* 裁判系统离线,将功率限制为固定值 */ power_limit = CHASSIS_POWER_MAX_WITHOUT_REF; } else { if (cap->cap_status == CAN_CAP_STATUS_RUNNING && cap->percentage > CAP_PERCENTAGE_CHARGE) { /* 电容在线且电量足够,使用电容 */ if (cap->percentage > CAP_PERCENTAGE_WORK) { /* 电容接近充满时不再限制功率 */ power_limit = -1.0f; } else { /* 按照电容能量百分比计算输出功率 */ power_limit = ref->chassis_power_limit + (cap->percentage - CAP_PERCENTAGE_CHARGE) / (CAP_PERCENTAGE_WORK - CAP_PERCENTAGE_CHARGE) * (float)CHASSIS_MAX_CAP_POWER; } } else { /* 电容不在工作,根据缓冲能量计算输出功率限制 */ power_limit = PowerLimit_TargetPower(ref->chassis_power_limit, ref->chassis_pwr_buff); } } /* 应用功率限制 */ PowerLimit_ChassicOutput(power_limit, c->out, c->feedback.motor_rpm, c->num_wheel); return CHASSIS_OK; } /** * \brief 复制底盘输出值 * * \param s 包含底盘数据的结构体 * \param out CAN设备底盘输出结构体 */ void Chassis_DumpOutput(Chassis_t *c, CAN_ChassisOutput_t *out) { for (uint8_t i = 0; i < c->num_wheel; i++) { out->as_array[i] = c->out[i]; } } /** * \brief 清空Chassis输出数据 * * \param out CAN设备底盘输出结构体 */ void Chassis_ResetOutput(CAN_ChassisOutput_t *out) { for (uint8_t i = 0; i < 4; i++) { out->as_array[i] = 0.0f; } } /** * @brief 导出底盘数据 * * @param chassis 底盘数据结构体 * @param ui UI数据结构体 */ void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui) { ui->mode = c->mode; ui->angle = c->feedback.gimbal_yaw_encoder - c->mech_zero->yaw; }