210 lines
7.6 KiB
C
210 lines
7.6 KiB
C
#include "module/balance_chassis.h"
|
||
#include "bsp/time.h"
|
||
#include "bsp/can.h"
|
||
#include <math.h>
|
||
|
||
|
||
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
|
||
if (c == NULL) return CHASSIS_ERR_NULL; /* 主结构体不能为空 */
|
||
if (mode == c->mode) return CHASSIS_OK; /* 模式未改变直接返回 */
|
||
|
||
PID_Reset(&c->pid.left_wheel);
|
||
PID_Reset(&c->pid.right_wheel);
|
||
PID_Reset(&c->pid.follow);
|
||
PID_Reset(&c->pid.balance);
|
||
c->mode = mode;
|
||
c->state = 0;
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
|
||
int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq){
|
||
if (c == NULL || param == NULL || target_freq <= 0.0f) {
|
||
return -1; // 参数错误
|
||
}
|
||
c->param = param;
|
||
/*初始化can*/
|
||
BSP_CAN_Init();
|
||
|
||
/*初始化和注册所有电机*/
|
||
MOTOR_LZ_Init();
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Register(&c->param->joint_motors[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_Register(&c->param->wheel_motors[i]);
|
||
}
|
||
/*初始化pid*/
|
||
PID_Init(&c->pid.left_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||
PID_Init(&c->pid.right_wheel, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, ¶m->follow_pid_param);
|
||
PID_Init(&c->pid.balance, KPID_MODE_CALC_D, target_freq, ¶m->motor_pid_param);
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
int8_t Chassis_UpdateFeedback(Chassis_t *c){
|
||
if (c == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
/*更新电机反馈*/
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Update(&c->param->joint_motors[i]);
|
||
}
|
||
MOTOR_LK_Update(&c->param->wheel_motors[0]);
|
||
MOTOR_LK_Update(&c->param->wheel_motors[1]);
|
||
|
||
/*将电机反馈数据赋值到标准反馈结构体，并检查是否离线*/
|
||
// 更新关节电机反馈并检查离线状态
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_t *joint_motor = MOTOR_LZ_GetMotor(&c->param->joint_motors[i]);
|
||
if (joint_motor != NULL) {
|
||
c->feedback.joint[i] = joint_motor->motor.feedback;
|
||
}
|
||
}
|
||
|
||
// 更新轮子电机反馈并检查离线状态
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_t *wheel_motor = MOTOR_LK_GetMotor(&c->param->wheel_motors[i]);
|
||
if (wheel_motor != NULL) {
|
||
c->feedback.wheel[i] = wheel_motor->motor.feedback;
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
int8_t Chassis_UpdateIMU(Chassis_t *c, const Chassis_IMU_t imu){
|
||
if (c == NULL) {
|
||
return -1; // 参数错误
|
||
}
|
||
c->feedback.imu = imu;
|
||
return 0;
|
||
}
|
||
|
||
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
|
||
if (c == NULL || c_cmd == NULL) {
|
||
return CHASSIS_ERR_NULL; // 参数错误
|
||
}
|
||
c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f; /* 计算两次调用的时间间隔，单位秒 */
|
||
c->lask_wakeup = BSP_TIME_Get_us();
|
||
|
||
/*设置底盘模式*/
|
||
if (Chassis_SetMode(c, c_cmd->mode) != CHASSIS_OK) {
|
||
return CHASSIS_ERR_MODE; // 设置模式失败
|
||
}
|
||
|
||
/*根据底盘模式执行不同的控制逻辑*/
|
||
switch (c->mode) {
|
||
case CHASSIS_MODE_RELAX:
|
||
// 放松模式，电机不输出
|
||
// for (int i = 0; i < 4; i++) {
|
||
// MOTOR_LZ_Relax(&c->param->joint_motors[i]);
|
||
// }
|
||
// for (int i = 0; i < 2; i++) {
|
||
// MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈
|
||
// }
|
||
MOTOR_LZ_Relax(&c->param->joint_motors[0]);
|
||
MOTOR_LZ_Relax(&c->param->joint_motors[1]);
|
||
MOTOR_LZ_Relax(&c->param->joint_motors[2]);
|
||
MOTOR_LZ_Relax(&c->param->joint_motors[3]);
|
||
MOTOR_LK_Relax(&c->param->wheel_motors[0]);
|
||
MOTOR_LK_Relax(&c->param->wheel_motors[1]);
|
||
break;
|
||
|
||
case CHASSIS_MODE_RECOVER:
|
||
switch (c->state) {
|
||
case 0:
|
||
//使能电机
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_Enable(&c->param->joint_motors[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
|
||
}
|
||
c->state += 1;
|
||
break;
|
||
case 1:
|
||
// 关节电机复位轮电机输出0；
|
||
for (int i = 0; i < 4; i++) {
|
||
MOTOR_LZ_RecoverToZero(&c->param->joint_motors[i]);
|
||
}
|
||
for (int i = 0; i < 2; i++) {
|
||
MOTOR_LK_Relax(&c->param->wheel_motors[i]);
|
||
}
|
||
break;
|
||
}
|
||
break;
|
||
|
||
case CHASSIS_MODE_WHELL_BALANCE:
|
||
{
|
||
// 只靠两轮平衡，关节电机锁死，通过pid实现倒立摆
|
||
// 锁定关节电机到固定位置（比如直立状态）
|
||
// for (int i = 0; i < 4; i++) {
|
||
// MOTOR_LZ_PositionControl(&c->param->joint_motors[i], 0.0f, 0.5f); // 目标位置0，最大速度0.5 rad/s
|
||
// }
|
||
|
||
// 双轮平衡控制
|
||
// 获取IMU pitch角度和角速度作为平衡反馈
|
||
float pitch_angle = c->feedback.imu.euler.pit;
|
||
float pitch_rate = c->feedback.imu.gyro.y; // pitch角速度
|
||
|
||
// 平衡PID计算 - 以直立为目标（0度）
|
||
float balance_output = PID_Calc(&c->pid.balance, 0.0f, pitch_angle, pitch_rate, c->dt);
|
||
|
||
// 前进后退控制（基于控制指令）
|
||
float forward_cmd = c_cmd->move_vec.vx; // 前进速度指令
|
||
|
||
// 转向控制（基于控制指令）
|
||
float turn_cmd = c_cmd->move_vec.wz; // 转向速度指令
|
||
|
||
// 计算左右轮速度设定值
|
||
c->setpoint.left_wheel = balance_output + forward_cmd - turn_cmd * 0.5f;
|
||
c->setpoint.right_wheel = balance_output + forward_cmd + turn_cmd * 0.5f;
|
||
|
||
// 左轮速度PID控制
|
||
float left_wheel_speed = c->feedback.wheel[0].rotor_speed;
|
||
float left_output = PID_Calc(&c->pid.left_wheel,
|
||
c->setpoint.left_wheel,
|
||
left_wheel_speed,
|
||
0.0f,
|
||
c->dt);
|
||
|
||
// 右轮速度PID控制
|
||
float right_wheel_speed = c->feedback.wheel[1].rotor_speed;
|
||
float right_output = PID_Calc(&c->pid.right_wheel,
|
||
c->setpoint.right_wheel,
|
||
right_wheel_speed,
|
||
0.0f,
|
||
c->dt);
|
||
|
||
// 限制输出范围
|
||
left_output = fmaxf(-1.0f, fminf(1.0f, left_output));
|
||
right_output = fmaxf(-1.0f, fminf(1.0f, right_output));
|
||
|
||
// 输出到电机
|
||
MOTOR_LK_SetOutput(&c->param->wheel_motors[0], left_output);
|
||
MOTOR_LK_SetOutput(&c->param->wheel_motors[1], right_output);
|
||
break;
|
||
}
|
||
|
||
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
||
// 轮子+腿平衡模式（暂时留空，后续实现）
|
||
break;
|
||
|
||
default:
|
||
return CHASSIS_ERR_MODE;
|
||
}
|
||
|
||
return CHASSIS_OK;
|
||
}
|
||
|
||
void Chassis_Output(Chassis_t *c) {
|
||
if (c == NULL) return;
|
||
|
||
// 这个函数已经在各个模式中直接调用了电机输出函数
|
||
// 如果需要统一输出，可以在这里实现
|
||
// 现在的设计是在控制逻辑中直接输出，所以这里留空
|
||
}
|