281 lines
8.0 KiB
C
281 lines
8.0 KiB
C
#include "odrive_can.h"
|
||
#include "can_it.h"
|
||
#include "main.h"
|
||
#include "can.h"
|
||
/******小端模式，低字节在前***/
|
||
/*********数据拆分************/
|
||
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
|
||
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
|
||
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
|
||
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))
|
||
|
||
//请在此处写明你所使用的can是can1还是can2
|
||
#define WHICH_CAN 2
|
||
//如果已经在其他地方完成了can初始化，则此处不进行can初始化
|
||
#define INITED 1
|
||
|
||
odrive_msg_t odrive_msg;
|
||
|
||
/**
|
||
* 用户自定义can1中断回调函数
|
||
*/
|
||
|
||
void User_Odrive_Can1_CB(void)
|
||
{
|
||
CAN_RxHeaderTypeDef rx_header;
|
||
uint8_t rx_data[8];
|
||
|
||
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &rx_header, rx_data);
|
||
switch (rx_header.StdId)
|
||
{
|
||
case (AXIS0_NODE << 5) | ODRIVE_ENCODER_VALUE:
|
||
odrive_msg.pos = rx_data[3] << 24 | rx_data[2] << 16 | rx_data[1] << 8 | rx_data[0];
|
||
odrive_msg.speed = rx_data[7] << 24 | rx_data[6] << 16 | rx_data[5] << 8 | rx_data[4];
|
||
break;
|
||
}
|
||
}
|
||
#if WHICH_CAN == 2
|
||
/**
|
||
* 用户自定义can2中断回调函数
|
||
*/
|
||
uint8_t rx_data[8];
|
||
void User_Odrive_Can2_CB(void)
|
||
{
|
||
CAN_RxHeaderTypeDef rx_header;
|
||
|
||
|
||
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &rx_header, rx_data);
|
||
switch (rx_header.StdId)
|
||
{
|
||
case (AXIS0_NODE << 5) | ODRIVE_ENCODER_VALUE:
|
||
odrive_msg.pos = rx_data[3] << 24 | rx_data[2] << 16 | rx_data[1] << 8 | rx_data[0];
|
||
odrive_msg.speed = rx_data[7] << 24 | rx_data[6] << 16 | rx_data[5] << 8 | rx_data[4];
|
||
break;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
|
||
/**
|
||
* @brief odrive初始化
|
||
* @param none
|
||
* @return device_status_e，若值为0，则为成功
|
||
*/
|
||
device_status_e Odrive_Can_Init(void)
|
||
{
|
||
#if INITED == 0
|
||
/**********************can初始化*********************/
|
||
CAN_FilterTypeDef can_filter_st;
|
||
can_filter_st.FilterActivation = ENABLE;
|
||
can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
|
||
can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
|
||
can_filter_st.FilterIdHigh = 0x0000;
|
||
can_filter_st.FilterIdLow = 0x0000;
|
||
can_filter_st.FilterMaskIdHigh = 0x0000;
|
||
can_filter_st.FilterMaskIdLow = 0x0000;
|
||
|
||
can_filter_st.FilterBank = 0;
|
||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
|
||
HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
|
||
HAL_CAN_Start(&hcan1);
|
||
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
|
||
#if WHICH_CAN == 1
|
||
//指向自定义的中断回调函数
|
||
BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);
|
||
#endif
|
||
#if WHICH_CAN == 2
|
||
//开启can2过滤器之前必须先开启can1
|
||
|
||
can_filter_st.SlaveStartFilterBank = 14;
|
||
can_filter_st.FilterBank = 14;
|
||
can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO1;
|
||
HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
|
||
HAL_CAN_Start(&hcan2);
|
||
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
|
||
//指向自定义的中断回调函数
|
||
BSP_CAN_RegisterCallback(BSP_CAN_2,HAL_CAN_RX_FIFO1_MSG_PENDING_CB,&User_Odrive_Can2_CB);
|
||
|
||
#endif
|
||
|
||
#else
|
||
#if WHICH_CAN == 1
|
||
//若已经在别处初始化can，则此处只进行指向自定义的中断回调函数
|
||
BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);
|
||
#else
|
||
|
||
BSP_CAN_RegisterCallback(BSP_CAN_2,HAL_CAN_RX_FIFO1_MSG_PENDING_CB,&User_Odrive_Can2_CB);
|
||
#endif
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
// void odrive_CB(void)
|
||
// {
|
||
// BSP_CAN_RegisterCallback(BSP_CAN_1,HAL_CAN_RX_FIFO0_MSG_PENDING_CB,&User_Odrive_Can1_CB);
|
||
|
||
// }//我自己加的
|
||
|
||
static CAN_TxHeaderTypeDef tx_message;
|
||
static uint8_t can_send_data[8];
|
||
|
||
/**
|
||
* @brief 速度模式控制电机
|
||
* @param node_id 节点id，AXIS0_NODE或者AXIS0_NODE
|
||
* @param speed 目标速度
|
||
* @return device_status_e，若值为0，则为成功
|
||
*/
|
||
device_status_e odrive_speed_cmd(node_id_e node_id, fp32 speed)
|
||
{
|
||
if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
|
||
{
|
||
return DEVICE_ERR;
|
||
}//如果节点id错误，则返回错误
|
||
|
||
|
||
uint32_t send_mail_box;
|
||
tx_message.StdId = node_id << 5 | ODRIVE_SPEED_SET;
|
||
tx_message.IDE = CAN_ID_STD;
|
||
tx_message.RTR = CAN_RTR_DATA;
|
||
tx_message.DLC = 0x08;
|
||
can_send_data[0] = BYTE0(speed);
|
||
can_send_data[1] = BYTE1(speed);
|
||
can_send_data[2] = BYTE2(speed);
|
||
can_send_data[3] = BYTE3(speed);
|
||
can_send_data[4] = 0;
|
||
can_send_data[5] = 0;
|
||
can_send_data[6] = 0;
|
||
can_send_data[7] = 0;
|
||
#if WHICH_CAN == 1
|
||
HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
|
||
#else
|
||
HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
uint8_t a;
|
||
/**
|
||
* @brief 位置模式控制电机
|
||
* @param node_id 节点id，AXIS0_NODE或者AXIS0_NODE
|
||
* @param pos 目标位置
|
||
* @param accel 加速度限制
|
||
* @param decel 减速度限制
|
||
* @return device_status_e，若值为0，则为成功
|
||
*/
|
||
device_status_e odrive_pos_cmd(node_id_e node_id, fp32 pos)
|
||
{
|
||
if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
|
||
{
|
||
return DEVICE_ERR;
|
||
}//如果节点id错误，则返回错误
|
||
|
||
uint32_t send_mail_box;
|
||
//发送位置
|
||
tx_message.StdId = node_id << 5 | ODRIVE_POS_SET;
|
||
tx_message.IDE = CAN_ID_STD;
|
||
tx_message.RTR = CAN_RTR_DATA;
|
||
tx_message.DLC = 0x08;
|
||
can_send_data[0] = BYTE0(pos);
|
||
can_send_data[1] = BYTE1(pos);
|
||
can_send_data[2] = BYTE2(pos);
|
||
can_send_data[3] = BYTE3(pos);
|
||
can_send_data[4] = 0;
|
||
can_send_data[5] = 0;
|
||
can_send_data[6] = 0;
|
||
can_send_data[7] = 0;
|
||
|
||
|
||
#if WHICH_CAN == 1
|
||
a= HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
|
||
#else
|
||
HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 位置模式控制电机
|
||
* @param node_id 节点id，AXIS0_NODE或者AXIS0_NODE
|
||
* @param pos 目标位置
|
||
* @param accel 加速度限制
|
||
* @param decel 减速度限制
|
||
* @return device_status_e，若值为0，则为成功
|
||
*/
|
||
device_status_e odrive_accel_cmd(node_id_e node_id, fp32 accel , fp32 decel)
|
||
{
|
||
if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
|
||
{
|
||
return DEVICE_ERR;
|
||
}//如果节点id错误，则返回错误
|
||
|
||
//发送加减速
|
||
uint32_t send_mail_box;
|
||
tx_message.StdId = node_id << 5 | ODRIVE_TRAJ_ACC_LIM;
|
||
tx_message.IDE = CAN_ID_STD;
|
||
tx_message.RTR = CAN_RTR_DATA;
|
||
tx_message.DLC = 0x08;
|
||
can_send_data[0] = BYTE0(accel);
|
||
can_send_data[1] = BYTE1(accel);
|
||
can_send_data[2] = BYTE2(accel);
|
||
can_send_data[3] = BYTE3(accel);
|
||
can_send_data[4] = BYTE0(decel);
|
||
can_send_data[5] = BYTE1(decel);
|
||
can_send_data[6] = BYTE2(decel);
|
||
can_send_data[7] = BYTE3(decel);
|
||
#if WHICH_CAN == 1
|
||
HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
|
||
#else
|
||
HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief 获取编码器估算值
|
||
* @param node_id 节点id，AXIS0_NODE或者AXIS0_NODE
|
||
* @return device_status_e，若值为0，则为成功
|
||
*/
|
||
device_status_e odrive_get_encode(node_id_e node_id)
|
||
{
|
||
if(node_id != AXIS0_NODE && node_id != AXIS1_NODE)
|
||
{
|
||
return DEVICE_ERR;
|
||
}//如果节点id错误，则返回错误
|
||
|
||
|
||
uint32_t send_mail_box;
|
||
tx_message.StdId = node_id << 5 | ODRIVE_ENCODER_VALUE;
|
||
tx_message.IDE = CAN_ID_STD;
|
||
tx_message.RTR = CAN_RTR_REMOTE;
|
||
tx_message.DLC = 0x08;
|
||
can_send_data[0] = 0x10;
|
||
can_send_data[1] = 0;
|
||
can_send_data[2] = 0;
|
||
can_send_data[3] = 0;
|
||
can_send_data[4] = 0;
|
||
can_send_data[5] = 0;
|
||
can_send_data[6] = 0;
|
||
can_send_data[7] = 0;
|
||
#if WHICH_CAN == 1
|
||
HAL_CAN_AddTxMessage(&hcan1, &tx_message, can_send_data, &send_mail_box);
|
||
#else
|
||
HAL_CAN_AddTxMessage(&hcan2, &tx_message, can_send_data, &send_mail_box);
|
||
#endif
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 获取odrive结构体指针
|
||
* @param none
|
||
* @return 信息结构体指针
|
||
*/
|
||
odrive_msg_t* Get_Odrive_Point(void)
|
||
{
|
||
return &odrive_msg;
|
||
}
|
||
|
||
|
||
|