171 lines
4.8 KiB
C
171 lines
4.8 KiB
C
#include "main.h"
|
||
#include "M3508.h"
|
||
#include "can.h"
|
||
#include "gpio.h"
|
||
#include <string.h>
|
||
#include <stdlib.h>
|
||
#include <math.h> // 用于isnan、isinf函数
|
||
#include "PID.h"
|
||
#include "Remote.h"
|
||
#include "CToC.h"
|
||
|
||
|
||
// 四个电机的控制器
|
||
Motor_Controller_t motor_controller[4] = {0};
|
||
// 存储四个电机的控制命令
|
||
int16_t motor_commands[4] = {0};
|
||
|
||
motor_measure_t motor_chassis[4] = {0};
|
||
|
||
static CAN_TxHeaderTypeDef chassis_tx_message;
|
||
static uint8_t chassis_can_send_data[8];
|
||
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
||
{
|
||
uint32_t send_mail_box;
|
||
chassis_tx_message.StdId = 0x200;
|
||
chassis_tx_message.IDE = CAN_ID_STD;
|
||
chassis_tx_message.RTR = CAN_RTR_DATA;
|
||
chassis_tx_message.DLC = 0x08;
|
||
chassis_can_send_data[0] = motor1 >> 8;
|
||
chassis_can_send_data[1] = motor1;
|
||
chassis_can_send_data[2] = motor2 >> 8;
|
||
chassis_can_send_data[3] = motor2;
|
||
chassis_can_send_data[4] = motor3 >> 8;
|
||
chassis_can_send_data[5] = motor3;
|
||
chassis_can_send_data[6] = motor4 >> 8;
|
||
chassis_can_send_data[7] = motor4;
|
||
HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message,
|
||
chassis_can_send_data, &send_mail_box);
|
||
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 将RPM转换为rad/s
|
||
*/
|
||
float RPM_To_RadPerSec(int16_t rpm)
|
||
{
|
||
return (float)rpm * 2.0f * 3.1415926535f / 60.0f;
|
||
}
|
||
|
||
/**
|
||
* @brief 将rad/s转换为RPM
|
||
*/
|
||
int16_t RadPerSec_To_RPM(float rad_per_sec)
|
||
{
|
||
return (int16_t)(rad_per_sec * 60.0f / (2.0f * 3.1415926535f));
|
||
}
|
||
|
||
/**
|
||
* @brief 设置单个电机的目标速度
|
||
* @param motor_id: 电机ID (0-3)
|
||
* @param target_speed_rad_s: 目标速度 (rad/s)
|
||
*/
|
||
void Set_Motor_Target_Speed(uint8_t motor_id, float target_speed_rad_s)
|
||
{
|
||
if (motor_id < 4) {
|
||
motor_controller[motor_id].target_speed = target_speed_rad_s;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 电机速度控制任务（带数据验证）
|
||
*/
|
||
void Motor_Speed_Control_Task(float dt)
|
||
{
|
||
for (int i = 0; i < 4; i++) {
|
||
// 验证电机反馈数据
|
||
if (isnan(motor_controller[i].measure.speed_rpm) ||
|
||
motor_controller[i].measure.speed_rpm > 10000 ||
|
||
motor_controller[i].measure.speed_rpm < -10000) {
|
||
// 数据异常，使用默认值
|
||
motor_controller[i].measure.speed_rpm = 0;
|
||
}
|
||
|
||
// 将电机反馈的RPM转换为rad/s
|
||
float actual_speed_rads = RPM_To_RadPerSec(motor_controller[i].measure.speed_rpm);
|
||
|
||
// 验证转换结果
|
||
if (isnan(actual_speed_rads) || isinf(actual_speed_rads)) {
|
||
actual_speed_rads = 0.0f;
|
||
}
|
||
|
||
// 计算速度误差
|
||
float speed_error = motor_controller[i].target_speed - actual_speed_rads;
|
||
|
||
// 验证误差值
|
||
if (isnan(speed_error) || isinf(speed_error)) {
|
||
speed_error = 0.0f;
|
||
}
|
||
|
||
// PID计算
|
||
float pid_output = PID_Calculate(&motor_controller[i].speed_pid, speed_error, dt);
|
||
|
||
// 验证PID输出
|
||
if (isnan(pid_output) || isinf(pid_output)) {
|
||
pid_output = 0.0f;
|
||
// 重置这个电机的PID状态
|
||
PID_PositionClean(&motor_controller[i].speed_pid);
|
||
}
|
||
|
||
// 存储控制命令
|
||
motor_commands[i] = (int16_t)pid_output;
|
||
|
||
|
||
}
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 麦克纳姆轮速度解算函数
|
||
* @param vx: X方向速度 (m/s) - 正数前进，负数后退
|
||
* @param vy: Y方向速度 (m/s) - 正数右移，负数左移
|
||
* @param vz: 旋转角速度 (rad/s) - 正数逆时针，负数顺时针
|
||
*/
|
||
void Drive_Motor(float vx, float vy, float vz)
|
||
{
|
||
float A = WHEELBASE_WIDTH + WHEELBASE_LENGTH;
|
||
|
||
// 标准麦克纳姆轮运动学逆解算公式
|
||
// 轮子编号：0-右上, 1-左上, 2-左下, 3-右下
|
||
|
||
// 标准麦克纳姆轮运动学逆解算公式
|
||
float wheel1_speed = ( vx +vy + vz * A ) / WHEEL_RADIUS; // 右上轮
|
||
float wheel2_speed = ( vx - vy + vz * A ) / WHEEL_RADIUS; // 左上轮
|
||
float wheel3_speed = (-vx -vy + vz * A ) / WHEEL_RADIUS; // 左下轮
|
||
float wheel4_speed = (-vx +vy +vz * A ) / WHEEL_RADIUS; // 右下轮
|
||
|
||
Set_Motor_Target_Speed(0, wheel1_speed); // 右上轮
|
||
Set_Motor_Target_Speed(1, wheel2_speed); // 左上轮
|
||
Set_Motor_Target_Speed(2, wheel3_speed); // 左下轮
|
||
Set_Motor_Target_Speed(3, wheel4_speed); // 右下轮
|
||
}
|
||
|
||
|
||
float Map_Remote_to_Speed(int16_t input, float max_speed)
|
||
{
|
||
if (input > 660) input = 660;
|
||
if (input < -660) input = -660;
|
||
|
||
// 死区内直接返回0
|
||
if (abs(input) < 50) {
|
||
return 0.0f;
|
||
}
|
||
|
||
// 不在死区内，正常计算速度
|
||
return (input / 660.0f) * max_speed;
|
||
}
|
||
/**
|
||
* @brief 处理遥控器数据并设置底盘速度
|
||
*/
|
||
void Process_Remote_Control(void)
|
||
{
|
||
// 映射遥控器值到速度 (m/s)
|
||
float vx = Map_Remote_to_Speed(Remote_RxData.Remote_L_UD,2.0f); // 前后方向，最大2m/s
|
||
float vy = Map_Remote_to_Speed(Remote_RxData.Remote_L_RL, 2.0f); // 左右方向，最大2m/s
|
||
float vz = Map_Remote_to_Speed(Remote_RxData.Remote_R_RL, 3.0f); // 旋转方向，最大3rad/s
|
||
|
||
// 调用运动学解算
|
||
Drive_Motor(vx, vy, vz);
|
||
}
|