diff --git a/User/device/djiMotor.c b/User/device/djiMotor.c index 8a32e26..01da57b 100644 --- a/User/device/djiMotor.c +++ b/User/device/djiMotor.c @@ -123,6 +123,260 @@ 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 + +#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 @@ -130,6 +384,8 @@ uint8_t dji_rx_data[8]; */ void djiMotorEncode() { + if(dji_rx_header.IDE == CAN_ID_STD) + { switch (dji_rx_header.StdId) { case 0x201: @@ -152,13 +408,35 @@ void djiMotorEncode() } 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() diff --git a/User/device/djiMotor.h b/User/device/djiMotor.h index ad363a1..a6e5b45 100644 --- a/User/device/djiMotor.h +++ b/User/device/djiMotor.h @@ -107,6 +107,96 @@ 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 大疆电机初始化 * @param none @@ -156,6 +246,17 @@ 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 数据处理函数 * @param[in] none diff --git a/User/task/dji_task.c b/User/task/dji_task.c index cca0c8e..abcdeac 100644 --- a/User/task/dji_task.c +++ b/User/task/dji_task.c @@ -15,7 +15,7 @@ extern int16_t t_result; extern motor_measure_t *trigger_motor_data; float vofa[8]; - +int tangle=0; /** * \brief 电机任务 @@ -34,7 +34,8 @@ void Task_Motor(void *argument) while(1) { - + + trigger_pos(tangle); CAN_cmd_200(t_result,0,0,0,&hcan1); osDelay(2); diff --git a/User/task/go_task.c b/User/task/go_task.c index 5137263..52eb9fa 100644 --- a/User/task/go_task.c +++ b/User/task/go_task.c @@ -7,14 +7,18 @@ #include "main.h" #include "remote_control.h" #include "led.h" +#include "djiMotor.h" #define GOUSE 1 extern RC_mess_t RC_mess; +int anglexiaomi = 0; + void Task_Go(void *argument) { #if GOUSE==1 - + HAL_Delay(500); //一定时间延时 等待电机初始化完成 + init_cybergear(&mi_motor[0], 0x01, Motion_mode);//小米电机 启动! //HAL_Delay(2000); // gimbalInit(); //HAL_GPIO_WritePin(ledGreen_GPIO_Port,ledGreen_Pin,GPIO_PIN_RESET); @@ -23,6 +27,8 @@ void Task_Go(void *argument) { LED_green(); + LED_bule(); + motor_controlmode(&mi_motor[0], 0.5, anglexiaomi, 4, 0.5 , 0.5); //小米暂时锁住就行 // gimbalFlow(); osDelay(1); } diff --git a/User/task/shoot_task.c b/User/task/shoot_task.c index 93ba555..0df63ff 100644 --- a/User/task/shoot_task.c +++ b/User/task/shoot_task.c @@ -31,6 +31,7 @@ void Task_Shoot(void *argument) Update_Encoder(&encoder); control_clutch_shoot(); + tick += delay_tick; /* 计算下一个唤醒时刻 */ osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */