104 lines
2.8 KiB
C
104 lines
2.8 KiB
C
#include "can_driver.h"
|
||
|
||
#include <string.h>
|
||
|
||
extern CAN_HandleTypeDef hcan1;
|
||
M2006_Motor_t Motor[MOTOR_NUM];
|
||
/**
|
||
* @brief CAN 初始化，滤波器，启动 CAN 并激活中断
|
||
*/
|
||
void CAN_Driver_Init(void)
|
||
{
|
||
// 启动 CAN
|
||
HAL_CAN_Start(&hcan1);
|
||
|
||
// M2006 电机 CAN 滤波器配置
|
||
CAN_FilterTypeDef sFilterConfig;
|
||
sFilterConfig.FilterBank = 0;
|
||
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
|
||
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
|
||
|
||
// 配置 ID 和 Mask (接收 0x201 - 0x208)
|
||
sFilterConfig.FilterIdHigh = 0x0402;
|
||
sFilterConfig.FilterIdLow = 0x0000;
|
||
sFilterConfig.FilterMaskIdHigh = 0x040F;
|
||
sFilterConfig.FilterMaskIdLow = 0x0000;
|
||
sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||
sFilterConfig.FilterActivation = ENABLE;
|
||
|
||
|
||
// 激活 CAN 接收 FIFO0 的中断
|
||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||
}
|
||
/**
|
||
* @brief CAN 发送函数
|
||
* @param std_id: 标准 ID
|
||
* @param TxData: 8 字节发送数据
|
||
* @param len: 数据长度 (对于 M2006 为 8)
|
||
*/
|
||
void CAN_Driver_Transmit(uint16_t std_id, uint8_t TxData[], uint8_t len)
|
||
{
|
||
CAN_TxHeaderTypeDef TxHeader;
|
||
uint32_t TxMailbox;
|
||
|
||
TxHeader.StdId = std_id;
|
||
TxHeader.RTR = CAN_RTR_DATA;
|
||
TxHeader.IDE = CAN_ID_STD;
|
||
TxHeader.DLC = len;
|
||
|
||
HAL_CAN_AddTxMessage(&hcan1, &TxHeader, TxData, &TxMailbox);
|
||
}
|
||
|
||
void Motor_Control_Init(void)
|
||
{
|
||
memset(Motor, 0, sizeof(Motor));//Motor各位置全部清零
|
||
Motor[0].Target_RPM = 0.0f; //第一个电机目标转速设0，确保开始时为静止
|
||
}
|
||
|
||
void CAN_Rx_Callback(uint16_t std_id, uint8_t RxData[])
|
||
{
|
||
// M2006 反馈 ID 范围: 0x201 到 0x204 (对应 ID 1-4)
|
||
if (std_id >= 0x201 && std_id <= (0x201 + MOTOR_NUM - 1))
|
||
{
|
||
|
||
uint8_t motor_index = std_id - 0x201;
|
||
|
||
if (motor_index < MOTOR_NUM)
|
||
{
|
||
M2006_Motor_t *m = &Motor[motor_index];
|
||
// 解析数据 (大端序)
|
||
m->rotor_angle = (uint16_t)(RxData[0] << 8 | RxData[1]);
|
||
m->speed_rpm = (int16_t)(RxData[2] << 8 | RxData[3]);
|
||
m->actual_current = (int16_t)(RxData[4] << 8 | RxData[5]);
|
||
m->temp = RxData[6];
|
||
}
|
||
}
|
||
}
|
||
|
||
void Motor_Control_Loop(void)
|
||
{
|
||
uint8_t TxData[8];
|
||
int16_t Motor1_Target_Current = 1000;
|
||
uint8_t len = 8; // 数据长度固定为 8
|
||
|
||
|
||
Motor[1].target_current = Motor1_Target_Current ;
|
||
// 电机 1: 1000 (0x03E8)
|
||
TxData[0] = (uint8_t)(Motor[1].target_current >> 8); // 高 8 位: 0x03
|
||
TxData[1] = (uint8_t)(Motor[1].target_current & 0xFF); // 低 8 位: 0xE8
|
||
|
||
|
||
TxData[2] = 0;
|
||
TxData[3] = 0;
|
||
TxData[4] = 0;
|
||
TxData[5] = 0;
|
||
TxData[6] = 0;
|
||
TxData[7] = 0;
|
||
|
||
|
||
|
||
|
||
// 3. 调用 CAN 驱动发送控制指令
|
||
CAN_Driver_Transmit(0x200, TxData, 8);
|
||
}
|