242 lines
12 KiB
C
242 lines
12 KiB
C
#include "dm_motor_drv.h"
|
||
#include "dm_motor_ctrl.h"
|
||
#include "string.h"
|
||
#include "stdbool.h"
|
||
|
||
motor_t motor[num];
|
||
|
||
|
||
/**
|
||
************************************************************************
|
||
* @brief: dm4310_motor_init: DM4310电机初始化函数
|
||
* @param: void
|
||
* @retval: void
|
||
* @details: 初始化1个DM4310型号的电机，设置默认参数和控制模式。
|
||
* 设置ID、控制模式和命令模式等信息。
|
||
************************************************************************
|
||
**/
|
||
void dm_motor_init(void)
|
||
{
|
||
// 初始化Motor1和Motor2的电机结构
|
||
memset(&motor[Motor1], 0, sizeof(motor[Motor1]));
|
||
memset(&motor[Motor2], 0, sizeof(motor[Motor2]));
|
||
memset(&motor[Motor3], 0, sizeof(motor[Motor3]));
|
||
memset(&motor[Motor4], 0, sizeof(motor[Motor4]));
|
||
memset(&motor[Motor5], 0, sizeof(motor[Motor5]));
|
||
memset(&motor[Motor6], 0, sizeof(motor[Motor6]));
|
||
|
||
// 设置Motor1的电机信息
|
||
motor[Motor1].id = 0x01;
|
||
motor[Motor1].mst_id = 0x05;
|
||
motor[Motor1].tmp.read_flag = 1;
|
||
motor[Motor1].ctrl.mode = mit_mode;
|
||
motor[Motor1].ctrl.vel_set = 0.0f;
|
||
motor[Motor1].ctrl.pos_set = -1.7f;
|
||
motor[Motor1].ctrl.cur_set = 0.0f;
|
||
motor[Motor1].ctrl.kp_set = 1.0f;
|
||
motor[Motor1].ctrl.kd_set = 0.2f;
|
||
motor[Motor1].tmp.PMAX = 12.5f;
|
||
motor[Motor1].tmp.VMAX = 30.0f;
|
||
motor[Motor1].tmp.TMAX = 10.0f;
|
||
|
||
// 设置Motor2的电机信息
|
||
motor[Motor2].id = 0x02;
|
||
motor[Motor2].mst_id = 0x06;
|
||
motor[Motor2].tmp.read_flag = 1;
|
||
motor[Motor2].ctrl.mode = mit_mode;
|
||
motor[Motor2].ctrl.vel_set = 0.0f;
|
||
motor[Motor2].ctrl.pos_set = 0.0f;
|
||
motor[Motor2].ctrl.cur_set = 0.0f;
|
||
motor[Motor2].ctrl.kp_set = 1.0f;
|
||
motor[Motor2].ctrl.kd_set = 0.2f;
|
||
motor[Motor2].tmp.PMAX = 12.5f;
|
||
motor[Motor2].tmp.VMAX = 30.0f;
|
||
motor[Motor2].tmp.TMAX = 10.0f;
|
||
|
||
// 设置Motor3的电机信息
|
||
motor[Motor3].id = 0x03;
|
||
motor[Motor3].mst_id = 0x07;
|
||
motor[Motor3].tmp.read_flag = 1;
|
||
motor[Motor3].ctrl.mode = mit_mode;
|
||
motor[Motor3].ctrl.vel_set = 0.0f;
|
||
motor[Motor3].ctrl.pos_set = 0.0f;
|
||
motor[Motor3].ctrl.cur_set = 0.0f;
|
||
motor[Motor3].ctrl.kp_set = 1.0f;
|
||
motor[Motor3].ctrl.kd_set = 0.2f;
|
||
motor[Motor3].tmp.PMAX = 12.5f;
|
||
motor[Motor3].tmp.VMAX = 30.0f;
|
||
motor[Motor3].tmp.TMAX = 10.0f;
|
||
|
||
// 设置Motor4的电机信息
|
||
motor[Motor4].id = 0x04;
|
||
motor[Motor4].mst_id = 0x08;
|
||
motor[Motor4].tmp.read_flag = 1;
|
||
motor[Motor4].ctrl.mode = mit_mode;
|
||
motor[Motor4].ctrl.vel_set = 0.0f;
|
||
motor[Motor4].ctrl.pos_set = 0.0f;
|
||
motor[Motor4].ctrl.cur_set = 0.0f;
|
||
motor[Motor4].ctrl.kp_set = 0.0f;
|
||
motor[Motor4].ctrl.kd_set = 0.0f;
|
||
motor[Motor4].tmp.PMAX = 12.5f;
|
||
motor[Motor4].tmp.VMAX = 30.0f;
|
||
motor[Motor4].tmp.TMAX = 10.0f;
|
||
}
|
||
/**
|
||
************************************************************************
|
||
* @brief: read_all_motor_data: 读取电机的所有寄存器的数据信息
|
||
* @param: motor_t：电机参数结构体
|
||
* @retval: void
|
||
* @details: 逐次发送读取命令
|
||
************************************************************************
|
||
**/
|
||
void read_all_motor_data(motor_t *motor)
|
||
{
|
||
switch (motor->tmp.read_flag)
|
||
{
|
||
case 1: read_motor_data(motor->id, RID_UV_VALUE); break; // UV_Value
|
||
case 2: read_motor_data(motor->id, RID_KT_VALUE); break; // KT_Value
|
||
case 3: read_motor_data(motor->id, RID_OT_VALUE); break; // OT_Value
|
||
case 4: read_motor_data(motor->id, RID_OC_VALUE); break; // OC_Value
|
||
case 5: read_motor_data(motor->id, RID_ACC); break; // ACC
|
||
case 6: read_motor_data(motor->id, RID_DEC); break; // DEC
|
||
case 7: read_motor_data(motor->id, RID_MAX_SPD); break; // MAX_SPD
|
||
case 8: read_motor_data(motor->id, RID_MST_ID); break; // MST_ID
|
||
case 9: read_motor_data(motor->id, RID_ESC_ID); break; // ESC_ID
|
||
case 10: read_motor_data(motor->id, RID_TIMEOUT); break; // TIMEOUT
|
||
case 11: read_motor_data(motor->id, RID_CMODE); break; // CTRL_MODE
|
||
case 12: read_motor_data(motor->id, RID_DAMP); break; // Damp
|
||
case 13: read_motor_data(motor->id, RID_INERTIA); break; // Inertia
|
||
case 14: read_motor_data(motor->id, RID_HW_VER); break; // Rsv1
|
||
case 15: read_motor_data(motor->id, RID_SW_VER); break; // sw_ver
|
||
case 16: read_motor_data(motor->id, RID_SN); break; // Rsv2
|
||
case 17: read_motor_data(motor->id, RID_NPP); break; // NPP
|
||
case 18: read_motor_data(motor->id, RID_RS); break; // Rs
|
||
case 19: read_motor_data(motor->id, RID_LS); break; // Ls
|
||
case 20: read_motor_data(motor->id, RID_FLUX); break; // Flux
|
||
case 21: read_motor_data(motor->id, RID_GR); break; // Gr
|
||
case 22: read_motor_data(motor->id, RID_PMAX); break; // PMAX
|
||
case 23: read_motor_data(motor->id, RID_VMAX); break; // VMAX
|
||
case 24: read_motor_data(motor->id, RID_TMAX); break; // TMAX
|
||
case 25: read_motor_data(motor->id, RID_I_BW); break; // I_BW
|
||
case 26: read_motor_data(motor->id, RID_KP_ASR); break; // KP_ASR
|
||
case 27: read_motor_data(motor->id, RID_KI_ASR); break; // KI_ASR
|
||
case 28: read_motor_data(motor->id, RID_KP_APR); break; // KP_APR
|
||
case 29: read_motor_data(motor->id, RID_KI_APR); break; // KI_APR
|
||
case 30: read_motor_data(motor->id, RID_OV_VALUE); break; // OV_Value
|
||
case 31: read_motor_data(motor->id, RID_GREF); break; // GREF
|
||
case 32: read_motor_data(motor->id, RID_DETA); break; // Deta
|
||
case 33: read_motor_data(motor->id, RID_V_BW); break; // V_BW
|
||
case 34: read_motor_data(motor->id, RID_IQ_CL); break; // IQ_c1
|
||
case 35: read_motor_data(motor->id, RID_VL_CL); break; // VL_c1
|
||
case 36: read_motor_data(motor->id, RID_CAN_BR); break; // can_br
|
||
case 37: read_motor_data(motor->id, RID_SUB_VER); break; // sub_ver
|
||
case 38: read_motor_data(motor->id, RID_U_OFF); break; // u_off
|
||
case 39: read_motor_data(motor->id, RID_V_OFF); break; // v_off
|
||
case 40: read_motor_data(motor->id, RID_K1); break; // k1
|
||
case 41: read_motor_data(motor->id, RID_K2); break; // k2
|
||
case 42: read_motor_data(motor->id, RID_M_OFF); break; // m_off
|
||
case 43: read_motor_data(motor->id, RID_DIR); break; // dir
|
||
case 44: read_motor_data(motor->id, RID_P_M); break; // pm
|
||
case 45: read_motor_data(motor->id, RID_X_OUT); break; // xout
|
||
}
|
||
}
|
||
/**
|
||
************************************************************************
|
||
* @brief: receive_motor_data: 接收电机返回的数据信息
|
||
* @param: motor_t：电机参数结构体
|
||
* @param: data：接收的数据
|
||
* @retval: void
|
||
* @details: 逐次接收电机回传的参数信息
|
||
************************************************************************
|
||
**/
|
||
void receive_motor_data(motor_t *motor, uint8_t *data)
|
||
{
|
||
if(motor->tmp.read_flag == 0)
|
||
return ;
|
||
|
||
float_type_u y;
|
||
|
||
if(data[2] == 0x33)
|
||
{
|
||
uint16_t rid_value = data[3];
|
||
y.b_val[0] = data[4];
|
||
y.b_val[1] = data[5];
|
||
y.b_val[2] = data[6];
|
||
y.b_val[3] = data[7];
|
||
|
||
switch (rid_value)
|
||
{
|
||
case RID_UV_VALUE: motor->tmp.UV_Value = y.f_val; motor->tmp.read_flag = 2; break;
|
||
case RID_KT_VALUE: motor->tmp.KT_Value = y.f_val; motor->tmp.read_flag = 3; break;
|
||
case RID_OT_VALUE: motor->tmp.OT_Value = y.f_val; motor->tmp.read_flag = 4; break;
|
||
case RID_OC_VALUE: motor->tmp.OC_Value = y.f_val; motor->tmp.read_flag = 5; break;
|
||
case RID_ACC: motor->tmp.ACC = y.f_val; motor->tmp.read_flag = 6; break;
|
||
case RID_DEC: motor->tmp.DEC = y.f_val; motor->tmp.read_flag = 7; break;
|
||
case RID_MAX_SPD: motor->tmp.MAX_SPD = y.f_val; motor->tmp.read_flag = 8; break;
|
||
case RID_MST_ID: motor->tmp.MST_ID = y.u_val; motor->tmp.read_flag = 9; break;
|
||
case RID_ESC_ID: motor->tmp.ESC_ID = y.u_val; motor->tmp.read_flag = 10; break;
|
||
case RID_TIMEOUT: motor->tmp.TIMEOUT = y.u_val; motor->tmp.read_flag = 11; break;
|
||
case RID_CMODE: motor->tmp.cmode = y.u_val; motor->tmp.read_flag = 12; break;
|
||
case RID_DAMP: motor->tmp.Damp = y.f_val; motor->tmp.read_flag = 13; break;
|
||
case RID_INERTIA: motor->tmp.Inertia = y.f_val; motor->tmp.read_flag = 14; break;
|
||
case RID_HW_VER: motor->tmp.hw_ver = y.u_val; motor->tmp.read_flag = 15; break;
|
||
case RID_SW_VER: motor->tmp.sw_ver = y.u_val; motor->tmp.read_flag = 16; break;
|
||
case RID_SN: motor->tmp.SN = y.u_val; motor->tmp.read_flag = 17; break;
|
||
case RID_NPP: motor->tmp.NPP = y.u_val; motor->tmp.read_flag = 18; break;
|
||
case RID_RS: motor->tmp.Rs = y.f_val; motor->tmp.read_flag = 19; break;
|
||
case RID_LS: motor->tmp.Ls = y.f_val; motor->tmp.read_flag = 20; break;
|
||
case RID_FLUX: motor->tmp.Flux = y.f_val; motor->tmp.read_flag = 21; break;
|
||
case RID_GR: motor->tmp.Gr = y.f_val; motor->tmp.read_flag = 22; break;
|
||
case RID_PMAX: motor->tmp.PMAX = y.f_val; motor->tmp.read_flag = 23; break;
|
||
case RID_VMAX: motor->tmp.VMAX = y.f_val; motor->tmp.read_flag = 24; break;
|
||
case RID_TMAX: motor->tmp.TMAX = y.f_val; motor->tmp.read_flag = 25; break;
|
||
case RID_I_BW: motor->tmp.I_BW = y.f_val; motor->tmp.read_flag = 26; break;
|
||
case RID_KP_ASR: motor->tmp.KP_ASR = y.f_val; motor->tmp.read_flag = 27; break;
|
||
case RID_KI_ASR: motor->tmp.KI_ASR = y.f_val; motor->tmp.read_flag = 28; break;
|
||
case RID_KP_APR: motor->tmp.KP_APR = y.f_val; motor->tmp.read_flag = 29; break;
|
||
case RID_KI_APR: motor->tmp.KI_APR = y.f_val; motor->tmp.read_flag = 30; break;
|
||
case RID_OV_VALUE: motor->tmp.OV_Value = y.f_val; motor->tmp.read_flag = 31; break;
|
||
case RID_GREF: motor->tmp.GREF = y.f_val; motor->tmp.read_flag = 32; break;
|
||
case RID_DETA: motor->tmp.Deta = y.f_val; motor->tmp.read_flag = 33; break;
|
||
case RID_V_BW: motor->tmp.V_BW = y.f_val; motor->tmp.read_flag = 34; break;
|
||
case RID_IQ_CL: motor->tmp.IQ_cl = y.f_val; motor->tmp.read_flag = 35; break;
|
||
case RID_VL_CL: motor->tmp.VL_cl = y.f_val; motor->tmp.read_flag = 36; break;
|
||
case RID_CAN_BR: motor->tmp.can_br = y.u_val; motor->tmp.read_flag = 37; break;
|
||
case RID_SUB_VER: motor->tmp.sub_ver = y.u_val; motor->tmp.read_flag = 38; break;
|
||
case RID_U_OFF: motor->tmp.u_off = y.f_val; motor->tmp.read_flag = 39; break;
|
||
case RID_V_OFF: motor->tmp.v_off = y.f_val; motor->tmp.read_flag = 40; break;
|
||
case RID_K1: motor->tmp.k1 = y.f_val; motor->tmp.read_flag = 41; break;
|
||
case RID_K2: motor->tmp.k2 = y.f_val; motor->tmp.read_flag = 42; break;
|
||
case RID_M_OFF: motor->tmp.m_off = y.f_val; motor->tmp.read_flag = 43; break;
|
||
case RID_DIR: motor->tmp.dir = y.f_val; motor->tmp.read_flag = 44; break;
|
||
case RID_P_M: motor->tmp.p_m = y.f_val; motor->tmp.read_flag = 45; break;
|
||
case RID_X_OUT: motor->tmp.x_out = y.f_val; motor->tmp.read_flag = 0 ; break;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
************************************************************************
|
||
* @brief: fdcan1_rx_callback: CAN1接收回调函数
|
||
* @param: void
|
||
* @retval: void
|
||
* @details: 处理CAN1接收中断回调，根据接收到的ID和数据，执行相应的处理。
|
||
* 当接收到ID为0时，调用dm4310_fbdata函数更新Motor的反馈数据。
|
||
************************************************************************
|
||
**/
|
||
void fdcan1_rx_callback(void)
|
||
{
|
||
uint16_t rec_id;
|
||
uint8_t rx_data[8] = {0};
|
||
fdcanx_receive(&hfdcan1, &rec_id, rx_data);
|
||
switch (rec_id)
|
||
{
|
||
case 0x05: dm_motor_fbdata(&motor[Motor1], rx_data); receive_motor_data(&motor[Motor1], rx_data); break;
|
||
case 0x06: dm_motor_fbdata(&motor[Motor2], rx_data); receive_motor_data(&motor[Motor2], rx_data); break;
|
||
case 0x07: dm_motor_fbdata(&motor[Motor3], rx_data); receive_motor_data(&motor[Motor3], rx_data); break;
|
||
case 0x08: dm_motor_fbdata(&motor[Motor4], rx_data); receive_motor_data(&motor[Motor4], rx_data); break;
|
||
}
|
||
}
|
||
|
||
|