127 lines
4.5 KiB
C
127 lines
4.5 KiB
C
#include "chassis_control.h"
|
||
#include "CAN_receive.h"
|
||
#include "remote_control.h"
|
||
#include <math.h>
|
||
#include <string.h>
|
||
|
||
#define PI 3.1415926535f
|
||
|
||
static pid_t pid_motor[4]; // 四个底盘电机的PID控制器
|
||
static int16_t target_speed[4]; // 四个轮子的目标转速(rpm)
|
||
static Chassis_Speed chassis_speed; // 底盘目标速度(vx,vy,vw)
|
||
static ChassisMode chassis_mode = CHASSIS_NORMAL; // 底盘当前模式
|
||
|
||
static void omni_calc(Chassis_Speed *speed, int16_t *out_speed);
|
||
static void absolute_cal(Chassis_Speed *absolute_speed, float angle);
|
||
|
||
/**
|
||
* @brief 初始化底盘电机的PID
|
||
*/
|
||
void chassis_control_init(void)
|
||
{
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
pid_init(&pid_motor[i], 15.0f, 0.1f, 2.0f, 4000.0f, 1000.0f);
|
||
}
|
||
memset(target_speed, 0, sizeof(target_speed));
|
||
memset(&chassis_speed, 0, sizeof(chassis_speed));
|
||
}
|
||
|
||
/**
|
||
* @brief 设置底盘目标速度（vx, vy, vw）
|
||
*/
|
||
void chassis_set_target_speed(const Chassis_Speed *speed)
|
||
{
|
||
if (speed == NULL) return;
|
||
chassis_speed.vx = speed->vx;
|
||
chassis_speed.vy = speed->vy;
|
||
chassis_speed.vw = speed->vw;
|
||
}
|
||
|
||
/**
|
||
* @brief 设置底盘模式
|
||
*/
|
||
void chassis_set_mode(ChassisMode mode)
|
||
{
|
||
chassis_mode = mode;
|
||
}
|
||
|
||
/**
|
||
* @brief 底盘控制主循环：全向轮运动学解算 + PID + CAN发送
|
||
*/
|
||
void chassis_control_task(void)
|
||
{
|
||
// 遥控器掉线检测：超过100ms无信号，停止CAN发送，电机随惯性停下
|
||
if (!is_rc_online())
|
||
{
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
pid_motor[i].integral = 0; // 清空PID积分，防止重新上线时积分饱和
|
||
pid_motor[i].last_error = 0; // 清空上次误差
|
||
}
|
||
memset(target_speed, 0, sizeof(target_speed)); // 目标转速清零
|
||
return; // 不发送CAN信号，电机随惯性停下
|
||
}
|
||
const motor_measure_t *m[4];
|
||
m[0] = get_chassis_motor_measure_point(0);
|
||
m[1] = get_chassis_motor_measure_point(1);
|
||
m[2] = get_chassis_motor_measure_point(2);
|
||
m[3] = get_chassis_motor_measure_point(3);
|
||
|
||
absolute_cal(&chassis_speed, 0);
|
||
|
||
int16_t cur[4];
|
||
for (int i = 0; i < 4; i++)
|
||
{
|
||
cur[i] = (int16_t)pid_calc(&pid_motor[i], (float)target_speed[i], (float)m[i]->speed_rpm);
|
||
}
|
||
|
||
CAN_cmd_chassis(cur[0], cur[1], cur[2], cur[3]);
|
||
}
|
||
|
||
/**
|
||
* @brief 全向轮运动学解算：将 vx,vy,vw 转换为四个轮子的转速
|
||
* 轮子编号：0=左, 1=前, 2=右, 3=后
|
||
*/
|
||
static void omni_calc(Chassis_Speed *speed, int16_t *out_speed)
|
||
{
|
||
float wheel_rpm_ratio; // 速度(m/s)到转速(rpm)的转换系数
|
||
float L; // 底盘中心到轮子的距离 = LENGTH_A + LENGTH_B
|
||
int16_t wheel_rpm[4];
|
||
|
||
// 60 / 轮周长 * 减速比 = 转速转换系数 (m/s → rpm)
|
||
// 例如：vx=1m/s → 轮子60/0.152/19=7500rpm(电机端)
|
||
wheel_rpm_ratio = 60.0f / WHEEL_PERIMETER * CHASSIS_DECELE_RATIO;
|
||
L = LENGTH_A + LENGTH_B;
|
||
|
||
// 轮子编号：基于用户观察修正
|
||
// M1(0x201)=左前, M2(0x202)=左后, M3(0x203)=右前, M4(0x204)=右后
|
||
// 物理层vy符号：+, -, -, + 是X型麦轮的标准平移公式（右侧电机反向安装，代码层vy: +, -, +, -）
|
||
// 四个轮子对角配对：左前+右后同向，右前+左后同向，两组反向
|
||
wheel_rpm[0] = (int16_t)(( speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左前轮
|
||
wheel_rpm[1] = (int16_t)(( speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 左后轮
|
||
wheel_rpm[2] = (int16_t)((-speed->vx + speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右前轮
|
||
wheel_rpm[3] = (int16_t)((-speed->vx - speed->vy + speed->vw * L) * wheel_rpm_ratio); // 右后轮
|
||
|
||
memcpy(out_speed, wheel_rpm, 4 * sizeof(int16_t));
|
||
}
|
||
|
||
/**
|
||
* @brief 坐标系变换：将遥控器坐标系的速度转换到底盘坐标系
|
||
* @param absolute_speed 遥控器输入的目标速度
|
||
* @param angle 云台相对于底盘的角度（度）
|
||
*/
|
||
static void absolute_cal(Chassis_Speed *absolute_speed, float angle)
|
||
{
|
||
float angle_hd; // 角度转弧度
|
||
Chassis_Speed temp_speed; // 旋转后的临时速度
|
||
|
||
angle_hd = angle * PI / 180.0f; // 角度转弧度
|
||
|
||
// 坐标系旋转矩阵：将遥控器坐标系速度旋转到当前底盘坐标系
|
||
temp_speed.vw = absolute_speed->vw;
|
||
temp_speed.vx = absolute_speed->vx * cosf(angle_hd) - absolute_speed->vy * sinf(angle_hd);
|
||
temp_speed.vy = absolute_speed->vx * sinf(angle_hd) + absolute_speed->vy * cosf(angle_hd);
|
||
|
||
omni_calc(&temp_speed, target_speed); // 全向轮解算，结果存入target_speed
|
||
} |