RoboDuoduo/shooter
Template
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

can2使用小米

Browse Source
This commit is contained in:
ws 2025-03-30 16:03:07 +08:00
parent 6909b34ccd
commit 09d8446f34
7 changed files with 785 additions and 728 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
User/bsp/can_init.c
View File

@ -18,18 +18,18 @@ void can_filter_init(void)
can_filter_st.FilterMaskIdHigh = 0x0000;
can_filter_st.FilterMaskIdLow = 0x0000;
can_filter_st.FilterBank = 0;
can_filter_st.SlaveStartFilterBank = 14;
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);
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);
}

314
User/device/Xiaomi.c Normal file
View File

@ -0,0 +1,314 @@
#include "Xiaomi.h"
#include "can_it.h"
#include "can_init.h"
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"
//小米电机
CAN_RxHeaderTypeDef rxMsg;//发送接收结构体
CAN_TxHeaderTypeDef txMsg;//发送配置结构体
uint32_t Motor_Can_ID; //接收数据电机ID
uint8_t byte[4]; //转换临时数据
uint32_t send_mail_box = {0};//NONE
#define can_txd() HAL_CAN_AddTxMessage(&hcan2, &txMsg, tx_data, &send_mail_box)//CAN发送宏定义
MI_Motor mi_motor[4];//预先定义四个小米电机
CAN_RxHeaderTypeDef xm_rx_header;
uint8_t xm_rx_data[8];
/**
* @brief 浮点数转4字节函数
* @param[in] f:浮点数
* @retval 4字节数组
* @description : IEEE 754 协议
*/
static uint8_t* Float_to_Byte(float f)
{
unsigned long longdata = 0;
longdata = *(unsigned long*)&f;
byte[0] = (longdata & 0xFF000000) >> 24;
byte[1] = (longdata & 0x00FF0000) >> 16;
byte[2] = (longdata & 0x0000FF00) >> 8;
byte[3] = (longdata & 0x000000FF);
return byte;
}
/**
* @brief 小米电机回文16位数据转浮点
* @param[in] x:16位回文
* @param[in] x_min:对应参数下限
* @param[in] x_max:对应参数上限
* @param[in] bits:参数位数
* @retval 返回浮点值
*/
static float uint16_to_float(uint16_t x,float x_min,float x_max,int bits)
{
uint32_t span = (1 << bits) - 1;
float offset = x_max - x_min;
return offset * x / span + x_min;
}
/**
* @brief 小米电机发送浮点转16位数据
* @param[in] x:浮点
* @param[in] x_min:对应参数下限
* @param[in] x_max:对应参数上限
* @param[in] bits:参数位数
* @retval 返回浮点值
*/
static int float_to_uint(float x, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
if(x > x_max) x=x_max;
else if(x < x_min) x= x_min;
return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}
/**
* @brief 写入电机参数
* @param[in] Motor:对应控制电机结构体
* @param[in] Index:写入参数对应地址
* @param[in] Value:写入参数值
* @param[in] Value_type:写入参数数据类型
* @retval none
*/
static void Set_Motor_Parameter(MI_Motor *Motor,uint16_t Index,float Value,char Value_type){
uint8_t tx_data[8];
txMsg.ExtId = Communication_Type_SetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
tx_data[0]=Index;
tx_data[1]=Index>>8;
tx_data[2]=0x00;
tx_data[3]=0x00;
if(Value_type == 'f'){
Float_to_Byte(Value);
tx_data[4]=byte[3];
tx_data[5]=byte[2];
tx_data[6]=byte[1];
tx_data[7]=byte[0];
}
else if(Value_type == 's'){
tx_data[4]=(uint8_t)Value;
tx_data[5]=0x00;
tx_data[6]=0x00;
tx_data[7]=0x00;
}
can_txd();
}
/**
* @brief 提取电机回复帧扩展ID中的电机CANID
* @param[in] CAN_ID_Frame:电机回复帧中的扩展CANID
* @retval 电机CANID
*/
static uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame)
{
return (CAN_ID_Frame&0xFFFF)>>8;
}
/**
* @brief 电机回复帧数据处理函数
* @param[in] Motor:对应控制电机结构体
* @param[in] DataFrame:数据帧
* @param[in] IDFrame:扩展ID帧
* @retval None
*/
static void Motor_Data_Handler(MI_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame)
{
Motor->Angle=uint16_to_float(DataFrame[0]<<8|DataFrame[1],MIN_P,MAX_P,16);
Motor->Speed=uint16_to_float(DataFrame[2]<<8|DataFrame[3],V_MIN,V_MAX,16);
Motor->Torque=uint16_to_float(DataFrame[4]<<8|DataFrame[5],T_MIN,T_MAX,16);
Motor->Temp=(DataFrame[6]<<8|DataFrame[7])*Temp_Gain;
Motor->error_code=(IDFrame&0x1F0000)>>16;
}
/**
* @brief 小米电机ID检查
* @param[in] id: 控制电机CAN_ID【出厂默认0x7F】
* @retval none
*/
void chack_cybergear(uint8_t ID)
{
uint8_t tx_data[8] = {0};
txMsg.ExtId = Communication_Type_GetID<<24|Master_CAN_ID<<8|ID;
can_txd();
}
/**
* @brief 使能小米电机
* @param[in] Motor:对应控制电机结构体
* @retval none
*/
void start_cybergear(MI_Motor *Motor)
{
uint8_t tx_data[8] = {0};
txMsg.ExtId = Communication_Type_MotorEnable<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 停止电机
* @param[in] Motor:对应控制电机结构体
* @param[in] clear_error:清除错误位（0 不清除 1清除）
* @retval None
*/
void stop_cybergear(MI_Motor *Motor,uint8_t clear_error)
{
uint8_t tx_data[8]={0};
tx_data[0]=clear_error;//清除错误位设置
txMsg.ExtId = Communication_Type_MotorStop<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 设置电机模式(必须停止时调整！)
* @param[in] Motor: 电机结构体
* @param[in] Mode: 电机工作模式（1.运动模式Motion_mode 2. 位置模式Position_mode 3. 速度模式Speed_mode 4. 电流模式Current_mode）
* @retval none
*/
void set_mode_cybergear(MI_Motor *Motor,uint8_t Mode)
{
Set_Motor_Parameter(Motor,Run_mode,Mode,'s');
}
/**
* @brief 电流控制模式下设置电流
* @param[in] Motor: 电机结构体
* @param[in] Current:电流设置
* @retval none
*/
void set_current_cybergear(MI_Motor *Motor,float Current)
{
Set_Motor_Parameter(Motor,Iq_Ref,Current,'f');
}
/**
* @brief 设置电机零点
* @param[in] Motor: 电机结构体
* @retval none
*/
void set_zeropos_cybergear(MI_Motor *Motor)
{
uint8_t tx_data[8]={0};
tx_data[0] = 1;
txMsg.ExtId = Communication_Type_SetPosZero<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 设置电机CANID
* @param[in] Motor: 电机结构体
* @param[in] Motor: 设置新ID
* @retval none
*/
void set_CANID_cybergear(MI_Motor *Motor,uint8_t CAN_ID)
{
uint8_t tx_data[8]={0};
txMsg.ExtId = Communication_Type_CanID<<24|CAN_ID<<16|Master_CAN_ID<<8|Motor->CAN_ID;
Motor->CAN_ID = CAN_ID;//将新的ID导入电机结构体
can_txd();
}
/**
* @brief 小米电机初始化
* @param[in] Motor: 电机结构体
* @param[in] Can_Id: 小米电机ID(默认0x7F)
* @param[in] Motor_Num: 电机编号
* @param[in] mode: 电机工作模式（0.运动模式Motion_mode 1. 位置模式Position_mode 2. 速度模式Speed_mode 3. 电流模式Current_mode）
* @retval none
*/
void init_cybergear(MI_Motor *Motor,uint8_t Can_Id, uint8_t mode)
{
txMsg.StdId = 0; //配置CAN发送：标准帧清零
txMsg.ExtId = 0; //配置CAN发送：扩展帧清零
txMsg.IDE = CAN_ID_EXT; //配置CAN发送：扩展帧
txMsg.RTR = CAN_RTR_DATA; //配置CAN发送：数据帧
txMsg.DLC = 0x08; //配置CAN发送：数据长度
Motor->CAN_ID=Can_Id; //ID设置
set_mode_cybergear(Motor,mode);//设置电机模式
start_cybergear(Motor); //使能电机
}
/**
* @brief 小米运控模式指令
* @param[in] Motor: 目标电机结构体
* @param[in] torque: 力矩设置[-12,12] N*M
* @param[in] MechPosition: 位置设置[-12.5,12.5] rad
* @param[in] speed: 速度设置[-30,30] rpm
* @param[in] kp: 比例参数设置
* @param[in] kd: 微分参数设置
* @retval none
*/
void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd)
{
uint8_t tx_data[8];//发送数据初始化
//装填发送数据
tx_data[0]=float_to_uint(MechPosition,P_MIN,P_MAX,16)>>8;
tx_data[1]=float_to_uint(MechPosition,P_MIN,P_MAX,16);
tx_data[2]=float_to_uint(speed,V_MIN,V_MAX,16)>>8;
tx_data[3]=float_to_uint(speed,V_MIN,V_MAX,16);
tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;
tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);
tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;
tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16);
txMsg.ExtId = Communication_Type_MotionControl<<24|float_to_uint(torque,T_MIN,T_MAX,16)<<8|Motor->CAN_ID;//装填扩展帧数据
can_txd();
}
/**
* @brief 小米电机数据处理函数
* @param[in] none
* @retval none
*/
void XM_MotorEncode()
{
if(xm_rx_header.IDE == CAN_ID_EXT)
{
Motor_Can_ID=Get_Motor_ID(xm_rx_header.ExtId);//首先获取回传电机ID信息
switch(Motor_Can_ID)
{
case 0x01:
if(xm_rx_header.ExtId>>24 != 0) //检查是否为广播模式
Motor_Data_Handler(&mi_motor[0],xm_rx_data,xm_rx_header.ExtId);
else
mi_motor[0].MCU_ID = xm_rx_data[0];
break;
default:
break;
}
}
}
static osEventFlagsId_t eventReceive;//事件标志
/**
* @brief 自定义小米电机回调函数
* @param[in] none
* @retval none
*/
void XM_Motor_CB()
{
HAL_CAN_GetRxMessage(&hcan2, CAN_RX_FIFO1, &xm_rx_header, xm_rx_data);
osEventFlagsSet(eventReceive, EVENT_CAN);
}
/**
* @brief 小米电机初始化
* @param none
* @retval none
*/
void xia0miInit(void)
{
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
XM_Motor_CB);
// can_filter_init();//dji那里已经初始化了
}

112
User/device/Xiaomi.h Normal file
View File

@ -0,0 +1,112 @@
#ifndef _XIAOMI_H
#define _XIAOMI_H
#include "struct_typedef.h"
#include "can.h"
/*小米电机部分参数和函数*/
//控制参数最值，谨慎更改
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -30.0f
#define V_MAX 30.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -12.0f
#define T_MAX 12.0f
#define MAX_P 720
#define MIN_P -720
//主机CANID设置
#define Master_CAN_ID 0x00 //主机ID
//控制命令宏定义
#define Communication_Type_GetID 0x00 //获取设备的ID和64位MCU唯一标识符
#define Communication_Type_MotionControl 0x01 //用来向主机发送控制指令
#define Communication_Type_MotorRequest 0x02 //用来向主机反馈电机运行状态
#define Communication_Type_MotorEnable 0x03 //电机使能运行
#define Communication_Type_MotorStop 0x04 //电机停止运行
#define Communication_Type_SetPosZero 0x06 //设置电机机械零位
#define Communication_Type_CanID 0x07 //更改当前电机CAN_ID
#define Communication_Type_Control_Mode 0x12
#define Communication_Type_GetSingleParameter 0x11 //读取单个参数
#define Communication_Type_SetSingleParameter 0x12 //设定单个参数
#define Communication_Type_ErrorFeedback 0x15 //故障反馈帧
//参数读取宏定义
#define Run_mode 0x7005
#define Iq_Ref 0x7006
#define Spd_Ref 0x700A
#define Limit_Torque 0x700B
#define Cur_Kp 0x7010
#define Cur_Ki 0x7011
#define Cur_Filt_Gain 0x7014
#define Loc_Ref 0x7016
#define Limit_Spd 0x7017
#define Limit_Cur 0x7018
#define Gain_Angle 720/32767.0
#define Bias_Angle 0x8000
#define Gain_Speed 30/32767.0
#define Bias_Speed 0x8000
#define Gain_Torque 12/32767.0
#define Bias_Torque 0x8000
#define Temp_Gain 0.1
#define Motor_Error 0x00
#define Motor_OK 0X01
enum CONTROL_MODE //控制模式定义
{
Motion_mode = 0,//运控模式
Position_mode, //位置模式
Speed_mode, //速度模式
Current_mode //电流模式
};
enum ERROR_TAG //错误回传对照
{
OK = 0,//无故障
BAT_LOW_ERR = 1,//欠压故障
OVER_CURRENT_ERR = 2,//过流
OVER_TEMP_ERR = 3,//过温
MAGNETIC_ERR = 4,//磁编码故障
HALL_ERR_ERR = 5,//HALL编码故障
NO_CALIBRATION_ERR = 6//未标定
};
typedef struct{ //小米电机结构体
uint8_t CAN_ID; //CAN ID
uint8_t MCU_ID; //MCU唯一标识符[后8位，共64位]
float Angle; //回传角度
float Speed; //回传速度
float Torque; //回传力矩
float Temp; //回传温度
uint16_t set_current;
uint16_t set_speed;
uint16_t set_position;
uint8_t error_code;
float Angle_Bias;
}MI_Motor;
extern MI_Motor mi_motor[4];//预先定义四个小米电机
//小米
void XM_MotorEncode(void);
void xia0miInit(void);
extern void chack_cybergear(uint8_t ID);
extern void start_cybergear(MI_Motor *Motor);
extern void stop_cybergear(MI_Motor *Motor, uint8_t clear_error);
extern void set_mode_cybergear(MI_Motor *Motor, uint8_t Mode);
extern void set_current_cybergear(MI_Motor *Motor, float Current);
extern void set_zeropos_cybergear(MI_Motor *Motor);
extern void set_CANID_cybergear(MI_Motor *Motor, uint8_t CAN_ID);
extern void init_cybergear(MI_Motor *Motor, uint8_t Can_Id, uint8_t mode);
extern void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd);
#endif

976
User/device/djiMotor.c
View File

@ -15,641 +15,365 @@
****************************(C) COPYRIGHT ZhouCc****************************
*/
#include "djiMotor.h"
#include "can_it.h"
#include "can_init.h"
#if FREERTOS_DJI == 1
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"
#include "odrive_can.h"
static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t vesc_send_data[4];
#endif
//motor data read
#define get_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \
}//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
#define get_motor_offset(ptr, data) \
{ \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->offset_ecd = (ptr)->ecd; \
}
#define get_6020_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \
}
#define get_2006_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \
}
// 解析出初始编码器值
// #define get_5065motor_measure(ptr, data) \
// { \
// (ptr)->last_ecd = (ptr)->ecd; \
// (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]); \
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
// }
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
const uint8_t *raw)
{
if (feedback == NULL || raw == NULL) return;
union
{
int x;
uint8_t data[4];
}speed;
speed.data[0]= raw[3];
speed.data[1]= raw[2];
speed.data[2]= raw[1];
speed.data[3]= raw[0];
feedback->rotor_speed = speed.x;
union
{
int16_t y;
uint8_t dat[2];
}current;
current.dat[0]= raw[5];
current.dat[1]= raw[4];
feedback->torque_current =(fp32)current.y/10;
}
#if DEBUG == 1
motor_measure_t motor_chassis[5];
CAN_MotorFeedback_t motor_6384;
#else
static motor_measure_t motor_chassis[5];
#endif
static CAN_TxHeaderTypeDef tx_meessage_1ff;
static uint8_t can_send_data_1ff[8];
static CAN_TxHeaderTypeDef tx_meessage_2ff;
static uint8_t can_send_data_2ff[8];
static CAN_TxHeaderTypeDef tx_message_200;
static uint8_t can_send_data_200[8];
CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];
//小米电机
CAN_RxHeaderTypeDef rxMsg;//发送接收结构体
CAN_TxHeaderTypeDef txMsg;//发送配置结构体
uint32_t Motor_Can_ID; //接收数据电机ID
uint8_t byte[4]; //转换临时数据
uint32_t send_mail_box = {0};//NONE
#include "djiMotor.h"
#include "can_it.h"
#include "can_init.h"
#if FREERTOS_DJI == 1
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"
static CAN_TxHeaderTypeDef vesc_tx_message;
static uint8_t vesc_send_data[4];
#endif
//motor data read
#define get_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508; \
}//ptr指向电机测量数据结构体的指针，data包含电机测量数据的数组.
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
#define get_motor_offset(ptr, data) \
{ \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->offset_ecd = (ptr)->ecd; \
}
#define get_6020_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020; \
}
#define get_2006_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
(ptr)->ecd - (ptr)->last_ecd > 4096 ? ((ptr)->round_cnt--) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->ecd - (ptr)->last_ecd < -4096 ? ((ptr)->round_cnt++) : ((ptr)->round_cnt=(ptr)->round_cnt); \
(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006; \
}
// 解析出初始编码器值
// #define get_5065motor_measure(ptr, data) \
// { \
// (ptr)->last_ecd = (ptr)->ecd; \
// (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]); \
// (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);; \
// }
/*(ptr)->real_angle = (ptr)->real_angle % 360; */
#define can_txd() HAL_CAN_AddTxMessage(&hcan1, &txMsg, tx_data, &send_mail_box)//CAN发送宏定义
MI_Motor mi_motor[4];//预先定义四个小米电机
/**
* @brief 浮点数转4字节函数
* @param[in] f:浮点数
* @retval 4字节数组
* @description : IEEE 754 协议
*/
static uint8_t* Float_to_Byte(float f)
{
unsigned long longdata = 0;
longdata = *(unsigned long*)&f;
byte[0] = (longdata & 0xFF000000) >> 24;
byte[1] = (longdata & 0x00FF0000) >> 16;
byte[2] = (longdata & 0x0000FF00) >> 8;
byte[3] = (longdata & 0x000000FF);
return byte;
}
/**
* @brief 小米电机回文16位数据转浮点
* @param[in] x:16位回文
* @param[in] x_min:对应参数下限
* @param[in] x_max:对应参数上限
* @param[in] bits:参数位数
* @retval 返回浮点值
*/
static float uint16_to_float(uint16_t x,float x_min,float x_max,int bits)
{
uint32_t span = (1 << bits) - 1;
float offset = x_max - x_min;
return offset * x / span + x_min;
}
/**
* @brief 小米电机发送浮点转16位数据
* @param[in] x:浮点
* @param[in] x_min:对应参数下限
* @param[in] x_max:对应参数上限
* @param[in] bits:参数位数
* @retval 返回浮点值
*/
static int float_to_uint(float x, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
if(x > x_max) x=x_max;
else if(x < x_min) x= x_min;
return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}
/**
* @brief 写入电机参数
* @param[in] Motor:对应控制电机结构体
* @param[in] Index:写入参数对应地址
* @param[in] Value:写入参数值
* @param[in] Value_type:写入参数数据类型
* @retval none
*/
static void Set_Motor_Parameter(MI_Motor *Motor,uint16_t Index,float Value,char Value_type){
uint8_t tx_data[8];
txMsg.ExtId = Communication_Type_SetSingleParameter<<24|Master_CAN_ID<<8|Motor->CAN_ID;
tx_data[0]=Index;
tx_data[1]=Index>>8;
tx_data[2]=0x00;
tx_data[3]=0x00;
if(Value_type == 'f'){
Float_to_Byte(Value);
tx_data[4]=byte[3];
tx_data[5]=byte[2];
tx_data[6]=byte[1];
tx_data[7]=byte[0];
}
else if(Value_type == 's'){
tx_data[4]=(uint8_t)Value;
tx_data[5]=0x00;
tx_data[6]=0x00;
tx_data[7]=0x00;
}
can_txd();
}
/**
* @brief 提取电机回复帧扩展ID中的电机CANID
* @param[in] CAN_ID_Frame:电机回复帧中的扩展CANID
* @retval 电机CANID
*/
static uint32_t Get_Motor_ID(uint32_t CAN_ID_Frame)
{
return (CAN_ID_Frame&0xFFFF)>>8;
}
/**
* @brief 电机回复帧数据处理函数
* @param[in] Motor:对应控制电机结构体
* @param[in] DataFrame:数据帧
* @param[in] IDFrame:扩展ID帧
* @retval None
*/
static void Motor_Data_Handler(MI_Motor *Motor,uint8_t DataFrame[8],uint32_t IDFrame)
{
Motor->Angle=uint16_to_float(DataFrame[0]<<8|DataFrame[1],MIN_P,MAX_P,16);
Motor->Speed=uint16_to_float(DataFrame[2]<<8|DataFrame[3],V_MIN,V_MAX,16);
Motor->Torque=uint16_to_float(DataFrame[4]<<8|DataFrame[5],T_MIN,T_MAX,16);
Motor->Temp=(DataFrame[6]<<8|DataFrame[7])*Temp_Gain;
Motor->error_code=(IDFrame&0x1F0000)>>16;
}
/**
* @brief 小米电机ID检查
* @param[in] id: 控制电机CAN_ID【出厂默认0x7F】
* @retval none
*/
void chack_cybergear(uint8_t ID)
{
uint8_t tx_data[8] = {0};
txMsg.ExtId = Communication_Type_GetID<<24|Master_CAN_ID<<8|ID;
can_txd();
}
/**
* @brief 使能小米电机
* @param[in] Motor:对应控制电机结构体
* @retval none
*/
void start_cybergear(MI_Motor *Motor)
{
uint8_t tx_data[8] = {0};
txMsg.ExtId = Communication_Type_MotorEnable<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 停止电机
* @param[in] Motor:对应控制电机结构体
* @param[in] clear_error:清除错误位（0 不清除 1清除）
* @retval None
*/
void stop_cybergear(MI_Motor *Motor,uint8_t clear_error)
{
uint8_t tx_data[8]={0};
tx_data[0]=clear_error;//清除错误位设置
txMsg.ExtId = Communication_Type_MotorStop<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 设置电机模式(必须停止时调整！)
* @param[in] Motor: 电机结构体
* @param[in] Mode: 电机工作模式（1.运动模式Motion_mode 2. 位置模式Position_mode 3. 速度模式Speed_mode 4. 电流模式Current_mode）
* @retval none
*/
void set_mode_cybergear(MI_Motor *Motor,uint8_t Mode)
{
Set_Motor_Parameter(Motor,Run_mode,Mode,'s');
}
/**
* @brief 电流控制模式下设置电流
* @param[in] Motor: 电机结构体
* @param[in] Current:电流设置
* @retval none
*/
void set_current_cybergear(MI_Motor *Motor,float Current)
{
Set_Motor_Parameter(Motor,Iq_Ref,Current,'f');
}
/**
* @brief 设置电机零点
* @param[in] Motor: 电机结构体
* @retval none
*/
void set_zeropos_cybergear(MI_Motor *Motor)
{
uint8_t tx_data[8]={0};
tx_data[0] = 1;
txMsg.ExtId = Communication_Type_SetPosZero<<24|Master_CAN_ID<<8|Motor->CAN_ID;
can_txd();
}
/**
* @brief 设置电机CANID
* @param[in] Motor: 电机结构体
* @param[in] Motor: 设置新ID
* @retval none
*/
void set_CANID_cybergear(MI_Motor *Motor,uint8_t CAN_ID)
{
uint8_t tx_data[8]={0};
txMsg.ExtId = Communication_Type_CanID<<24|CAN_ID<<16|Master_CAN_ID<<8|Motor->CAN_ID;
Motor->CAN_ID = CAN_ID;//将新的ID导入电机结构体
can_txd();
}
/**
* @brief 小米电机初始化
* @param[in] Motor: 电机结构体
* @param[in] Can_Id: 小米电机ID(默认0x7F)
* @param[in] Motor_Num: 电机编号
* @param[in] mode: 电机工作模式（0.运动模式Motion_mode 1. 位置模式Position_mode 2. 速度模式Speed_mode 3. 电流模式Current_mode）
* @retval none
*/
void init_cybergear(MI_Motor *Motor,uint8_t Can_Id, uint8_t mode)
{
txMsg.StdId = 0; //配置CAN发送：标准帧清零
txMsg.ExtId = 0; //配置CAN发送：扩展帧清零
txMsg.IDE = CAN_ID_EXT; //配置CAN发送：扩展帧
txMsg.RTR = CAN_RTR_DATA; //配置CAN发送：数据帧
txMsg.DLC = 0x08; //配置CAN发送：数据长度
Motor->CAN_ID=Can_Id; //ID设置
set_mode_cybergear(Motor,mode);//设置电机模式
start_cybergear(Motor); //使能电机
}
/**
* @brief 小米运控模式指令
* @param[in] Motor: 目标电机结构体
* @param[in] torque: 力矩设置[-12,12] N*M
* @param[in] MechPosition: 位置设置[-12.5,12.5] rad
* @param[in] speed: 速度设置[-30,30] rpm
* @param[in] kp: 比例参数设置
* @param[in] kd: 微分参数设置
* @retval none
*/
void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd)
{
uint8_t tx_data[8];//发送数据初始化
//装填发送数据
tx_data[0]=float_to_uint(MechPosition,P_MIN,P_MAX,16)>>8;
tx_data[1]=float_to_uint(MechPosition,P_MIN,P_MAX,16);
tx_data[2]=float_to_uint(speed,V_MIN,V_MAX,16)>>8;
tx_data[3]=float_to_uint(speed,V_MIN,V_MAX,16);
tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;
tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);
tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;
tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16);
txMsg.ExtId = Communication_Type_MotionControl<<24|float_to_uint(torque,T_MIN,T_MAX,16)<<8|Motor->CAN_ID;//装填扩展帧数据
can_txd();
}
/**
* @brief 数据处理函数
* @param[in] none
* @retval none
*/
void djiMotorEncode()
{
if(dji_rx_header.IDE == CAN_ID_STD)
static void CAN_VescMotor_Decode_1(CAN_MotorFeedback_t *feedback,
const uint8_t *raw)
{
switch (dji_rx_header.StdId)
if (feedback == NULL || raw == NULL) return;
union
{
case 0x201:
case 0x202:
case 0x203:
case 0x204:
case 0x205:
case 0x206:
case 0x207:
int x;
uint8_t data[4];
}speed;
speed.data[0]= raw[3];
speed.data[1]= raw[2];
speed.data[2]= raw[1];
speed.data[3]= raw[0];
feedback->rotor_speed = speed.x;
union
{
static uint8_t i = 0;
//get motor id
i = dji_rx_header.StdId - 0x201;
if(motor_chassis[i].msg_cnt<=50)
{
motor_chassis[i].msg_cnt++;
get_motor_offset(&motor_chassis[i], dji_rx_data);
}else{
get_motor_measure(&motor_chassis[i], dji_rx_data);
}
break;
int16_t y;
uint8_t dat[2];
}current;
current.dat[0]= raw[5];
current.dat[1]= raw[4];
feedback->torque_current =(fp32)current.y/10;
}
#if DEBUG == 1
motor_measure_t motor_chassis[5];
CAN_MotorFeedback_t motor_6384;
#else
static motor_measure_t motor_chassis[5];
#endif
static CAN_TxHeaderTypeDef tx_meessage_1ff;
static uint8_t can_send_data_1ff[8];
static CAN_TxHeaderTypeDef tx_meessage_2ff;
static uint8_t can_send_data_2ff[8];
static CAN_TxHeaderTypeDef tx_message_200;
static uint8_t can_send_data_200[8];
CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];
/**
* @brief 数据处理函数
* @param[in] none
* @retval none
*/
void djiMotorEncode()
{
if(dji_rx_header.IDE == CAN_ID_STD)
{
switch (dji_rx_header.StdId)
{
case 0x201:
case 0x202:
case 0x203:
case 0x204:
case 0x205:
case 0x206:
case 0x207:
{
static uint8_t i = 0;
//get motor id
i = dji_rx_header.StdId - 0x201;
if(motor_chassis[i].msg_cnt<=50)
{
motor_chassis[i].msg_cnt++;
get_motor_offset(&motor_chassis[i], dji_rx_data);
}else{
get_motor_measure(&motor_chassis[i], dji_rx_data);
}
break;
}
default:
{
break;
}
}
}
default:
{
break;
}
}
}
if(dji_rx_header.IDE == CAN_ID_EXT)
{
Motor_Can_ID=Get_Motor_ID(dji_rx_header.ExtId);//首先获取回传电机ID信息
switch(Motor_Can_ID)
{
case 0x01:
if(dji_rx_header.ExtId>>24 != 0) //检查是否为广播模式
Motor_Data_Handler(&mi_motor[0],dji_rx_data,dji_rx_header.ExtId);
else
mi_motor[0].MCU_ID = dji_rx_data[0];
break;
default:
break;
}
}
}
void vescMotorEncode()
{
switch (dji_rx_header.ExtId) {
case CAN_VESC5065_M1_MSG1:
// 存储消息到对应的电机结构体中
CAN_VescMotor_Decode_1(&motor_6384, dji_rx_data);
break;
void vescMotorEncode()
{
switch (dji_rx_header.ExtId) {
default:
break;
}
}
#if FREERTOS_DJI == 0
/**
* @brief hal库CAN回调函数,接收电机数据
* @param[in] hcan:CAN句柄指针
* @retval none
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
djiMotorEncode();
}
#else
static osEventFlagsId_t eventReceive;//事件标志
/**
* @brief 自定义大疆电机回调函数
* @param[in] none
* @retval none
*/
void Dji_Motor_CB()
{
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
case CAN_VESC5065_M1_MSG1:
// 存储消息到对应的电机结构体中
CAN_VescMotor_Decode_1(&motor_6384, dji_rx_data);
break;
default:
break;
}
osEventFlagsSet(eventReceive, EVENT_CAN);
}
/**
* @brief 大疆电机初始化
* @param none
* @retval none
*/
void djiInit(void)
{
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
Dji_Motor_CB);
can_filter_init();
}
/**
* @brief 等待新数据
*/
uint32_t waitNewDji()
{
return osEventFlagsWait(
eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
}
#endif
/**
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
* @param[in] motor1: (0x205) 电机控制电流
* @param[in] motor2: (0x206) 电机控制电流
* @param[in] motor3: (0x207) 电机控制电流
* @param[in] motor4: (0x208) 电机控制电流
* @retval none
}
#if FREERTOS_DJI == 0
/**
* @brief hal库CAN回调函数,接收电机数据
* @param[in] hcan:CAN句柄指针
* @retval none
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
djiMotorEncode();
}
#else
static osEventFlagsId_t eventReceive;//事件标志
/**
* @brief 自定义大疆电机回调函数
* @param[in] none
* @retval none
*/
void Dji_Motor_CB()
{
HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
osEventFlagsSet(eventReceive, EVENT_CAN);
}
/**
* @brief 大疆电机初始化
* @param none
* @retval none
*/
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_meessage_1ff.StdId = 0x1FF;
tx_meessage_1ff.IDE = CAN_ID_STD;
tx_meessage_1ff.RTR = CAN_RTR_DATA;
tx_meessage_1ff.DLC = 0x08;
can_send_data_1ff[0] = (motor1 >> 8);
can_send_data_1ff[1] = motor1;
can_send_data_1ff[2] = (motor2 >> 8);
can_send_data_1ff[3] = motor2;
can_send_data_1ff[4] = (motor3 >> 8);
can_send_data_1ff[5] = motor3;
can_send_data_1ff[6] = (motor4 >> 8);
can_send_data_1ff[7] = motor4;
HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
}
/**
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
* @param[in] motor1: (0x201) 电机控制电流
* @param[in] motor2: (0x202) 电机控制电流
* @param[in] motor3: (0x203) 电机控制电流
* @param[in] motor4: (0x204) 电机控制电流
* @retval none
void djiInit(void)
{
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
Dji_Motor_CB);
can_filter_init();
}
/**
* @brief 等待新数据
*/
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_message_200.StdId = 0x200;
tx_message_200.IDE = CAN_ID_STD;
tx_message_200.RTR = CAN_RTR_DATA;
tx_message_200.DLC = 0x08;
can_send_data_200[0] = motor1 >> 8;
can_send_data_200[1] = motor1;
can_send_data_200[2] = motor2 >> 8;
can_send_data_200[3] = motor2;
can_send_data_200[4] = motor3 >> 8;
can_send_data_200[5] = motor3;
can_send_data_200[6] = motor4 >> 8;
can_send_data_200[7] = motor4;
HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
}
/**
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
* @param[in] motor1: (0x209) 电机控制电流
* @param[in] motor2: (0x20A) 电机控制电流
* @param[in] motor3: (0x20B) 电机控制电流
* @retval none
*/
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_meessage_2ff.StdId = 0x1FF;
tx_meessage_2ff.IDE = CAN_ID_STD;
tx_meessage_2ff.RTR = CAN_RTR_DATA;
tx_meessage_2ff.DLC = 0x08;
can_send_data_2ff[0] = (motor1 >> 8);
can_send_data_2ff[1] = motor1;
can_send_data_2ff[2] = (motor2 >> 8);
can_send_data_2ff[3] = motor2;
can_send_data_2ff[4] = (motor3 >> 8);
can_send_data_2ff[5] = motor3;
can_send_data_2ff[6] = (0 >> 8);
can_send_data_2ff[7] = 0;
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}
/**
* @brief 返回电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_motor_point(uint8_t i)
{
return &motor_chassis[i];
}
/**
* @brief 限制vesc电机转速
* @param[in/out] rpm: vesce电机转速
* @retval none
*
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
*/
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
/**
* @brief 使vesc电机进入转速模式
* @param[in] controller_id: vesc电机控制器id
* @param[in] RPM: 电机转速
* @retval RPM（1000-50000之间的数）
*/
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
uint32_t id;
int32_t data;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
vesc_send_data[0] = data >> 24 ;
vesc_send_data[1] = data >> 16 ;
vesc_send_data[2] = data >> 8 ;
vesc_send_data[3] = data ;
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/**
* @brief 使vesc电机进入制动模式
* @param[in] controller_id: vesc电机控制器id
* @retval none
*/
void CAN_VESC_HEAD(uint8_t controller_id)
{
uint32_t id;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
uint32_t waitNewDji()
{
return osEventFlagsWait(
eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
}
#endif
/**
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
* @param[in] motor1: (0x205) 电机控制电流
* @param[in] motor2: (0x206) 电机控制电流
* @param[in] motor3: (0x207) 电机控制电流
* @param[in] motor4: (0x208) 电机控制电流
* @retval none
*/
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_meessage_1ff.StdId = 0x1FF;
tx_meessage_1ff.IDE = CAN_ID_STD;
tx_meessage_1ff.RTR = CAN_RTR_DATA;
tx_meessage_1ff.DLC = 0x08;
can_send_data_1ff[0] = (motor1 >> 8);
can_send_data_1ff[1] = motor1;
can_send_data_1ff[2] = (motor2 >> 8);
can_send_data_1ff[3] = motor2;
can_send_data_1ff[4] = (motor3 >> 8);
can_send_data_1ff[5] = motor3;
can_send_data_1ff[6] = (motor4 >> 8);
can_send_data_1ff[7] = motor4;
HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
}
/**
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
* @param[in] motor1: (0x201) 电机控制电流
* @param[in] motor2: (0x202) 电机控制电流
* @param[in] motor3: (0x203) 电机控制电流
* @param[in] motor4: (0x204) 电机控制电流
* @retval none
*/
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_message_200.StdId = 0x200;
tx_message_200.IDE = CAN_ID_STD;
tx_message_200.RTR = CAN_RTR_DATA;
tx_message_200.DLC = 0x08;
can_send_data_200[0] = motor1 >> 8;
can_send_data_200[1] = motor1;
can_send_data_200[2] = motor2 >> 8;
can_send_data_200[3] = motor2;
can_send_data_200[4] = motor3 >> 8;
can_send_data_200[5] = motor3;
can_send_data_200[6] = motor4 >> 8;
can_send_data_200[7] = motor4;
HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
}
/**
* @brief 发送电机控制电流(0x205,0x206,0x207,0x208)
* @param[in] motor1: (0x209) 电机控制电流
* @param[in] motor2: (0x20A) 电机控制电流
* @param[in] motor3: (0x20B) 电机控制电流
* @retval none
*/
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
uint32_t send_mail_box;
tx_meessage_2ff.StdId = 0x1FF;
tx_meessage_2ff.IDE = CAN_ID_STD;
tx_meessage_2ff.RTR = CAN_RTR_DATA;
tx_meessage_2ff.DLC = 0x08;
can_send_data_2ff[0] = (motor1 >> 8);
can_send_data_2ff[1] = motor1;
can_send_data_2ff[2] = (motor2 >> 8);
can_send_data_2ff[3] = motor2;
can_send_data_2ff[4] = (motor3 >> 8);
can_send_data_2ff[5] = motor3;
can_send_data_2ff[6] = (0 >> 8);
can_send_data_2ff[7] = 0;
HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}
/**
* @brief 返回电机数据指针
* @param[in] i: 电机编号
* @retval 电机数据指针
*/
motor_measure_t *get_motor_point(uint8_t i)
{
return &motor_chassis[i];
}
/**
* @brief 限制vesc电机转速
* @param[in/out] rpm: vesce电机转速
* @retval none
*
* 如果rpm的绝对值大于wtrcfg_VESC_COMMAND_ERPM_MAX,
* 则将rpm的值设置为wtrcfg_VESC_COMMAND_ERPM_MAX或-wtrcfg_VESC_COMMAND_ERPM_MAX
*/
void assert_param_rpm(float *rpm){
if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
}
/**
* @brief 使vesc电机进入转速模式
* @param[in] controller_id: vesc电机控制器id
* @param[in] RPM: 电机转速
* @retval RPM（1000-50000之间的数）
*/
void CAN_VESC_RPM(uint8_t controller_id, float RPM)
{
uint32_t id;
int32_t data;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8);
assert_param_rpm(&RPM);
data = (int32_t)(RPM);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
vesc_send_data[0] = data >> 24 ;
vesc_send_data[1] = data >> 16 ;
vesc_send_data[2] = data >> 8 ;
vesc_send_data[3] = data ;
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
}
/**
* @brief 使vesc电机进入制动模式
* @param[in] controller_id: vesc电机控制器id
* @retval none
*/
void CAN_VESC_HEAD(uint8_t controller_id)
{
uint32_t id;
uint32_t send_mail_box;
id = controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8);
vesc_tx_message.ExtId = id;
vesc_tx_message.IDE = CAN_ID_EXT;
vesc_tx_message.RTR = CAN_RTR_DATA;
vesc_tx_message.DLC = 0x04;
HAL_CAN_AddTxMessage(&hcan1, &vesc_tx_message, vesc_send_data, &send_mail_box);
}

99
User/device/djiMotor.h
View File

@ -108,94 +108,6 @@ typedef enum {
#if FREERTOS_DJI == 1
/*小米电机部分参数和函数*/
//控制参数最值，谨慎更改
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -30.0f
#define V_MAX 30.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -12.0f
#define T_MAX 12.0f
#define MAX_P 720
#define MIN_P -720
//主机CANID设置
#define Master_CAN_ID 0x00 //主机ID
//控制命令宏定义
#define Communication_Type_GetID 0x00 //获取设备的ID和64位MCU唯一标识符
#define Communication_Type_MotionControl 0x01 //用来向主机发送控制指令
#define Communication_Type_MotorRequest 0x02 //用来向主机反馈电机运行状态
#define Communication_Type_MotorEnable 0x03 //电机使能运行
#define Communication_Type_MotorStop 0x04 //电机停止运行
#define Communication_Type_SetPosZero 0x06 //设置电机机械零位
#define Communication_Type_CanID 0x07 //更改当前电机CAN_ID
#define Communication_Type_Control_Mode 0x12
#define Communication_Type_GetSingleParameter 0x11 //读取单个参数
#define Communication_Type_SetSingleParameter 0x12 //设定单个参数
#define Communication_Type_ErrorFeedback 0x15 //故障反馈帧
//参数读取宏定义
#define Run_mode 0x7005
#define Iq_Ref 0x7006
#define Spd_Ref 0x700A
#define Limit_Torque 0x700B
#define Cur_Kp 0x7010
#define Cur_Ki 0x7011
#define Cur_Filt_Gain 0x7014
#define Loc_Ref 0x7016
#define Limit_Spd 0x7017
#define Limit_Cur 0x7018
#define Gain_Angle 720/32767.0
#define Bias_Angle 0x8000
#define Gain_Speed 30/32767.0
#define Bias_Speed 0x8000
#define Gain_Torque 12/32767.0
#define Bias_Torque 0x8000
#define Temp_Gain 0.1
#define Motor_Error 0x00
#define Motor_OK 0X01
enum CONTROL_MODE //控制模式定义
{
Motion_mode = 0,//运控模式
Position_mode, //位置模式
Speed_mode, //速度模式
Current_mode //电流模式
};
enum ERROR_TAG //错误回传对照
{
OK = 0,//无故障
BAT_LOW_ERR = 1,//欠压故障
OVER_CURRENT_ERR = 2,//过流
OVER_TEMP_ERR = 3,//过温
MAGNETIC_ERR = 4,//磁编码故障
HALL_ERR_ERR = 5,//HALL编码故障
NO_CALIBRATION_ERR = 6//未标定
};
typedef struct{ //小米电机结构体
uint8_t CAN_ID; //CAN ID
uint8_t MCU_ID; //MCU唯一标识符[后8位，共64位]
float Angle; //回传角度
float Speed; //回传速度
float Torque; //回传力矩
float Temp; //回传温度
uint16_t set_current;
uint16_t set_speed;
uint16_t set_position;
uint8_t error_code;
float Angle_Bias;
}MI_Motor;
extern MI_Motor mi_motor[4];//预先定义四个小米电机
/**
* @brief 大疆电机初始化
@ -246,16 +158,7 @@ extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_Hand
extern motor_measure_t *get_motor_point(uint8_t i);
//小米
extern void chack_cybergear(uint8_t ID);
extern void start_cybergear(MI_Motor *Motor);
extern void stop_cybergear(MI_Motor *Motor, uint8_t clear_error);
extern void set_mode_cybergear(MI_Motor *Motor, uint8_t Mode);
extern void set_current_cybergear(MI_Motor *Motor, float Current);
extern void set_zeropos_cybergear(MI_Motor *Motor);
extern void set_CANID_cybergear(MI_Motor *Motor, uint8_t CAN_ID);
extern void init_cybergear(MI_Motor *Motor, uint8_t Can_Id, uint8_t mode);
extern void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd);
/**
* @brief 数据处理函数

5
User/task/can_task.c
View File

@ -5,6 +5,7 @@
#include "TopDefine.h"
#include "attrTask.h"
#include "djiMotor.h"
#include "Xiaomi.h"
extern motor_measure_t *motor_3508_data;
/**
* \brief can任务
@ -16,7 +17,8 @@ void Task_Can(void *argument)
(void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
djiInit();//FIFO滤波器初始化
djiInit();//FIFO滤波器初始化
xia0miInit();//小米电机初始化
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
@ -25,6 +27,7 @@ void Task_Can(void *argument)
{
waitNewDji();//等待新数据
djiMotorEncode();//数据解析
XM_MotorEncode();//数据解析
vescMotorEncode();//数据解析
//将can数据添加到消息队列

1
User/task/go_task.c
View File

@ -8,6 +8,7 @@
#include "remote_control.h"
#include "led.h"
#include "djiMotor.h"
#include "Xiaomi.h"
#define GOUSE 1