diff --git a/User/bsp/can_init.c b/User/bsp/can_init.c index feb34ad..aee2407 100644 --- a/User/bsp/can_init.c +++ b/User/bsp/can_init.c @@ -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); - - } diff --git a/User/device/Xiaomi.c b/User/device/Xiaomi.c new file mode 100644 index 0000000..1166d58 --- /dev/null +++ b/User/device/Xiaomi.c @@ -0,0 +1,314 @@ +#include "Xiaomi.h" +#include "can_it.h" +#include "can_init.h" +#include "TopDefine.h" +#include +#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<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那里已经初始化了 +} + diff --git a/User/device/Xiaomi.h b/User/device/Xiaomi.h new file mode 100644 index 0000000..edfed60 --- /dev/null +++ b/User/device/Xiaomi.h @@ -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 diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index 01da57b..a514fad 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -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 -#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 + #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<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); } diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index a6e5b45..bdc100f 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -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 数据处理函数 diff --git a/User/task/can_task.c b/User/task/can_task.c index dee3e4b..3d682bd 100644 --- a/User/task/can_task.c +++ b/User/task/can_task.c @@ -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数据添加到消息队列 diff --git a/User/task/go_task.c b/User/task/go_task.c index 52eb9fa..809a0bb 100644 --- a/User/task/go_task.c +++ b/User/task/go_task.c @@ -8,6 +8,7 @@ #include "remote_control.h" #include "led.h" #include "djiMotor.h" +#include "Xiaomi.h" #define GOUSE 1