cleanup: remove unused motor drivers and simplify motor_def.h
- Remove DJImotor, HTmotor, LKmotor, step_motor, servo_motor directories - Remove Motor_Control_Setting_s and related enums (Closeloop_Type_e, Feedback_Source_e, Feedfoward_Type_e, MOTOR_SEND_MODE_e, Motor_Working_Type_e) - Merge motor_reverse_flag and feedback_reverse_flag into output_reverse - Hardcode DM motor control: IMU angle feedback → angle PID → velocity output - Simplify Motor_Init_Config_s to only: PID params, output_reverse, motor_type, CAN config
This commit is contained in:
@@ -341,14 +341,15 @@ FDCAN1.DataPrescaler=3
|
||||
FDCAN1.DataSyncJumpWidth=10
|
||||
FDCAN1.DataTimeSeg1=29
|
||||
FDCAN1.DataTimeSeg2=10
|
||||
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,RxFifo1ElmtsNbr,NominalTimeSeg2,DataPrescaler,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ProtocolException
|
||||
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,RxFifo1ElmtsNbr,NominalTimeSeg2,DataPrescaler,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ProtocolException,NominalSyncJumpWidth
|
||||
FDCAN1.NominalPrescaler=3
|
||||
FDCAN1.NominalSyncJumpWidth=10
|
||||
FDCAN1.NominalTimeSeg1=29
|
||||
FDCAN1.NominalTimeSeg2=10
|
||||
FDCAN1.ProtocolException=ENABLE
|
||||
FDCAN1.RxFifo0ElmtsNbr=4
|
||||
FDCAN1.RxFifo1ElmtsNbr=4
|
||||
FDCAN1.StdFiltersNbr=14
|
||||
FDCAN1.StdFiltersNbr=1
|
||||
FDCAN1.TxFifoQueueElmtsNbr=4
|
||||
FDCAN2.CalculateBaudRateNominal=1000000
|
||||
FDCAN2.CalculateTimeBitNominal=1000
|
||||
|
||||
@@ -43,13 +43,7 @@ void GimbalInit()
|
||||
.other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle,
|
||||
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2],
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP,
|
||||
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
||||
},
|
||||
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
||||
.motor_type = DM_MOTOR,
|
||||
},
|
||||
.ctrl_mode = DM_MODE_VELOCITY,
|
||||
@@ -77,13 +71,7 @@ void GimbalInit()
|
||||
.other_angle_feedback_ptr = &gimbal_IMU_data->Pitch,
|
||||
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = OTHER_FEED,
|
||||
.speed_feedback_source = OTHER_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP,
|
||||
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
||||
},
|
||||
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
||||
.motor_type = DM_MOTOR,
|
||||
},
|
||||
.ctrl_mode = DM_MODE_VELOCITY,
|
||||
|
||||
@@ -1,416 +0,0 @@
|
||||
#include "dji_motor.h"
|
||||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
|
||||
static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在control任务中遍历该指针数组进行pid计算
|
||||
|
||||
|
||||
#ifdef FDCAN
|
||||
static CANInstance sender_assignment[15] = {
|
||||
[0] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[1] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[2] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[3] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[4] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[5] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[6] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[7] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[8] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[9] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[10] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[11] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[12] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[13] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
[14] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
|
||||
};
|
||||
|
||||
#else
|
||||
|
||||
/**
|
||||
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
|
||||
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
|
||||
*
|
||||
* @note 因为只用于发送,所以不需要在bsp_can中注册
|
||||
*
|
||||
* C610(m2006)/C620(m3508):0x1ff,0x200;
|
||||
* GM6020:0x1ff,0x2ff
|
||||
* 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
|
||||
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
|
||||
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
|
||||
*/
|
||||
static CANInstance sender_assignment[6] = {
|
||||
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
|
||||
* flag的初始化在 MotorSenderGrouping()中进行
|
||||
*/
|
||||
static uint8_t sender_enable_flag[15] = {0};
|
||||
|
||||
/**
|
||||
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
|
||||
* 并对电机进行分组以便处理多电机控制命令
|
||||
*/
|
||||
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
|
||||
{
|
||||
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
|
||||
uint8_t motor_send_num;
|
||||
uint8_t motor_grouping;
|
||||
|
||||
uint8_t grouping_offset;
|
||||
//通过CAN计算分组偏移量
|
||||
if(config->can_handle == &hcan1)
|
||||
{
|
||||
grouping_offset=0;
|
||||
}
|
||||
else if(config->can_handle == &hcan2)
|
||||
{
|
||||
grouping_offset=3;
|
||||
}
|
||||
else
|
||||
{
|
||||
grouping_offset=6;
|
||||
}
|
||||
|
||||
switch (motor->motor_type)
|
||||
{
|
||||
case M2006:
|
||||
case M3508:
|
||||
if (motor_id < 4) // 根据ID分组
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = grouping_offset + 1;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = grouping_offset + 0;
|
||||
}
|
||||
|
||||
// 计算接收id并设置分组发送id
|
||||
config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
|
||||
// 检查是否发生id冲突
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case GM6020:
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = grouping_offset + 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = grouping_offset + 2;
|
||||
}
|
||||
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case GM6020_NEW:
|
||||
if(config->can_handle == &hcan1)
|
||||
{
|
||||
grouping_offset=9;
|
||||
}
|
||||
else if(config->can_handle == &hcan2)
|
||||
{
|
||||
grouping_offset=11;
|
||||
}
|
||||
else
|
||||
{
|
||||
grouping_offset=13;
|
||||
}
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
motor_grouping = grouping_offset + 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_send_num = motor_id - 4;
|
||||
motor_grouping = grouping_offset + 1;
|
||||
}
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
|
||||
motor->message_num = motor_send_num;
|
||||
motor->sender_group = motor_grouping;
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
|
||||
{
|
||||
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
|
||||
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
|
||||
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
|
||||
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default: // other motors should not be registered here
|
||||
while (1)
|
||||
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @todo 是否可以简化多圈角度的计算？
|
||||
* @brief 根据返回的can_instance对反馈报文进行解析
|
||||
*
|
||||
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
|
||||
*/
|
||||
static void DecodeDJIMotor(CANInstance *_instance)
|
||||
{
|
||||
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
|
||||
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
|
||||
uint8_t *rxbuff = _instance->rx_buff;
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
|
||||
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
|
||||
|
||||
DaemonReload(motor->daemon);
|
||||
motor->dt = DWT_GetDeltaT(&motor->feed_cnt);
|
||||
|
||||
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
|
||||
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
|
||||
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
|
||||
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
|
||||
measure->temperature = rxbuff[6];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
if (measure->ecd - measure->last_ecd > 4096)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -4096)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void DJIMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
|
||||
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
|
||||
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
|
||||
}
|
||||
|
||||
// 电机初始化,返回一个电机实例
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
|
||||
memset(instance, 0, sizeof(DJIMotorInstance));
|
||||
|
||||
// motor basic setting 电机基本设置
|
||||
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
|
||||
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
|
||||
|
||||
// motor controller init 电机控制器初始化
|
||||
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
|
||||
instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
|
||||
// 后续增加电机前馈控制器(速度和电流)
|
||||
|
||||
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
|
||||
MotorSenderGrouping(instance, &config->can_init_config);
|
||||
|
||||
// 注册电机到CAN总线
|
||||
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
|
||||
config->can_init_config.id = instance; // set id,eq to address(it is identity)
|
||||
instance->motor_can_instance = CANRegister(&config->can_init_config);
|
||||
|
||||
// 注册守护线程
|
||||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
};
|
||||
instance->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
DJIMotorEnable(instance);
|
||||
dji_motor_instance[idx++] = instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
|
||||
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
|
||||
{
|
||||
if (loop == ANGLE_LOOP)
|
||||
motor->motor_settings.angle_feedback_source = type;
|
||||
else if (loop == SPEED_LOOP)
|
||||
motor->motor_settings.speed_feedback_source = type;
|
||||
else
|
||||
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
|
||||
}
|
||||
|
||||
void DJIMotorStop(DJIMotorInstance *motor)
|
||||
{
|
||||
motor->motor_controller.speed_PID.Iout = 0; // 停止时清除速度环积分项
|
||||
motor->motor_controller.angle_PID.Iout = 0; // 停止时清除位置环积分项
|
||||
motor->motor_controller.current_PID.Iout = 0; // 停止时清
|
||||
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void DJIMotorEnable(DJIMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
/* 修改电机的实际闭环对象 */
|
||||
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = outer_loop;
|
||||
}
|
||||
|
||||
// 设置参考值
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
void DJIMotorSetExternal(DJIMotorInstance *motor, float external)
|
||||
{
|
||||
motor->motor_controller.output_external = external;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 为所有电机实例计算三环PID,发送控制报文
|
||||
void DJIMotorControl()
|
||||
{
|
||||
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
|
||||
uint8_t group, num; // 电机组号和组内编号
|
||||
int16_t set; // 电机控制CAN发送设定值
|
||||
DJIMotorInstance *motor;
|
||||
Motor_Control_Setting_s *motor_setting; // 电机控制参数
|
||||
Motor_Controller_s *motor_controller; // 电机控制器
|
||||
DJI_Motor_Measure_s *measure; // 电机测量值
|
||||
float pid_measure, pid_ref, output_external; // 电机PID测量值和设定值
|
||||
|
||||
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{ // 减小访存开销,先保存指针引用
|
||||
motor = dji_motor_instance[i];
|
||||
motor_setting = &motor->motor_settings;
|
||||
motor_controller = &motor->motor_controller;
|
||||
measure = &motor->measure;
|
||||
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
|
||||
output_external = motor_controller->output_external;
|
||||
|
||||
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
pid_ref *= -1; // 设置反转
|
||||
|
||||
// pid_ref会顺次通过被启用的闭环充当数据的载体
|
||||
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
|
||||
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (motor_setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
|
||||
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->speed_feedforward_ptr;
|
||||
|
||||
if (motor_setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor_controller->other_speed_feedback_ptr;
|
||||
else // MOTOR_FEED
|
||||
pid_measure = measure->speed_aps;
|
||||
// 更新pid_ref进入下一个环
|
||||
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
|
||||
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->current_feedforward_ptr;
|
||||
if (motor_setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
// 获取最终输出
|
||||
if (motor->motor_settings.send_mode == SEND_MODE_NORMAL)
|
||||
{
|
||||
set = (int16_t)pid_ref;
|
||||
}
|
||||
else
|
||||
{
|
||||
set = (int16_t)output_external;
|
||||
}
|
||||
// 分组填入发送数据
|
||||
group = motor->sender_group;
|
||||
num = motor->message_num;
|
||||
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
|
||||
|
||||
// 若该电机处于停止状态,直接将buff置零
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
|
||||
}
|
||||
|
||||
// 遍历flag,检查是否要发送这一帧报文
|
||||
#ifdef FDCAN
|
||||
for (size_t i = 0; i < 15; ++i)
|
||||
#else
|
||||
for (size_t i = 0; i < 6; ++i)
|
||||
#endif
|
||||
{
|
||||
if (sender_enable_flag[i])
|
||||
{
|
||||
CANTransmit(&sender_assignment[i], 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,133 +0,0 @@
|
||||
/**
|
||||
* @file dji_motor.h
|
||||
* @author neozng
|
||||
* @brief DJI智能电机头文件
|
||||
* @version 0.2
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @todo 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
|
||||
2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
|
||||
|
||||
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef DJI_MOTOR_H
|
||||
#define DJI_MOTOR_H
|
||||
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "stdint.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define DJI_MOTOR_CNT 12
|
||||
|
||||
/* 滤波系数设置为1的时候即关闭滤波 */
|
||||
#define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9
|
||||
#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
|
||||
|
||||
/* DJI电机CAN反馈信息*/
|
||||
typedef struct
|
||||
{
|
||||
uint16_t last_ecd; // 上一次读取的编码器值
|
||||
uint16_t ecd; // 0-8191,刻度总共有8192格
|
||||
float angle_single_round; // 单圈角度
|
||||
float speed_aps; // 角速度,单位为:度/秒
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperature; // 温度 Celsius
|
||||
|
||||
float total_angle; // 总角度,注意方向
|
||||
int32_t total_round; // 总圈数,注意方向
|
||||
} DJI_Motor_Measure_s;
|
||||
|
||||
/**
|
||||
* @brief DJI intelligent motor typedef
|
||||
*
|
||||
*/
|
||||
typedef struct
|
||||
{
|
||||
DJI_Motor_Measure_s measure; // 电机测量值
|
||||
Motor_Control_Setting_s motor_settings; // 电机设置
|
||||
Motor_Controller_s motor_controller; // 电机控制器
|
||||
|
||||
CANInstance *motor_can_instance; // 电机CAN实例
|
||||
// 分组发送设置
|
||||
uint8_t sender_group;
|
||||
uint8_t message_num;
|
||||
|
||||
Motor_Type_e motor_type; // 电机类型
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
DaemonInstance* daemon;
|
||||
uint32_t feed_cnt;
|
||||
float dt;
|
||||
} DJIMotorInstance;
|
||||
|
||||
/**
|
||||
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
|
||||
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
|
||||
* recommend: type xxxinitStructure = {.member1=xx,
|
||||
* .member2=xx,
|
||||
* ....};
|
||||
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
|
||||
* 并减小DJIMotorControl()任务的运行频率.
|
||||
*
|
||||
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
|
||||
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
|
||||
*
|
||||
* @param config 电机初始化结构体,包含了电机控制设置,电机PID参数设置,电机类型以及电机挂载的CAN设置
|
||||
*
|
||||
* @return DJIMotorInstance*
|
||||
*/
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config);
|
||||
|
||||
/**
|
||||
* @brief 被application层的应用调用,给电机设定参考值.
|
||||
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
|
||||
*
|
||||
* @param motor 要设置的电机
|
||||
* @param ref 设定参考值
|
||||
*/
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref);
|
||||
|
||||
/**
|
||||
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
|
||||
*
|
||||
* @param motor 要切换反馈数据来源的电机
|
||||
* @param loop 要切换反馈数据来源的控制闭环
|
||||
* @param type 目标反馈模式
|
||||
*/
|
||||
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type);
|
||||
|
||||
/**
|
||||
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
|
||||
*/
|
||||
void DJIMotorControl();
|
||||
|
||||
/**
|
||||
* @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
|
||||
*
|
||||
*/
|
||||
void DJIMotorStop(DJIMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 启动电机,此时电机会响应设定值
|
||||
* 初始化时不需要此函数,因为stop_flag的默认值为0
|
||||
*
|
||||
*/
|
||||
void DJIMotorEnable(DJIMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 修改电机闭环目标(外层闭环)
|
||||
*
|
||||
* @param motor 要修改的电机实例指针
|
||||
* @param outer_loop 外层闭环类型
|
||||
*/
|
||||
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop);
|
||||
|
||||
|
||||
void DJIMotorSetExternal(DJIMotorInstance *motor, float external);
|
||||
|
||||
#endif // !DJI_MOTOR_H
|
||||
@@ -1,470 +0,0 @@
|
||||
# dji_motor
|
||||
|
||||
<p align='right'>neozng1@hnu.edu.cn</p>
|
||||
|
||||
> TODO:
|
||||
>
|
||||
> 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
|
||||
> 2. 为M2006和M3508增加开环的零位校准函数
|
||||
|
||||
---
|
||||
|
||||
> 建议将电机的反馈频率通过RoboMaster Assistant统一设置为500Hz。当前默认的`MotorTask()`执行频率为500Hz，若不修改电机反馈频率可能导致单条总线挂载的电机数量有限，且容易出现帧错误和仲裁失败的情况。
|
||||
|
||||
## 总览和封装说明
|
||||
|
||||
> 如果你不需要理解该模块的工作原理，你只需要查看这一小节。
|
||||
|
||||
dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。
|
||||
|
||||
**==设定值的单位==**
|
||||
|
||||
1. ==位置环为**角度制**（0-360，total_angle可以为任意值）==
|
||||
|
||||
2. ==速度环为角速度，单位为**度/每秒**（deg/sec）==
|
||||
|
||||
3. ==电流环为A==
|
||||
|
||||
4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）==
|
||||
|
||||
==M3508的输入设定为-20A~20A （-16384~16384）==
|
||||
|
||||
==M2006的输入设定为-10A~10A （-10000~10000）==
|
||||
|
||||
如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
|
||||
|
||||
要获得一个电机，请通过`DJIMotorInit()`并传入一些参数，他就会返回一个电机的指针。你也不再需要查看这些电机和电调的说明书，**只需要设置其电机id**（6020为拨码开关值，2006和3508为电调的闪动次数），该模块会自动为你计算CAN发送和接收ID并搞定所有硬件层的琐事。
|
||||
|
||||
初始化电机时，你需要传入的参数包括：
|
||||
|
||||
- **电机挂载的CAN总线设置**，CAN1 or CAN2，以及电机的id，使用`can_instance_config_s`封装，只需要设置这两个参数:
|
||||
|
||||
```c
|
||||
CAN_HandleTypeDef *can_handle;
|
||||
uint32_t tx_id; // tx_id设置为电机id,不需要查说明书计算，直接为电调的闪动次数或拨码开关值，为1-8
|
||||
```
|
||||
|
||||
- **电机类型**，使用`Motor_Type_e`：
|
||||
|
||||
```c
|
||||
GM6020 = 0
|
||||
M3508 = 1
|
||||
M2006 = 2
|
||||
```
|
||||
|
||||
- **电机控制设置**
|
||||
|
||||
- 闭环类型
|
||||
|
||||
```c
|
||||
OPEN_LOOP
|
||||
CURRENT_LOOP
|
||||
SPEED_LOOP
|
||||
ANGLE_LOOP
|
||||
CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环
|
||||
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
|
||||
CURRENT_LOOP | SPEED_LOOP |ANGLE_LOOP // 三环全开
|
||||
```
|
||||
|
||||
- 是否反转
|
||||
|
||||
```c
|
||||
MOTOR_DIRECTION_NORMAL
|
||||
MOTOR_DIRECTION_REVERSE
|
||||
```
|
||||
|
||||
- 是否其他反馈来源，以及他们对应的数据指针（如果有的话）
|
||||
|
||||
```c
|
||||
MOTOR_FEED = 0
|
||||
OTHER_FEED = 1
|
||||
---
|
||||
|
||||
// 电流只能从电机传感器获得所以无法设置其他来源
|
||||
```
|
||||
|
||||
- 每个环的PID参数以及是否使用改进功能，以及其他反馈来源指针（如果在上一步启用了其他数据来源）
|
||||
|
||||
```c
|
||||
typedef struct // config parameter
|
||||
{
|
||||
float Kp;
|
||||
float Ki;
|
||||
float Kd;
|
||||
|
||||
float MaxOut; // 输出限幅
|
||||
// 以下是优化参数
|
||||
float IntegralLimit; // 积分限幅
|
||||
float DeadBand; // 死区
|
||||
float CoefA; // For Changing Integral
|
||||
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
|
||||
float Output_LPF_RC; // RC = 1/omegac
|
||||
float Derivative_LPF_RC;
|
||||
|
||||
PID_Improvement_e Improve; // 优化环节，定义在下一个代码块
|
||||
} PIDInit_config_s;
|
||||
// 只有当你设启用了对应的优化环节，优化参数才会生效
|
||||
```
|
||||
|
||||
```c
|
||||
typedef enum
|
||||
{
|
||||
NONE = 0b00000000,
|
||||
Integral_Limit = 0b00000001,
|
||||
Derivative_On_Measurement = 0b00000010,
|
||||
Trapezoid_Intergral = 0b00000100,
|
||||
Proportional_On_Measurement = 0b00001000,
|
||||
OutputFilter = 0b00010000,
|
||||
ChangingIntegrationRate = 0b00100000,
|
||||
DerivativeFilter = 0b01000000,
|
||||
ErrorHandle = 0b10000000,
|
||||
} PID_Improvement_e;
|
||||
// 若希望使用多个环节的优化，这样就行：Integral_Limit |Trapezoid_Intergral|...|...
|
||||
```
|
||||
|
||||
```c
|
||||
float *other_angle_feedback_ptr
|
||||
float *other_speed_feedback_ptr
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
推荐的初始化参数编写格式如下：
|
||||
|
||||
```c
|
||||
Motor_Init_Config_s config = {
|
||||
.motor_type = M3508, // 要注册的电机为3508电机
|
||||
.can_init_config = {.can_handle = &hcan1, // 挂载在CAN1
|
||||
.tx_id = 1}, // C620每隔一段时间闪动1次,设置为1
|
||||
// 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转,最外层闭环为速度环
|
||||
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED,
|
||||
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL},
|
||||
// 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数
|
||||
// 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针
|
||||
.controller_param_init_config = {.current_PID = {.Improve = 0,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.DeadBand = 0,
|
||||
.MaxOut = 4000},
|
||||
.speed_PID = {.Improve = 0,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.DeadBand = 0,
|
||||
.MaxOut = 4000}}};
|
||||
|
||||
dji_motor_instance *djimotor = DJIMotorInit(config); // 设置好参数后进行初始化并保留返回的指针
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
要控制一个DJI电机，我们提供了2个接口：
|
||||
|
||||
```c
|
||||
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
|
||||
|
||||
void DJIMotorChangeFeed(dji_motor_instance *motor,
|
||||
Closeloop_Type_e loop,
|
||||
Feedback_Source_e type);
|
||||
```
|
||||
|
||||
调用第一个并传入设定值，它会自动根据你设定的PID参数进行动作。 如果对不同闭环都有参考输入,则设置最外层的闭环(通过此函数)并将剩下的参考输入通过前馈数据指针进行设定
|
||||
|
||||
调用第二个并设定要修改的反馈环节和反馈类型，它会将反馈数据指针切换到你设定好的变量（需要在初始化的时候设置反馈指针）。
|
||||
|
||||
**如果需要获取电机的反馈数据**（如小陀螺模式需要根据麦克纳姆轮逆运动学解算底盘速度），直接通过你拥有的`dji_motor_instance`访问成员变量：
|
||||
|
||||
```c
|
||||
// LeftForwardMotor是一个dji_motor_instance实例
|
||||
float speed=LeftForwardMotor->motor_measure->speed_rpm;
|
||||
...
|
||||
```
|
||||
|
||||
***现在，忘记PID的计算和发送、接收以及协议解析，专注于模块之间的逻辑交互吧。***
|
||||
|
||||
---
|
||||
|
||||
## 代码结构
|
||||
|
||||
.h文件内包括了外部接口和类型定义,以及模块对应的宏。c文件内为私有函数和外部接口的定义。
|
||||
|
||||
motor_def.h内包含了一些电机通用的定义。
|
||||
|
||||
## 类型定义
|
||||
|
||||
```c
|
||||
#define DJI_MOTOR_CNT 12
|
||||
#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
|
||||
|
||||
typedef struct /* DJI电机CAN反馈信息*/
|
||||
{
|
||||
uint16_t ecd;
|
||||
uint16_t last_ecd;
|
||||
int16_t speed_rpm;
|
||||
int16_t given_current;
|
||||
uint8_t temperate;
|
||||
int16_t total_round;
|
||||
int32_t total_angle;
|
||||
} dji_motor_measure;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* motor measurement recv from CAN feedback */
|
||||
dji_motor_measure motor_measure;
|
||||
/* basic config of a motor*/
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
/* controller used in the motor (3 loops)*/
|
||||
Motor_Controller_s motor_controller;
|
||||
/* the CAN instance own by motor instance*/
|
||||
can_instance motor_can_instance;
|
||||
/* sender assigment*/
|
||||
uint8_t sender_group;
|
||||
uint8_t message_num;
|
||||
|
||||
uint8_t stop_flag;
|
||||
|
||||
Motor_Type_e motor_type;
|
||||
} dji_motor_instance;
|
||||
```
|
||||
|
||||
- `DJI_MOTOR_CNT`是允许的最大DJI电机数量，根据经验，暂定为每个CAN6个，防止出现拥塞。
|
||||
|
||||
- `SPEED_SMOOTH_COEF`和`CURRENT_SMOOTH_COEF`是电机反馈的电流和速度数据低通滤波器惯性系数，数值越小平滑效果越大，但滞后也越大。设定时不应当低于推荐值。
|
||||
- `dji_motor_measure`是DJI电机的反馈信息，包括当前编码器值、上次测量编码器值、速度、电流、温度、总圈数和单圈角度。
|
||||
|
||||
- `Motor_Control_Setting_s`的定义在`motor_def.h`之中，它和`Motor_Controller_s`都是所有电机通用的组件（如M3508，LK9025，HT04，MT6023等），其包含内容如下：
|
||||
|
||||
```c
|
||||
typedef struct /* 电机控制配置 */
|
||||
{
|
||||
Closeloop_Type_e outer_loop_type;
|
||||
Closeloop_Type_e close_loop_type;
|
||||
Motor_Reverse_Flag_e motor_reverse_flag;
|
||||
Feedback_Source_e angle_feedback_source;
|
||||
Feedback_Source_e speed_feedback_source;
|
||||
} Motor_Control_Setting_s;
|
||||
```
|
||||
|
||||
`Motor_Control_Setting_s`里包含了电机的闭环类型，反转标志以及额外的反馈来源标志。
|
||||
|
||||
- 闭环类型指示该电机使用的控制器配置，其枚举定义如下：
|
||||
|
||||
```c
|
||||
typedef enum
|
||||
{
|
||||
CURRENT_LOOP = 0b0001,
|
||||
SPEED_LOOP = 0b0010,
|
||||
ANGLE_LOOP = 0b0100,
|
||||
_ = 0b0011,
|
||||
__ = 0b0110,
|
||||
___ = 0b0111
|
||||
} Closeloop_Type_e;
|
||||
```
|
||||
|
||||
以M3508为例，假设需要进行**速度闭环**和**电流闭环**，那么在初始化时就将这个变量的值设为`CURRENT_LOOP | SPEED_LOOP`。在`DJIMotorControl()`中，函数将会根据此标志位判断设定的参考值需要经过那些控制器的计算。
|
||||
另外,你还需要设置当前电机的最外层闭环，即电机的闭环目标为什么类型的值。初始化时需要设置`outer_loop_type`。以M2006作为拨盘电机时为例，你希望它在单发/双发等固定发射数量的模式下对位置进行闭环（拨盘转过一定角度对应拨出一颗弹丸），但你也有可能希望在连发的时候让拨盘连续的转动，以一定的频率发射弹丸。我们提供了`DJIMotorOuterLoop()`用于修改电机的外层闭环，改变电机的闭环对象。
|
||||
|
||||
> 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。
|
||||
|
||||
- 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`motor_reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下：
|
||||
|
||||
```c
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_DIRECTION_NORMAL = 0,
|
||||
MOTOR_DIRECTION_REVERSE = 1
|
||||
} Motor_Reverse_Flag_e;
|
||||
```
|
||||
|
||||
- `speed_feedback_source`以及`angle_feedback_source`是指示电机反馈来源的标志位。一般情况下电机使用自身的编码器作为控制反馈量。但在某些时候，如小陀螺模式，云台电机会使用IMU的姿态数据作为反馈数据来源。其定义如下：
|
||||
|
||||
```c
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_FEED = 0,
|
||||
OTHER_FEED = 1
|
||||
} Feedback_Source_e;
|
||||
```
|
||||
|
||||
**注意，如果启用其他数据来源，你需要在电机的控制器配置`Motor_Controller_s`下的`other_xxx_feedback_ptr`中指定其他数据来源。**
|
||||
|
||||
你可以在`DJIMotorChangeFeed()`中修改电机的数据来源。
|
||||
|
||||
- `Motor_Controller_s`的定义也在`motor_def.h`之中：
|
||||
|
||||
```c
|
||||
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
|
||||
typedef struct
|
||||
{
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
PID_t current_PID;
|
||||
PID_t speed_PID;
|
||||
PID_t angle_PID;
|
||||
|
||||
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
|
||||
} Motor_Controller_s;
|
||||
```
|
||||
|
||||
两个`float*`指针应当指向其他反馈来源数据（如果有的话，需要在`motor_settings`中设定）。
|
||||
|
||||
三个PID分别为三个控制闭环所用，在`DJIMotorControl()`中，该函数会根据`close_loop_type`的设定计算对应的闭环。
|
||||
|
||||
**`pid_ref`是控制的设定值，app层的应用想要更改电机的输出，就要调用`DJIMotorSetRef()`更改此值。**
|
||||
|
||||
- `dji_motor_instance`是一个DJI电机实例。一个电机实例内包含电机的反馈信息，电机的控制设置，电机控制器，电机对应的CAN实例以及电机的类型；由于DJI电机支持**一帧报文控制至多4个电机**，该结构体还包含了用于给电机分组发送进行特殊处理的`sender_group`和`message_num`（具体实现细节参考`MotorSenderGrouping()`函数）。
|
||||
|
||||
## 外部接口
|
||||
|
||||
```c
|
||||
dji_motor_instance *DJIMotorInit(can_instance_config config,
|
||||
Motor_Control_Setting_s motor_setting,
|
||||
Motor_Controller_Init_s controller_init,
|
||||
Motor_Type_e type);
|
||||
|
||||
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
|
||||
|
||||
void DJIMotorChangeFeed(dji_motor_instance *motor,
|
||||
Closeloop_Type_e loop,
|
||||
Feedback_Source_e type);
|
||||
|
||||
void DJIMotorControl();
|
||||
|
||||
void DJIMotorStop(dji_motor_instance *motor);
|
||||
|
||||
void DJIMotorEnable(dji_motor_instance *motor);
|
||||
|
||||
void DJIMotorOuterLoop(dji_motor_instance *motor);
|
||||
```
|
||||
|
||||
- `DJIMotorInit()`是用于初始化电机对象的接口，传入包括电机can配置、电机控制配置、电机控制器配置以及电机类型在内的初始化参数。**它将会返回一个电机实例指针**，你应当在应用层保存这个指针，这样才能操控这个电机。
|
||||
|
||||
- `DJIMotorSetRef()`是设定电机输出的接口，**在调用这个函数的时候，你可以认为你的设定值会直接转变为电机的输出**。`DJIMotorControl()`会帮你完成闭环计算，不用担心PID。
|
||||
|
||||
- `DJIMotorChangeFeed()`一般在更改云台或底盘的运动模式的时候被调用，传入要修改反馈来源的电机实例指针、要修改的闭环以及反馈来源类型。如希望切换到IMU的yaw值作为云台设定值，传入yaw轴电机实例和`ANGLE_LOOP`（位置环）、`OTHER_FEED`（启用其他数据来源）即可。当然，你需要在初始化的时候设定`motor_controller`中的 `other_angle_feedback_ptr`，使其指向yaw值的变量。
|
||||
|
||||
- `DJIMotorControl()`是根据电机的配置计算控制值的函数。该函数在`motor_task.c`中被调用，应当在freeRTOS中以一定频率运行。此函数为PID的计算进行了彻底的封装，要修改电机的参考输入，请在app层的应用中调用`DJIMotorSetRef()`。
|
||||
|
||||
该函数的具体实现请参照代码，注释已经较为清晰。流程大致为：
|
||||
|
||||
1. 根据电机的初始化控制配置，计算各个控制闭环
|
||||
2. 根据反转标志位，确定是否将输出反转
|
||||
3. 根据每个电机的发送分组，将最终输出值填入对应的分组buff
|
||||
4. 检查每一个分组，若该分组有电机，发送报文
|
||||
|
||||
- `DJIMotorStop()`和`DJIMotorEnable()`用于控制电机的启动和停止。当电机被设为stop的时候，不会响应任何的参考输入。
|
||||
|
||||
- `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。
|
||||
|
||||
## 私有函数和变量
|
||||
|
||||
在.c文件内设为static的函数和变量
|
||||
|
||||
```c
|
||||
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
|
||||
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
|
||||
```
|
||||
|
||||
这是管理所有电机实例的入口。idx用于电机初始化。
|
||||
|
||||
```c
|
||||
#define PI2 (3.141592f * 2)
|
||||
#define ECD_ANGLE_COEF_DJI 3.835e-4 // ecd/8192*pi
|
||||
```
|
||||
|
||||
这两个宏用于在电机反馈信息中的多圈角度计算，将编码器的0~8192转化为角度表示。
|
||||
|
||||
```c
|
||||
/* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
|
||||
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
|
||||
*
|
||||
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
|
||||
* can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF */
|
||||
static can_instance sender_assignment[6] =
|
||||
{
|
||||
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
|
||||
...
|
||||
...
|
||||
};
|
||||
|
||||
static uint8_t sender_enable_flag[6] = {0};
|
||||
```
|
||||
|
||||
- 这些是电机分组发送所需的变量。注册电机时，会根据挂载的总线以及发送id，将电机分组。在CAN发送电机控制信息的时候，根据`sender_assignment[]`保存的分组进行发送，而不会使用电机实例自带的`can_instance`。
|
||||
- DJI电机共有3种分组，分别为0x1FF,0x200,0x2FF。注册电机的时候，`MotorSenderGrouping()`函数会根据发送id计算出CAN的`tx_id`（即上述三个中的一个）和`rx_id`。然后为电机实例分配用于指示其在`sender_assignment[]`中的编号的 `sender_group`和其在该发送组中的位置`message_num`（一帧报文可以发送四条控制指令，`message_num`会指定电机是这四个中的哪一个）。具体的分配请查看`MotorSenderGrouping()`的定义。
|
||||
- 当某一个分组有电机注册时，该分组的索引将会在`sender_enable_flag`[]中被置1，这样，就可以避免发送没有电机注册的报文，防止总线拥塞。具体的，在`DecodeDJIMotor()`中，该函数会查看`sender_enable_flag[]`的每一个位置，确定这一组是否有电机被注册，若有则发送`sender_assignment[]`中对应位置的`tx_buff`。
|
||||
|
||||
```c
|
||||
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
|
||||
|
||||
static void MotorSenderGrouping(can_instance_config *config)
|
||||
|
||||
static void DecodeDJIMotor(can_instance *_instance)
|
||||
```
|
||||
|
||||
- `IDcrash_Handler()`在电机id发生冲突的时候会被`MotorSenderGrouping()`调用，陷入死循环之中，并把冲突的id保存在函数里。这样就可以通过debug确定是否发生冲突以及冲突的编号。
|
||||
|
||||
- `MotorSenderGrouping()`被`DJIMotorInit()`调用，他将会根据电机id计算出CAN的发送和接收ID，并根据发送ID对电机进行分组。
|
||||
|
||||
- `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。
|
||||
|
||||
该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
|
||||
|
||||
**电机反馈的电流值为说明书中的映射值，需转换为实际值。**
|
||||
|
||||
**反馈的速度单位是rpm（转每分钟），转换为角度每秒。**
|
||||
|
||||
**反馈的位置是编码器值（0~8191），转换为角度。**
|
||||
|
||||
## 使用范例
|
||||
|
||||
```c
|
||||
//初始化设置
|
||||
Motor_Init_Config_s config = {
|
||||
.motor_type = GM6020,
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 6
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Improve = 0,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.DeadBand = 0,
|
||||
.MaxOut = 4000},
|
||||
.speed_PID = {
|
||||
.Improve = 0,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.DeadBand = 0,
|
||||
.MaxOut = 4000
|
||||
}
|
||||
}
|
||||
};
|
||||
//注册电机并保存实例指针
|
||||
dji_motor_instance *djimotor = DJIMotorInit(&config);
|
||||
```
|
||||
|
||||
然后在任务中修改电机设定值即可实现控制：
|
||||
|
||||
```
|
||||
DJIMotorSetRef(djimotor, 10);
|
||||
```
|
||||
|
||||
前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。
|
||||
@@ -153,20 +153,16 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
|
||||
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
|
||||
memset(motor, 0, sizeof(DMMotorInstance));
|
||||
|
||||
motor->motor_settings = config->motor_config.controller_setting_init_config;
|
||||
motor->output_reverse = config->motor_config.output_reverse;
|
||||
motor->ctrl_mode = config->ctrl_mode;
|
||||
motor->mit_kp = config->mit_kp;
|
||||
motor->mit_kd = config->mit_kd;
|
||||
|
||||
/* init PID controllers */
|
||||
PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID);
|
||||
PIDInit(&motor->motor_controller.speed_PID, &config->motor_config.controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->motor_controller.current_PID, &config->motor_config.controller_param_init_config.current_PID);
|
||||
/* init angle PID controller */
|
||||
PIDInit(&motor->motor_controller.angle_PID, &config->motor_config.controller_param_init_config.angle_PID);
|
||||
|
||||
motor->motor_controller.other_angle_feedback_ptr = config->motor_config.controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->motor_controller.other_speed_feedback_ptr = config->motor_config.controller_param_init_config.other_speed_feedback_ptr;
|
||||
motor->motor_controller.speed_feedforward_ptr = config->motor_config.controller_param_init_config.speed_feedforward_ptr;
|
||||
motor->motor_controller.current_feedforward_ptr = config->motor_config.controller_param_init_config.current_feedforward_ptr;
|
||||
|
||||
/* register CAN instance */
|
||||
config->motor_config.can_init_config.can_module_callback = DMMotorDecode;
|
||||
@@ -195,30 +191,23 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref)
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = type;
|
||||
}
|
||||
|
||||
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
|
||||
{
|
||||
motor->ctrl_mode = mode;
|
||||
}
|
||||
|
||||
/* ---- Main control: called from Motor Task at 1kHz ---- */
|
||||
/* 硬编码: IMU 角度反馈 → 角度 PID → 速度指令 → 电机速度模式 */
|
||||
|
||||
void DMMotorControl(void)
|
||||
{
|
||||
for (uint8_t i = 0; i < idx; i++)
|
||||
{
|
||||
DMMotorInstance *motor = dm_motor_instance[i];
|
||||
DM_Motor_Measure_s *measure = &motor->measure;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
Motor_Controller_s *ctrl = &motor->motor_controller;
|
||||
CANInstance *can = motor->motor_can_instance;
|
||||
|
||||
/* handle motor error states */
|
||||
if (measure->state != 0x00 && measure->state != 0x01)
|
||||
if (motor->measure.state != 0x00 && motor->measure.state != 0x01)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
|
||||
DWT_Delay(0.001);
|
||||
@@ -226,60 +215,29 @@ void DMMotorControl(void)
|
||||
continue;
|
||||
}
|
||||
|
||||
float pid_ref = motor->pid_ref;
|
||||
/* angle PID: IMU feedback → velocity output */
|
||||
float pid_ref = PIDCalculate(&motor->motor_controller.angle_PID,
|
||||
*motor->motor_controller.other_angle_feedback_ptr,
|
||||
motor->pid_ref);
|
||||
|
||||
/* ---- Angle loop (outermost) ---- */
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
float pid_measure;
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *ctrl->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle;
|
||||
|
||||
pid_ref = PIDCalculate(&ctrl->angle_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
/* ---- Speed loop (inner) ---- */
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
|
||||
{
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *ctrl->speed_feedforward_ptr;
|
||||
|
||||
float pid_measure;
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *ctrl->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->velocity;
|
||||
|
||||
pid_ref = PIDCalculate(&ctrl->speed_PID, pid_measure, pid_ref);
|
||||
}
|
||||
|
||||
/* output reverse */
|
||||
if (setting->output_reverse == OUTPUT_DIRECTION_REVERSE)
|
||||
if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
/* ---- Send via selected control mode ---- */
|
||||
float vel_cmd = pid_ref * DEGREE_2_RAD; // deg/s → rad/s
|
||||
float vel_cmd = pid_ref * DEGREE_2_RAD;
|
||||
LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
|
||||
|
||||
switch (motor->ctrl_mode)
|
||||
{
|
||||
case DM_MODE_VELOCITY:
|
||||
/* Velocity mode: MCU angle PID output → motor velocity ref */
|
||||
DMMotorSendVelocity(can, vel_cmd);
|
||||
break;
|
||||
|
||||
case DM_MODE_MIT: {
|
||||
/* MIT mode: MCU angle PID output → motor position ref, motor handles position-velocity internally */
|
||||
float pos_cmd = pid_ref * DEGREE_2_RAD;
|
||||
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
|
||||
DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
|
||||
break;
|
||||
}
|
||||
|
||||
case DM_MODE_POS_VEL: {
|
||||
/* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */
|
||||
float pos_cmd = pid_ref * DEGREE_2_RAD;
|
||||
LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
|
||||
DMMotorSendPosVel(can, pos_cmd, vel_cmd);
|
||||
|
||||
@@ -48,16 +48,16 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
DM_Motor_Measure_s measure;
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
Motor_Controller_s motor_controller;
|
||||
|
||||
CANInstance *motor_can_instance; // CAN communication instance
|
||||
DaemonInstance *motor_daemon; // daemon watchdog
|
||||
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
|
||||
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
|
||||
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
|
||||
Output_Reverse_Flag_e output_reverse; // output reverse flag
|
||||
DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
|
||||
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
|
||||
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
|
||||
|
||||
float pid_ref; // reference input from application
|
||||
float pid_ref; // reference input from application
|
||||
} DMMotorInstance;
|
||||
|
||||
/* DM motor init config */
|
||||
@@ -83,7 +83,6 @@ typedef enum
|
||||
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
|
||||
|
||||
void DMMotorSetRef(DMMotorInstance *motor, float ref);
|
||||
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type);
|
||||
void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
|
||||
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
|
||||
|
||||
|
||||
@@ -1,235 +0,0 @@
|
||||
#include "HT04.h"
|
||||
#include "memory.h"
|
||||
#include "general_def.h"
|
||||
#include "user_lib.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "string.h"
|
||||
#include "daemon.h"
|
||||
#include "stdlib.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
|
||||
static osThreadId ht_task_handle[HT_MOTOR_CNT];
|
||||
static uint8_t zero_buff[6] = {0};
|
||||
|
||||
/**
|
||||
* @brief 设置电机模式,报文内容[0xff,0xff,0xff,0xff,0xff,0xff,0xff,cmd]
|
||||
*
|
||||
* @param cmd
|
||||
* @param motor
|
||||
*/
|
||||
static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
|
||||
{
|
||||
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
|
||||
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
|
||||
}
|
||||
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
|
||||
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
|
||||
}
|
||||
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||
{
|
||||
float span = x_max - x_min;
|
||||
float offset = x_min;
|
||||
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 解析电机反馈值
|
||||
*
|
||||
* @param motor_can 收到
|
||||
*/
|
||||
static void HTMotorDecode(CANInstance *motor_can)
|
||||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t const *rxbuff = motor_can->rx_buff;
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_can->id;
|
||||
HTMotor_Measure_t *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
|
||||
|
||||
DaemonReload(motor->motor_daemon);
|
||||
measure->feed_dt = DWT_GetDeltaT(&measure->feed_cnt);
|
||||
|
||||
measure->last_angle = measure->total_angle;
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
|
||||
|
||||
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
|
||||
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
|
||||
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
|
||||
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
|
||||
}
|
||||
|
||||
static void HTMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
|
||||
LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
|
||||
if (++motor->lost_cnt % 10 != 0)
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
|
||||
}
|
||||
|
||||
/* 海泰电机一生黑,什么垃圾协议! */
|
||||
void HTMotorCalibEncoder(HTMotorInstance *motor)
|
||||
{
|
||||
uint16_t p, v, kp, kd, t;
|
||||
p = float_to_uint(0, P_MIN, P_MAX, 16);
|
||||
v = float_to_uint(0, V_MIN, V_MAX, 12);
|
||||
kp = float_to_uint(0, KP_MIN, KP_MAX, 12);
|
||||
kd = float_to_uint(0, KD_MIN, KD_MAX, 12);
|
||||
t = float_to_uint(0, T_MIN, T_MAX, 12);
|
||||
|
||||
uint8_t *buf = motor->motor_can_instace->tx_buff;
|
||||
buf[0] = p >> 8;
|
||||
buf[1] = p & 0xFF;
|
||||
buf[2] = v >> 4;
|
||||
buf[3] = ((v & 0xF) << 4) | (kp >> 8);
|
||||
buf[4] = kp & 0xFF;
|
||||
buf[5] = kd >> 4;
|
||||
buf[6] = ((kd & 0xF) << 4) | (t >> 8);
|
||||
buf[7] = t & 0xff;
|
||||
memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
|
||||
CANTransmit(motor->motor_can_instace, 1);
|
||||
DWT_Delay(0.005);
|
||||
HTMotorSetMode(CMD_ZERO_POSITION, motor); // sb 玩意校准完了编码器也不为0
|
||||
DWT_Delay(0.005);
|
||||
// HTMotorSetMode(CMD_MOTOR_MODE, motor);
|
||||
}
|
||||
|
||||
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
HTMotorInstance *motor = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
|
||||
memset(motor, 0, sizeof(HTMotorInstance));
|
||||
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
config->can_init_config.can_module_callback = HTMotorDecode;
|
||||
config->can_init_config.id = motor;
|
||||
motor->motor_can_instace = CANRegister(&config->can_init_config);
|
||||
|
||||
Daemon_Init_Config_s conf = {
|
||||
.callback = HTMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
.reload_count = 5, // 20ms
|
||||
};
|
||||
motor->motor_daemon = DaemonRegister(&conf);
|
||||
|
||||
HTMotorEnable(motor);
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 确保电机已经上电并执行电机模式
|
||||
DWT_Delay(0.05f);
|
||||
HTMotorCalibEncoder(motor); // 将当前编码器位置作为零位
|
||||
DWT_Delay(0.05f); // 保证下一个电机发送时CAN是空闲的,注意应用在初始化模块的时候不应该进入中断
|
||||
ht_motor_instance[idx++] = motor;
|
||||
return motor;
|
||||
}
|
||||
|
||||
void HTMotorSetRef(HTMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 为了避免总线堵塞,为每个电机创建一个发送任务
|
||||
* @param argument 传入的电机指针
|
||||
*/
|
||||
__attribute__((noreturn)) void HTMotorTask(void const *argument)
|
||||
{
|
||||
float set, pid_measure, pid_ref;
|
||||
HTMotorInstance *motor = (HTMotorInstance *)argument;
|
||||
HTMotor_Measure_t *measure = &motor->measure;
|
||||
Motor_Control_Setting_s *setting = &motor->motor_settings;
|
||||
CANInstance *motor_can = motor->motor_can_instace;
|
||||
uint16_t tmp;
|
||||
|
||||
while (1)
|
||||
{
|
||||
pid_ref = motor->pid_ref;
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->total_angle;
|
||||
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
|
||||
}
|
||||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
|
||||
{
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor->speed_feedforward_ptr;
|
||||
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->speed_rads;
|
||||
// measure单位是rad / s ,ref是angle per sec,统一到angle下计算
|
||||
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
|
||||
}
|
||||
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
set = pid_ref;
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
set *= -1;
|
||||
|
||||
LIMIT_MIN_MAX(set, T_MIN, T_MAX);
|
||||
tmp = float_to_uint(set, T_MIN, T_MAX, 12);
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
tmp = float_to_uint(0, T_MIN, T_MAX, 12);
|
||||
motor_can->tx_buff[6] = (tmp >> 8);
|
||||
motor_can->tx_buff[7] = tmp & 0xff;
|
||||
|
||||
CANTransmit(motor_can, 0.5);
|
||||
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
||||
void HTMotorControlInit()
|
||||
{
|
||||
char ht_task_name[5] = "ht";
|
||||
// 遍历所有电机实例,创建任务
|
||||
if (!idx)
|
||||
return;
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
char ht_id_buff[2] = {0};
|
||||
__itoa(i, ht_id_buff, 10);
|
||||
strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
|
||||
// @todo 还需要一个更优雅的方案来区分不同的电机任务
|
||||
osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
|
||||
ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void HTMotorStop(HTMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void HTMotorEnable(HTMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type)
|
||||
{
|
||||
motor->motor_settings.outer_loop_type = type;
|
||||
}
|
||||
@@ -1,123 +0,0 @@
|
||||
#ifndef HT04_H
|
||||
#define HT04_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define HT_MOTOR_CNT 4
|
||||
#define CURRENT_SMOOTH_COEF 0.9f
|
||||
#define SPEED_BUFFER_SIZE 5
|
||||
#define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
|
||||
|
||||
#define P_MIN -95.5f // Radians
|
||||
#define P_MAX 95.5f
|
||||
#define V_MIN -45.0f // Rad/s
|
||||
#define V_MAX 45.0f
|
||||
#define T_MIN -18.0f // N·m
|
||||
#define T_MAX 18.0f
|
||||
#define KP_MIN 0.0f // N-m/rad
|
||||
#define KP_MAX 500.0f
|
||||
#define KD_MIN 0.0f // N-m/rad/s
|
||||
#define KD_MAX 5.0f
|
||||
|
||||
typedef struct // HT04
|
||||
{
|
||||
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
|
||||
float last_angle;
|
||||
|
||||
float speed_rads;
|
||||
float speed_buff[SPEED_BUFFER_SIZE];
|
||||
|
||||
float real_current;
|
||||
|
||||
float feed_dt;
|
||||
uint32_t feed_cnt;
|
||||
} HTMotor_Measure_t;
|
||||
|
||||
/* HT电机类型定义*/
|
||||
typedef struct
|
||||
{
|
||||
HTMotor_Measure_t measure;
|
||||
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr;
|
||||
float *current_feedforward_ptr;
|
||||
float pid_ref;
|
||||
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
CANInstance *motor_can_instace;
|
||||
|
||||
DaemonInstance *motor_daemon;
|
||||
uint32_t lost_cnt;
|
||||
} HTMotorInstance;
|
||||
|
||||
/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
|
||||
typedef enum
|
||||
{
|
||||
CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
|
||||
CMD_RESET_MODE = 0xfd, // 停止
|
||||
CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位
|
||||
} HTMotor_Mode_t;
|
||||
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
* @param config
|
||||
* @return HTMotorInstance*
|
||||
*/
|
||||
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
|
||||
|
||||
/**
|
||||
* @brief 设定电机的参考值
|
||||
*
|
||||
* @param motor 要设定的电机
|
||||
* @param current 设定值
|
||||
*/
|
||||
void HTMotorSetRef(HTMotorInstance *motor, float ref);
|
||||
|
||||
/**
|
||||
* @brief 初始化电机任务,若要使用,需要在motortask的死循环前调用
|
||||
*
|
||||
*/
|
||||
void HTMotorControlInit();
|
||||
|
||||
/**
|
||||
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
|
||||
*
|
||||
* @param motor
|
||||
*/
|
||||
void HTMotorStop(HTMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 启动电机
|
||||
*
|
||||
* @param motor 要启动的电机
|
||||
*/
|
||||
void HTMotorEnable(HTMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 修改电机闭环对象
|
||||
*
|
||||
* @param motor
|
||||
* @param type
|
||||
*/
|
||||
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type);
|
||||
|
||||
/**
|
||||
* @brief 将当前编码器位置置零
|
||||
*
|
||||
* @param motor
|
||||
*/
|
||||
void HTMotorCalibEncoder(HTMotorInstance *motor);
|
||||
|
||||
#endif // !HT04_H#define HT04_H
|
||||
@@ -1,44 +0,0 @@
|
||||
- 在通过串口设置电机的时候，注意发送指令不要追加回车\n，否则电机端不响应。可以使用sscom，其会将你的按键直接当作ascii字符发送出去。
|
||||
|
||||
- **此电机的CAN线序和RoboMaster开发板相反，注意单独制作CAN线**
|
||||
|
||||
- 电机控制和反馈报文：
|
||||
|
||||
|
||||
注意，我们的代码实现不使用其电调协议的位置PD算法；自行实现了三环，并在最后给出电流参考值，发送给电调。因此command packed structure中的**pos cmd & vel cmd & Kp & Kd均为零**。报文均为小端，**低位在前**。
|
||||
|
||||
请注意发送和反馈数据的**单位**。
|
||||
|
||||
> ~~HT的电机协议做的真不行，纯纯直接抄mit还抄不明白，之后全部换成LK！~~
|
||||
|
||||
另一种上电时校准编码器的方法:
|
||||
|
||||
```c
|
||||
static void HTMotorDecode(CANInstance *motor_can)
|
||||
{
|
||||
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
|
||||
uint8_t *rxbuff = motor_can->rx_buff;
|
||||
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
|
||||
|
||||
measure->last_angle = measure->total_angle;
|
||||
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
|
||||
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
|
||||
|
||||
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
|
||||
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
|
||||
|
||||
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
|
||||
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
|
||||
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
|
||||
|
||||
if (!measure->first_feedback_flag) // 初始化的时候设置偏置
|
||||
{
|
||||
measure->first_feedback_flag = 1;
|
||||
measure->angle_bias = measure->total_angle;
|
||||
measure->total_angle = 0;
|
||||
measure->speed_rads = 0;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
第一次收到数据时默认电机处于限位处,将速度和角度都设置为零,记录当前的编码器数据,之后每次收到都减去该值.
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 103 KiB |
Binary file not shown.
Binary file not shown.
@@ -1,169 +0,0 @@
|
||||
#include "LK9025.h"
|
||||
#include "stdlib.h"
|
||||
#include "general_def.h"
|
||||
#include "daemon.h"
|
||||
#include "bsp_dwt.h"
|
||||
#include "bsp_log.h"
|
||||
|
||||
static uint8_t idx;
|
||||
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
|
||||
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
|
||||
// 后续考虑兼容单电机和多电机指令.
|
||||
|
||||
/**
|
||||
* @brief 电机反馈报文解析
|
||||
*
|
||||
* @param _instance 发生中断的caninstance
|
||||
*/
|
||||
static void LKMotorDecode(CANInstance *_instance)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
|
||||
LKMotor_Measure_t *measure = &motor->measure;
|
||||
uint8_t *rx_buff = _instance->rx_buff;
|
||||
|
||||
DaemonReload(motor->daemon); // 喂狗
|
||||
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
|
||||
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
|
||||
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
|
||||
|
||||
measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
|
||||
DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
|
||||
|
||||
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
|
||||
|
||||
measure->temperature = rx_buff[1];
|
||||
|
||||
if (measure->ecd - measure->last_ecd > 32768)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -32768)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void LKMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)motor_ptr;
|
||||
LOGWARNING("[LKMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
|
||||
}
|
||||
|
||||
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
|
||||
motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
|
||||
memset(motor, 0, sizeof(LKMotorInstance));
|
||||
|
||||
motor->motor_settings = config->controller_setting_init_config;
|
||||
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
|
||||
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
|
||||
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
|
||||
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
config->can_init_config.id = motor;
|
||||
config->can_init_config.can_module_callback = LKMotorDecode;
|
||||
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
|
||||
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
|
||||
motor->motor_can_ins = CANRegister(&config->can_init_config);
|
||||
|
||||
if (idx == 0) // 用第一个电机的can instance发送数据
|
||||
{
|
||||
sender_instance = motor->motor_can_ins;
|
||||
sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
|
||||
}
|
||||
|
||||
LKMotorEnable(motor);
|
||||
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
|
||||
lkmotor_instance[idx++] = motor;
|
||||
|
||||
Daemon_Init_Config_s daemon_config = {
|
||||
.callback = LKMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
.reload_count = 5, // 50ms
|
||||
};
|
||||
motor->daemon = DaemonRegister(&daemon_config);
|
||||
|
||||
return motor;
|
||||
}
|
||||
|
||||
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
|
||||
void LKMotorControl()
|
||||
{
|
||||
float pid_measure, pid_ref;
|
||||
int16_t set;
|
||||
LKMotorInstance *motor;
|
||||
LKMotor_Measure_t *measure;
|
||||
Motor_Control_Setting_s *setting;
|
||||
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{
|
||||
motor = lkmotor_instance[i];
|
||||
measure = &motor->measure;
|
||||
setting = &motor->motor_settings;
|
||||
pid_ref = motor->pid_ref;
|
||||
|
||||
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_angle_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->real_current;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
|
||||
pid_ref += *motor->speed_feedforward_ptr;
|
||||
}
|
||||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->speed_rads;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
}
|
||||
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
set = pid_ref;
|
||||
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
|
||||
set *= -1;
|
||||
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
|
||||
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
|
||||
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
{ // 若该电机处于停止状态,直接将发送buff置零
|
||||
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
|
||||
}
|
||||
}
|
||||
|
||||
if (idx) // 如果有电机注册了
|
||||
CANTransmit(sender_instance, 0.2);
|
||||
}
|
||||
|
||||
void LKMotorStop(LKMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void LKMotorEnable(LKMotorInstance *motor)
|
||||
{
|
||||
motor->stop_flag = MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void LKMotorSetRef(LKMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref = ref;
|
||||
}
|
||||
|
||||
uint8_t LKMotorIsOnline(LKMotorInstance *motor)
|
||||
{
|
||||
return DaemonIsOnline(motor->daemon);
|
||||
}
|
||||
@@ -1,98 +0,0 @@
|
||||
#ifndef LK9025_H
|
||||
#define LK9025_H
|
||||
|
||||
#include "stdint.h"
|
||||
#include "bsp_can.h"
|
||||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
#include "daemon.h"
|
||||
|
||||
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
|
||||
|
||||
#define I_MIN -2000
|
||||
#define I_MAX 2000
|
||||
#define CURRENT_SMOOTH_COEF 0.9f
|
||||
#define SPEED_SMOOTH_COEF 0.85f
|
||||
#define REDUCTION_RATIO_DRIVEN 1
|
||||
#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
|
||||
#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
|
||||
|
||||
typedef struct // 9025
|
||||
{
|
||||
uint16_t last_ecd; // 上一次读取的编码器值
|
||||
uint16_t ecd; // 当前编码器值
|
||||
float angle_single_round; // 单圈角度
|
||||
float speed_rads; // speed rad/s
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperature; // 温度,C°
|
||||
|
||||
float total_angle; // 总角度
|
||||
int32_t total_round; // 总圈数
|
||||
|
||||
float feed_dt;
|
||||
uint32_t feed_dwt_cnt;
|
||||
} LKMotor_Measure_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
LKMotor_Measure_t measure;
|
||||
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
|
||||
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入
|
||||
float *current_feedforward_ptr; // 电流前馈指针
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
float pid_ref;
|
||||
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
CANInstance *motor_can_ins;
|
||||
|
||||
DaemonInstance *daemon;
|
||||
|
||||
} LKMotorInstance;
|
||||
|
||||
/**
|
||||
* @brief 初始化LK电机
|
||||
*
|
||||
* @param config 电机配置
|
||||
* @return LKMotorInstance* 返回实例指针
|
||||
*/
|
||||
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config);
|
||||
|
||||
/**
|
||||
* @brief 设置参考值
|
||||
* @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置
|
||||
*
|
||||
* @param motor 要设置的电机
|
||||
* @param ref 设定值
|
||||
*/
|
||||
void LKMotorSetRef(LKMotorInstance *motor, float ref);
|
||||
|
||||
/**
|
||||
* @brief 为所有LK电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文)
|
||||
*
|
||||
*/
|
||||
void LKMotorControl();
|
||||
|
||||
/**
|
||||
* @brief 停止LK电机,之后电机不会响应任何指令
|
||||
*
|
||||
* @param motor
|
||||
*/
|
||||
void LKMotorStop(LKMotorInstance *motor);
|
||||
|
||||
/**
|
||||
* @brief 启动LK电机
|
||||
*
|
||||
* @param motor
|
||||
*/
|
||||
void LKMotorEnable(LKMotorInstance *motor);
|
||||
|
||||
uint8_t LKMotorIsOnline(LKMotorInstance *motor);
|
||||
|
||||
#endif // LK9025_H
|
||||
@@ -1,12 +0,0 @@
|
||||
# LK motor
|
||||
|
||||
这是瓴控电机的模块封装说明文档。关于LK电机的控制报文和反馈报文值，详见LK电机的说明文档，由于电机实例已经自带三环PID计算，一般来说**我们能用到的只有单电机的力矩指令和多电机指令。**
|
||||
|
||||
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持发送id 0x280为接收ID为0x140+id.
|
||||
|
||||
要设置为多电机模式，请通过串口连接电机，并使用该文件夹下的LK motor tool.exe进行配置。
|
||||
|
||||
|
||||
## LK的其他电机
|
||||
|
||||
若使用其他LK电机，唯一需要修改的是确定编码器的精度，即LKMotorDecode()部分的速度反馈和编码器反馈解析。
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 163 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 276 KiB |
+23
-104
@@ -1,14 +1,3 @@
|
||||
/**
|
||||
* @file motor_def.h
|
||||
* @author neozng
|
||||
* @brief 电机通用的数据结构定义
|
||||
* @version beta
|
||||
* @date 2022-11-01
|
||||
*
|
||||
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef MOTOR_DEF_H
|
||||
#define MOTOR_DEF_H
|
||||
|
||||
@@ -17,89 +6,13 @@
|
||||
|
||||
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
|
||||
|
||||
/**
|
||||
* @brief 闭环类型,如果需要多个闭环,则使用或运算
|
||||
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
|
||||
*/
|
||||
typedef enum
|
||||
{
|
||||
OPEN_LOOP = 0b0000,
|
||||
CURRENT_LOOP = 0b0001,
|
||||
SPEED_LOOP = 0b0010,
|
||||
ANGLE_LOOP = 0b0100,
|
||||
|
||||
// only for checking
|
||||
SPEED_AND_CURRENT_LOOP = 0b0011,
|
||||
ANGLE_AND_SPEED_LOOP = 0b0110,
|
||||
ALL_THREE_LOOP = 0b0111,
|
||||
} Closeloop_Type_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
FEEDFORWARD_NONE = 0b00,
|
||||
CURRENT_FEEDFORWARD = 0b01,
|
||||
SPEED_FEEDFORWARD = 0b10,
|
||||
CURRENT_AND_SPEED_FEEDFORWARD = CURRENT_FEEDFORWARD | SPEED_FEEDFORWARD,
|
||||
} Feedfoward_Type_e;
|
||||
|
||||
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_FEED = 0,
|
||||
OTHER_FEED,
|
||||
} Feedback_Source_e;
|
||||
|
||||
/* 输出反向标志: 取反最终PID输出 */
|
||||
/* 输出反向标志 */
|
||||
typedef enum
|
||||
{
|
||||
OUTPUT_DIRECTION_NORMAL = 0,
|
||||
OUTPUT_DIRECTION_REVERSE = 1,
|
||||
} Output_Reverse_Flag_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MOTOR_STOP = 0,
|
||||
MOTOR_ENALBED = 1,
|
||||
} Motor_Working_Type_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
SEND_MODE_NORMAL = 0,
|
||||
SEND_MODE_MANUAL = 1
|
||||
} MOTOR_SEND_MODE_e;
|
||||
|
||||
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
|
||||
typedef struct
|
||||
{
|
||||
Closeloop_Type_e outer_loop_type; // 最外层的闭环
|
||||
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
|
||||
Output_Reverse_Flag_e output_reverse; // 输出反向
|
||||
Feedback_Source_e angle_feedback_source; // 角度反馈类型
|
||||
Feedback_Source_e speed_feedback_source; // 速度反馈类型
|
||||
uint8_t feedforward_flag; // 前馈标志
|
||||
MOTOR_SEND_MODE_e send_mode; // 发送模式
|
||||
|
||||
|
||||
} Motor_Control_Setting_s;
|
||||
|
||||
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
|
||||
// 后续增加前馈数据指针
|
||||
typedef struct
|
||||
{
|
||||
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
|
||||
float *other_speed_feedback_ptr;
|
||||
float *speed_feedforward_ptr;
|
||||
float *current_feedforward_ptr;
|
||||
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
|
||||
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
|
||||
float output_external; // 外部输入,用于手动发送报文
|
||||
|
||||
} Motor_Controller_s;
|
||||
|
||||
/* 电机类型枚举 */
|
||||
typedef enum
|
||||
{
|
||||
@@ -107,31 +20,37 @@ typedef enum
|
||||
DM_MOTOR,
|
||||
} Motor_Type_e;
|
||||
|
||||
/**
|
||||
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
|
||||
* 如果不需要某个控制环,可以不设置对应的pid config
|
||||
* 需要其他数据来源进行反馈闭环,不仅要设置这里的指针还需要在Motor_Control_Setting_s启用其他数据来源标志
|
||||
*/
|
||||
/* 电机控制器: PID 实例和外部反馈指针 */
|
||||
typedef struct
|
||||
{
|
||||
float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle
|
||||
float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec
|
||||
float *other_angle_feedback_ptr; // 外部角度反馈指针 (如 IMU)
|
||||
float *other_speed_feedback_ptr; // 外部速度反馈指针 (如 IMU 陀螺仪)
|
||||
|
||||
float *speed_feedforward_ptr; // 速度前馈数据指针
|
||||
float *current_feedforward_ptr; // 电流前馈数据指针
|
||||
PIDInstance angle_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance current_PID;
|
||||
|
||||
float pid_ref;
|
||||
} Motor_Controller_s;
|
||||
|
||||
/* 电机控制器初始化配置 */
|
||||
typedef struct
|
||||
{
|
||||
float *other_angle_feedback_ptr;
|
||||
float *other_speed_feedback_ptr;
|
||||
|
||||
PID_Init_Config_s current_PID;
|
||||
PID_Init_Config_s speed_PID;
|
||||
PID_Init_Config_s angle_PID;
|
||||
PID_Init_Config_s speed_PID;
|
||||
PID_Init_Config_s current_PID;
|
||||
} Motor_Controller_Init_s;
|
||||
|
||||
/* 用于初始化CAN电机的结构体,各类电机通用 */
|
||||
/* CAN 电机通用初始化结构体 */
|
||||
typedef struct
|
||||
{
|
||||
Motor_Controller_Init_s controller_param_init_config;
|
||||
Motor_Control_Setting_s controller_setting_init_config;
|
||||
Motor_Type_e motor_type;
|
||||
CAN_Init_Config_s can_init_config;
|
||||
Output_Reverse_Flag_e output_reverse;
|
||||
Motor_Type_e motor_type;
|
||||
CAN_Init_Config_s can_init_config;
|
||||
} Motor_Init_Config_s;
|
||||
|
||||
#endif // !MOTOR_DEF_H
|
||||
#endif // MOTOR_DEF_H
|
||||
|
||||
@@ -1,78 +0,0 @@
|
||||
#include "servo_motor.h"
|
||||
#include "stdlib.h"
|
||||
#include "memory.h"
|
||||
#include "bsp_log.h"
|
||||
uint8_t servo_angle_read[6]={0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
|
||||
uint8_t servo_angle_write[16]={0x55 ,0x55, 0x08, 0x03, 0x01 ,0xF4 ,0x01 ,0x01 ,0x20 ,0x03,0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
|
||||
uint8_t servo_unload[6]={0x55,0x55,0x04,0x14,0x01,0x01};
|
||||
|
||||
/*第二版*/
|
||||
static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT];
|
||||
static uint8_t servo_idx = 0; // register servo_idx,是该文件的全局舵机索引,在注册时使用
|
||||
static void DecodeServo();
|
||||
// 通过此函数注册一个舵机
|
||||
ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config)
|
||||
{
|
||||
ServoInstance *servo = (ServoInstance *)malloc(sizeof(ServoInstance));
|
||||
memset(servo, 0, sizeof(ServoInstance));
|
||||
USART_Init_Config_s config;
|
||||
servo->servo_type = Servo_Init_Config->servo_type;
|
||||
switch (Servo_Init_Config->servo_type)
|
||||
{
|
||||
case Bus_Servo:
|
||||
config.module_callback = DecodeServo;
|
||||
config.recv_buff_size = Servo_MAX_BUFF;
|
||||
config.usart_handle = Servo_Init_Config->_handle;
|
||||
servo->usart_instance = USARTRegister(&config);
|
||||
break;
|
||||
case PWM_Servo:
|
||||
servo->pwm_instance = PWMRegister(&Servo_Init_Config->pwm_init_config);
|
||||
break;
|
||||
default:
|
||||
LOGERROR("Servo type error");
|
||||
break;
|
||||
}
|
||||
servo->servo_id = Servo_Init_Config->servo_id;
|
||||
servo_idx++;
|
||||
servo_motor_instance[servo->servo_id] = servo;
|
||||
|
||||
return servo;
|
||||
}
|
||||
//@todo PWM舵机的角度设置需要根据相应定时器PWM等参数进行计算(是否需要规范定时器PWM的初始化参数，以便于计算)
|
||||
void ServoSetAngle(ServoInstance *servo, float angle)
|
||||
{
|
||||
|
||||
switch (servo->servo_type)
|
||||
{
|
||||
case Bus_Servo:
|
||||
servo_angle_write[8] = (uint16_t)angle&0xff;
|
||||
servo_angle_write[9] = (uint16_t)angle>>8;
|
||||
USARTSend(servo->usart_instance, servo_angle_write, 16, USART_TRANSFER_DMA);
|
||||
// USARTSend(servo->usart_instance, servo_angle_read, 6, USART_TRANSFER_DMA);
|
||||
break;
|
||||
case PWM_Servo:
|
||||
servo->angle = angle;
|
||||
PWMSetDutyRatio(servo->pwm_instance, angle);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//@todo 只读取了角度 还有电压，动作是否完成等 且只支持一个串口
|
||||
static void DecodeServo()
|
||||
{
|
||||
for (uint8_t i = 0; i < servo_idx; i++)
|
||||
{
|
||||
if (servo_motor_instance[i]->servo_type == Bus_Servo)
|
||||
{
|
||||
if (servo_motor_instance[i]->usart_instance->recv_buff[0] == Servo_Frame_First && servo_motor_instance[i]->usart_instance->recv_buff[1] == Servo_Frame_Second)
|
||||
{
|
||||
if (servo_motor_instance[i]->usart_instance->recv_buff[3] == 21)
|
||||
{
|
||||
servo_motor_instance[i]->recv_angle = (servo_motor_instance[i]->usart_instance->recv_buff[7] << 8 | servo_motor_instance[i]->usart_instance->recv_buff[6]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
#ifndef SERVO_MOTOR_H
|
||||
#define SERVO_MOTOR_H
|
||||
|
||||
#include "main.h"
|
||||
#include "tim.h"
|
||||
#include <stdint-gcc.h>
|
||||
#include "bsp_pwm.h"
|
||||
#include "bsp_usart.h"
|
||||
|
||||
#define SERVO_MOTOR_CNT 7
|
||||
#define Servo_Frame_First 0x55
|
||||
#define Servo_Frame_Second 0x55
|
||||
#define Servo_MAX_BUFF 10
|
||||
#define SERVO_MOVE_CMD 0x03
|
||||
#define SERVO_UNLOAD_CMD 0x14
|
||||
#define SERVO_POS_READ_CMD 0x15
|
||||
typedef enum
|
||||
{
|
||||
Servo_None_Type = 0,
|
||||
Bus_Servo = 1,
|
||||
PWM_Servo = 2,
|
||||
}ServoType_e;
|
||||
|
||||
|
||||
/* 用于初始化不同舵机的结构体,各类舵机通用 */
|
||||
typedef struct
|
||||
{
|
||||
PWM_Init_Config_s pwm_init_config;
|
||||
ServoType_e servo_type;
|
||||
UART_HandleTypeDef *_handle;
|
||||
uint8_t servo_id;
|
||||
}Servo_Init_Config_s;
|
||||
typedef struct
|
||||
{
|
||||
uint8_t servo_id;
|
||||
float angle;
|
||||
uint16_t recv_angle;
|
||||
PWMInstance *pwm_instance;
|
||||
USARTInstance *usart_instance;
|
||||
ServoType_e servo_type;
|
||||
}ServoInstance;
|
||||
|
||||
ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config);
|
||||
void ServoSetAngle(ServoInstance *servo, float angle);
|
||||
#endif // SERVO_MOTOR_H
|
||||
@@ -1,23 +0,0 @@
|
||||
##舵机的使用
|
||||
|
||||
<p align='left' >panrui@hnu.edu.cn</p>
|
||||
|
||||
> todo: 角度设置等需要规范化
|
||||
### 舵机基础知识
|
||||
|
||||
已最常见的SG90舵机为例，SG90舵机要求工作在频率为50HZ——周期为20ms的PWM波，且对应信号的高低电平在0.5ms - 2.5ms之间，对应的舵机转动角度如下表所示（当然也可以按照这个线性的对应关系去达到转动自己想要的角度，如想要转动60°，则高电平脉宽为大概为1.2ms，具体能不能转到特定的角度还和舵机的精度有关）
|
||||
|
||||
>0.5ms-------------0度； 2.5%
|
||||
>1.0ms------------45度； 5.0%
|
||||
>1.5ms------------90度； 7.5%
|
||||
>2.0ms-----------135度； 10.0%
|
||||
>2.5ms-----------180度； 12.5%
|
||||
|
||||
根据`<font color=black size=3>Tout = (PSC+1)* (ARR+1)/Tclk</font>`公式
|
||||
则我们需要产生50Hz的PWM波，则预分频的系数为 Prescaler = 168-1，自动重装载值 Counter Period = 20000-1，此时定时器产生的频率为 168Mhz/168/20000 = 50Hz。 当然这个值也可以自己设置，只要满足产生的频率为50Hz即可.
|
||||
`__HAL_TIM_SET_COMPARE(htim, Channel, compare_value);`
|
||||
这是设置占空比的函数
|
||||
eg：当初始占空比为1200/20000则为6%，根据20*6%=1.2ms （1.2-0.5）/(2.5-0.5)*180=63° 故舵机会转动63°
|
||||
为了方便通过上述eg我们将所需要的角度与PWM计数值对应关系封装成函数。需要在初始化的時候输入我们所需要的角度和相关定时器参数即可。这样我们就可以设置SG90为参数范围内(0~180°)任意度数。
|
||||
|
||||
---
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,231 +0,0 @@
|
||||
#ifndef __X_V2_H
|
||||
#define __X_V2_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/**********************************************************
|
||||
*** X_V2步进闭环控制例程
|
||||
*** 编写作者：ZHANGDATOU
|
||||
*** 技术支持：张大头闭环伺服
|
||||
*** 淘宝店铺：https://zhangdatou.taobao.com
|
||||
*** CSDN博客：http s://blog.csdn.net/zhangdatou666
|
||||
*** qq交流群：262438510
|
||||
**********************************************************/
|
||||
|
||||
#define ABS(x) ((x) > 0 ? (x) : -(x))
|
||||
|
||||
typedef enum {
|
||||
S_VBUS = 5, // 读取总线电压
|
||||
S_CBUS = 6, // 读取总线电流
|
||||
S_CPHA = 7, // 读取相电流
|
||||
S_ENCO = 8, // 读取编码器原始值
|
||||
S_CLKC = 9, // 读取实时脉冲数
|
||||
S_ENCL = 10, // 读取经过线性化校准后的编码器值
|
||||
S_CLKI = 11, // 读取输入脉冲数
|
||||
S_TPOS = 12, // 读取电机目标位置
|
||||
S_SPOS = 13, // 读取电机实时设定的目标位置
|
||||
S_VEL = 14, // 读取电机实时转速
|
||||
S_CPOS = 15, // 读取电机实时位置
|
||||
S_PERR = 16, // 读取电机位置误差
|
||||
S_VBAT = 17, // 读取多圈编码器电池电压（Y42）
|
||||
S_TEMP = 18, // 读取电机实时温度（Y42）
|
||||
S_FLAG = 19, // 读取电机状态标志位
|
||||
S_OFLAG = 20, // 读取回零状态标志位
|
||||
S_OAF = 21, // 读取电机状态标志位 + 回零状态标志位（Y42）
|
||||
S_PIN = 22, // 读取引脚状态（Y42）
|
||||
}SysParams_t;
|
||||
|
||||
#define MMCL_LEN 512
|
||||
extern uint16_t MMCL_count, MMCL_cmd[MMCL_LEN];
|
||||
|
||||
/**
|
||||
***********************************************************
|
||||
***********************************************************
|
||||
***
|
||||
***
|
||||
*** @brief 后缀带有（Y42）为Y42新增命令，X42不能用，其他通用
|
||||
***
|
||||
***
|
||||
***********************************************************
|
||||
***********************************************************
|
||||
***/
|
||||
/**********************************************************
|
||||
*** 触发动作命令
|
||||
**********************************************************/
|
||||
// 触发编码器校准
|
||||
void X_V2_Trig_Encoder_Cal(uint8_t addr);
|
||||
// 重启电机（Y42）
|
||||
void X_V2_Reset_Motor(uint8_t addr);
|
||||
// 将当前位置清零
|
||||
void X_V2_Reset_CurPos_To_Zero(uint8_t addr);
|
||||
// 解除堵转保护
|
||||
void X_V2_Reset_Clog_Pro(uint8_t addr);
|
||||
// 恢复出厂设置
|
||||
void X_V2_Restore_Motor(uint8_t addr);
|
||||
/**********************************************************
|
||||
*** 运动控制命令
|
||||
**********************************************************/
|
||||
// 多电机命令（Y42）
|
||||
void X_V2_Multi_Motor_Cmd(uint8_t addr);
|
||||
// 电机使能控制
|
||||
void X_V2_En_Control(uint8_t addr, bool state, bool snF);
|
||||
// 力矩模式
|
||||
void X_V2_Torque_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF);
|
||||
// 力矩模式限速控制（Y42）
|
||||
void X_V2_Torque_LV_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF, float maxVel);
|
||||
// 速度模式控制
|
||||
void X_V2_Vel_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF);
|
||||
// 速度模式限电流控制（Y42）
|
||||
void X_V2_Vel_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF, uint16_t maxCur);
|
||||
// 直通限速位置模式控制
|
||||
void X_V2_Bypass_Pos_LV_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF);
|
||||
// 直通限速位置模式限电流控制（Y42）
|
||||
void X_V2_Bypass_Pos_LV_LC_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur);
|
||||
// 梯形曲线加减速位置模式控制
|
||||
void X_V2_Traj_Pos_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF);
|
||||
// 梯形曲线加减速位置模式限电流控制（Y42）
|
||||
void X_V2_Traj_Pos_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur);
|
||||
// 让电机立即停止运动
|
||||
void X_V2_Stop_Now(uint8_t addr, bool snF);
|
||||
// 触发多机同步开始运动
|
||||
void X_V2_Synchronous_motion(uint8_t addr);
|
||||
/**********************************************************
|
||||
*** 原点回零命令
|
||||
**********************************************************/
|
||||
// 设置单圈回零的零点位置
|
||||
void X_V2_Origin_Set_O(uint8_t addr, bool svF);
|
||||
// 触发回零
|
||||
void X_V2_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
|
||||
// 强制中断并退出回零
|
||||
void X_V2_Origin_Interrupt(uint8_t addr);
|
||||
// 读取回零参数
|
||||
void X_V2_Origin_Read_Params(uint8_t addr);
|
||||
// 修改回零参数
|
||||
void X_V2_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF);
|
||||
/**********************************************************
|
||||
*** 读取系统参数命令
|
||||
**********************************************************/
|
||||
// 定时返回信息命令（Y42）
|
||||
void X_V2_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms);
|
||||
// 读取系统参数
|
||||
void X_V2_Read_Sys_Params(uint8_t addr, SysParams_t s);
|
||||
/**********************************************************
|
||||
*** 读写驱动参数命令
|
||||
**********************************************************/
|
||||
// 修改电机ID地址
|
||||
void X_V2_Modify_Motor_ID(uint8_t addr, bool svF, uint8_t id);
|
||||
// 修改细分值
|
||||
void X_V2_Modify_MicroStep(uint8_t addr, bool svF, uint8_t mstep);
|
||||
// 修改掉电标志
|
||||
void X_V2_Modify_PDFlag(uint8_t addr, bool pdf);
|
||||
// 读取选项参数状态（Y42）
|
||||
void X_V2_Read_Opt_Param_Sta(uint8_t addr);
|
||||
// 修改电机类型（Y42）
|
||||
void X_V2_Modify_Motor_Type(uint8_t addr, bool svF, bool mottype);
|
||||
// 修改固件类型（Y42）
|
||||
void X_V2_Modify_Firmware_Type(uint8_t addr, bool svF, bool fwtype);
|
||||
// 修改开环/闭环控制模式（Y42）
|
||||
void X_V2_Modify_Ctrl_Mode(uint8_t addr, bool svF, bool ctrl_mode);
|
||||
// 修改电机运动正方向（Y42）
|
||||
void X_V2_Modify_Motor_Dir(uint8_t addr, bool svF, bool dir);
|
||||
// 修改锁定按键功能（Y42）
|
||||
void X_V2_Modify_Lock_Btn(uint8_t addr, bool svF, bool lockbtn);
|
||||
// 修改命令位置角度是否继续缩小10倍输入（Y42）
|
||||
void X_V2_Modify_S_Vel(uint8_t addr, bool svF, bool s_vel);
|
||||
// 修改开环模式工作电流
|
||||
void X_V2_Modify_OM_ma(uint8_t addr, bool svF, uint16_t om_ma);
|
||||
// 修改闭环模式最大电流
|
||||
void X_V2_Modify_FOC_mA(uint8_t addr, bool svF, uint16_t foc_mA);
|
||||
// 读取PID参数
|
||||
void X_V2_Read_PID_Params(uint8_t addr);
|
||||
// 修改PID参数
|
||||
void X_V2_Modify_PID_Params(uint8_t addr, bool svF, uint32_t kp, uint32_t ki, uint32_t kd);
|
||||
// 读取DMX512协议参数（Y42）
|
||||
void X_V2_Read_DMX512_Params(uint8_t addr);
|
||||
// 修改DMX512协议参数（Y42）
|
||||
void X_V2_Modify_DMX512_Params(uint8_t addr, bool svF, uint16_t tch, uint8_t nch, uint8_t mode, uint16_t vel, uint16_t acc, uint16_t vel_step, uint32_t pos_step);
|
||||
// 读取位置到达窗口（Y42）
|
||||
void X_V2_Read_Pos_Window(uint8_t addr);
|
||||
// 修改位置到达窗口（Y42）
|
||||
void X_V2_Modify_Pos_Window(uint8_t addr, bool svF, uint16_t prw);
|
||||
// 读取过热过流保护检测阈值（Y42）
|
||||
void X_V2_Read_Otocp(uint8_t addr);
|
||||
// 修改过热过流保护检测阈值（Y42）
|
||||
void X_V2_Modify_Otocp(uint8_t addr, bool svF, uint16_t otp, uint16_t ocp, uint16_t time_ms);
|
||||
// 读取心跳保护功能时间（Y42）
|
||||
void X_V2_Read_Heart_Protect(uint8_t addr);
|
||||
// 修改心跳保护功能时间（Y42）
|
||||
void X_V2_Modify_Heart_Protect(uint8_t addr, bool svF, uint32_t hp);
|
||||
// 读取积分限幅/刚性系数（Y42）
|
||||
void X_V2_Read_Integral_Limit(uint8_t addr);
|
||||
// 修改积分限幅/刚性系数（Y42）
|
||||
void X_V2_Modify_Integral_Limit(uint8_t addr, bool svF, uint32_t il);
|
||||
/**********************************************************
|
||||
*** 读取所有驱动参数命令
|
||||
**********************************************************/
|
||||
// 读取系统状态参数
|
||||
void X_V2_Read_System_State_Params(uint8_t addr);
|
||||
// 读取驱动配置参数
|
||||
void X_V2_Read_Motor_Conf_Params(uint8_t addr);
|
||||
|
||||
/**
|
||||
***********************************************************
|
||||
***********************************************************
|
||||
***
|
||||
***
|
||||
*** @brief 以下是把相应命令加载到Y42多电机命令上的函数（Y42）
|
||||
***
|
||||
***
|
||||
***********************************************************
|
||||
***********************************************************
|
||||
***/
|
||||
/**********************************************************
|
||||
*** 触发动作命令
|
||||
**********************************************************/
|
||||
// 触发编码器校准 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Trig_Encoder_Cal(uint8_t addr);
|
||||
// 重启电机 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Reset_Motor(uint8_t addr);
|
||||
// 将当前位置清零 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Reset_CurPos_To_Zero(uint8_t addr);
|
||||
// 解除堵转保护 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Reset_Clog_Pro(uint8_t addr);
|
||||
// 恢复出厂设置 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Restore_Motor(uint8_t addr);
|
||||
/**********************************************************
|
||||
*** 运动控制命令
|
||||
**********************************************************/
|
||||
// 电机使能控制 - 加载到多电机指令上
|
||||
void X_V2_MMCL_En_Control(uint8_t addr, bool state, bool snF);
|
||||
// 速度模式控制 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF);
|
||||
// 位置模式控制 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF);
|
||||
// 让电机立即停止运动 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Stop_Now(uint8_t addr, bool snF);
|
||||
// 触发多机同步开始运动 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Synchronous_motion(uint8_t addr);
|
||||
/**********************************************************
|
||||
*** 原点回零命令
|
||||
**********************************************************/
|
||||
// 设置单圈回零的零点位置 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Origin_Set_O(uint8_t addr, bool svF);
|
||||
// 触发回零 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
|
||||
// 强制中断并退出回零 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Origin_Interrupt(uint8_t addr);
|
||||
// 修改回零参数 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF);
|
||||
/**********************************************************
|
||||
*** 读取系统参数命令
|
||||
**********************************************************/
|
||||
// 定时返回信息命令（Y42） - 加载到多电机指令上
|
||||
void X_V2_MMCL_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms);
|
||||
// 读取系统参数 - 加载到多电机指令上
|
||||
void X_V2_MMCL_Read_Sys_Params(uint8_t addr, SysParams_t s);
|
||||
/**********************************************************
|
||||
*** 读写驱动参数命令
|
||||
**********************************************************/
|
||||
|
||||
#endif
|
||||
@@ -1,26 +0,0 @@
|
||||
#include "step_motor.h"
|
||||
#include "X_V2.h"
|
||||
#include "usart.h"
|
||||
|
||||
#define MOTOR_UHART0 &huart1
|
||||
#define MOTOR_UHART1 &huart1
|
||||
|
||||
|
||||
|
||||
|
||||
void step_sendcmd(uint8_t addr ,uint8_t *cmd, uint16_t len)
|
||||
{
|
||||
// 发送步进电机控制命令
|
||||
if (len > 0 && cmd != NULL)
|
||||
{
|
||||
if(addr == 1 || addr == 2)
|
||||
{
|
||||
HAL_UART_Transmit_DMA(MOTOR_UHART0, cmd, len );
|
||||
}
|
||||
else if (addr == 3)
|
||||
{
|
||||
HAL_UART_Transmit_DMA(MOTOR_UHART1, cmd, len );
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,12 +0,0 @@
|
||||
#ifndef STEP_MOTOR_H
|
||||
#define STEP_MOTOR_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
|
||||
|
||||
// 步进电机控制命令
|
||||
void step_sendcmd(uint8_t addr, uint8_t *cmd, uint16_t len);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user