From 0eaf1bd9d89eb39db0f0c7f9c7f05c97c72814e1 Mon Sep 17 00:00:00 2001 From: zcj <2487150395@qq.com> Date: Wed, 17 Apr 2024 15:26:00 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=BE=BE=E5=A6=99=E9=A9=B1?= =?UTF-8?q?=E5=8A=A8=EF=BC=88=E6=9C=AA=E9=AA=8C=E8=AF=81=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 +- application/cmd/robot_cmd.c | 12 ++- application/robot_def.h | 4 +- application/robot_task.h | 2 + application/shoot/shoot.c | 38 +++---- modules/motor/DMmotor/dm4310.c | 185 +++++++++++++++++++++++++++++++++ modules/motor/DMmotor/dm4310.h | 78 ++++++++++++++ modules/motor/motor_task.c | 4 +- 8 files changed, 298 insertions(+), 29 deletions(-) create mode 100644 modules/motor/DMmotor/dm4310.c create mode 100644 modules/motor/DMmotor/dm4310.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 65e2097..fd0cd75 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim - modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor + modules/motor/DJImotor modules/motor/DMmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor application application/chassis application/cmd application/gimbal application/shoot Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) @@ -85,4 +85,4 @@ add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD COMMAND ${CMAKE_OBJCOPY} -Oihex $ ${HEX_FILE} COMMAND ${CMAKE_OBJCOPY} -Obinary $ ${BIN_FILE} COMMENT "Building ${HEX_FILE} -Building ${BIN_FILE}") +Building ${BIN_FILE}") \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index d1d729d..0504d2f 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -59,6 +59,7 @@ static Robot_Status_e robot_state; // 机器人整体工作状态 static referee_info_t* referee_data; // 用于获取裁判系统的数据 extern float horizontal_angle; //static int target_index = -1; +static uint16_t base_HP; void RobotCMDInit() { @@ -124,14 +125,15 @@ static void CalcOffsetAngle() // else // chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle; } +//功能:死亡后清除小陀螺的状态 static void death_check() { if(referee_data->GameRobotState.current_HP <= 0) { rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0; } - } + static void update_ui_data() { ui_data.chassis_mode = chassis_cmd_send.chassis_mode; @@ -220,11 +222,13 @@ static void RemoteControlSet() // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺 { - chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; - gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + shoot_cmd_send.friction_mode = FRICTION_ON; +// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; +// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台 { + shoot_cmd_send.friction_mode = FRICTION_OFF; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; } @@ -265,7 +269,7 @@ static void RemoteControlSet() shoot_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 if (rc_data[TEMP].rc.dial < -500) - shoot_cmd_send.load_mode = LOAD_STOP; + shoot_cmd_send.load_mode = LOAD_1_BULLET; else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, diff --git a/application/robot_def.h b/application/robot_def.h index 1eabef8..1763aef 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -32,8 +32,8 @@ #define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度) #define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪,则填写陀螺仪的角度) // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 1383 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 19.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49 +#define ONE_BULLET_DELTA_ANGLE 1933.272 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49,增加为27 #define NUM_PER_CIRCLE 5 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) #define WHEEL_BASE 433.86 // 纵向轴距(前进后退方向) diff --git a/application/robot_task.h b/application/robot_task.h index 252e77b..fb6ccf9 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -16,6 +16,7 @@ #include "buzzer.h" #include "bsp_log.h" +#include "dm4310.h" osThreadId insTaskHandle; osThreadId robotTaskHandle; @@ -52,6 +53,7 @@ void OSTaskInit() uiTaskHandle = osThreadCreate(osThread(uitask), NULL); HTMotorControlInit(); // 没有注册HT电机则不会执行 + DMMotorControlInit(); // 没有注册DM电机则不会执行 } __attribute__((noreturn)) void StartINSTASK(void const *argument) diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 38b6493..79fb4ca 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -55,15 +55,15 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP , - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, }, .motor_type = M3508}; - friction_config.can_init_config.tx_id = 3, - friction_l = DJIMotorInit(&friction_config); +// friction_config.can_init_config.tx_id = 3, +// friction_l = DJIMotorInit(&friction_config); -// friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行 -// friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; -// friction_r = DJIMotorInit(&friction_config); + friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行 + friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; + friction_r = DJIMotorInit(&friction_config); friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行 friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; @@ -134,15 +134,15 @@ void ShootTask() // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止) if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) { - DJIMotorStop(friction_l); -// DJIMotorStop(friction_r); +// DJIMotorStop(friction_l); + DJIMotorStop(friction_r); DJIMotorStop(friction_z); DJIMotorStop(loader); } else // 恢复运行 { - DJIMotorEnable(friction_l); -// DJIMotorEnable(friction_r); +// DJIMotorEnable(friction_l); + DJIMotorEnable(friction_r); DJIMotorEnable(friction_z); DJIMotorEnable(loader); } @@ -156,13 +156,13 @@ void ShootTask() { // 停止拨盘 case LOAD_STOP: - DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 - DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 + DJIMotorStop(loader); +// DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 +// DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 break; // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. - DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 - stop_location = loader->measure.total_angle; + DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环v DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 dead_time = shoot_cmd_recv.shoot_rate; // 完成1发弹丸发射的时间 @@ -198,14 +198,14 @@ void ShootTask() // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) if (shoot_cmd_recv.friction_mode == FRICTION_ON) { - DJIMotorSetRef(friction_l, 48000); -// DJIMotorSetRef(friction_r, 39000); - DJIMotorSetRef(friction_z, 48000);//39000/6 = 6500 +// DJIMotorSetRef(friction_l, 39000); + DJIMotorSetRef(friction_r, 39000); + DJIMotorSetRef(friction_z, 39000);//39000/6 = 6500 } else // 关闭摩擦轮 { - DJIMotorSetRef(friction_l, 0); -// DJIMotorSetRef(friction_r, 0); +// DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); DJIMotorSetRef(friction_z, 0); } diff --git a/modules/motor/DMmotor/dm4310.c b/modules/motor/DMmotor/dm4310.c new file mode 100644 index 0000000..bcc8f39 --- /dev/null +++ b/modules/motor/DMmotor/dm4310.c @@ -0,0 +1,185 @@ +#include "dm4310.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 DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT]; +static osThreadId dm_task_handle[DM_MOTOR_CNT]; +/* 两个用于将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; +} + +static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) +{ + memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff + motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id + CANTransmit(motor->motor_can_instance, 1); +} + +static void DMMotorDecode(CANInstance *motor_can) +{ + uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 + uint8_t *rxbuff = motor_can->rx_buff; + DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; + DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针 + + DaemonReload(motor->motor_daemon); + + measure->last_position = measure->position; + + tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); + measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); + + measure->angle_single_round = ECD_ANGLE_COEF_DM * (measure->position+3.141593); + if(measure->angle_single_round > 360) measure->angle_single_round -= 360; + if(measure->angle_single_round < 0) measure->angle_single_round += 360 ; + + tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4); + measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12); + + tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); + measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12); + + measure->T_Mos = (float)rxbuff[6]; + measure->T_Rotor = (float)rxbuff[7]; +} + +static void DMMotorLostCallback(void *motor_ptr) +{ + DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; + uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2; + LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id); +} +void DMMotorCaliEncoder(DMMotorInstance *motor) +{ + DMMotorSetMode(DM_CMD_ZERO_POSITION, motor); + DWT_Delay(0.1); +} +DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config) +{ + DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance)); + memset(motor, 0, sizeof(DMMotorInstance)); + + 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 = DMMotorDecode; + config->can_init_config.id = motor; + motor->motor_can_instance = CANRegister(&config->can_init_config); + + Daemon_Init_Config_s conf = { + .callback = DMMotorLostCallback, + .owner_id = motor, + .reload_count = 10, + }; + motor->motor_daemon = DaemonRegister(&conf); + + DMMotorEnable(motor); + DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); + DWT_Delay(0.1f); + dm_motor_instance[idx++] = motor; + return motor; +} + +void DMMotorSetRef(DMMotorInstance *motor, float ref) +{ + motor->pid_ref = ref; +} + +void DMMotorEnable(DMMotorInstance *motor) +{ + motor->stop_flag = MOTOR_ENALBED; +} + +void DMMotorStop(DMMotorInstance *motor)//不使用使能模式是因为需要收到反馈 +{ + motor->stop_flag = MOTOR_STOP; +} + +void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type) +{ + motor->motor_settings.outer_loop_type = type; +} + + +//位置控制 +void DMMotorTask(void const *argument) +{ + float pid_ref, set; + DMMotorInstance *motor = (DMMotorInstance *)argument; + //DM_Motor_Measure_s *measure = &motor->measure; + Motor_Control_Setting_s *setting = &motor->motor_settings; + //CANInstance *motor_can = motor->motor_can_instance; + //uint16_t tmp; + DMMotor_Send_s motor_send_mailbox; + while (1) + { + pid_ref = motor->pid_ref; + + set = pid_ref; + if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + set *= -1; + + LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX); + motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16); + motor_send_mailbox.velocity_des = float_to_uint(set, DM_V_MIN, DM_V_MAX, 12); + motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12); + motor_send_mailbox.Kp = motor->angle_PID.Kp; + motor_send_mailbox.Kd = motor->angle_PID.Kd; + + if(motor->stop_flag == MOTOR_STOP) + { + motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16); + motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12); + motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12); + } + + motor->motor_can_instance->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8); + motor->motor_can_instance->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des); + motor->motor_can_instance->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4); + motor->motor_can_instance->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8)); + motor->motor_can_instance->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp); + motor->motor_can_instance->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4); + motor->motor_can_instance->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8)); + motor->motor_can_instance->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des); + + CANTransmit(motor->motor_can_instance, 1); + + osDelay(2); + } +} +void DMMotorControlInit() +{ + char dm_task_name[5] = "dm"; + // 遍历所有电机实例,创建任务 + if (!idx) + return; + for (size_t i = 0; i < idx; i++) + { + char dm_id_buff[2] = {0}; + __itoa(i, dm_id_buff, 10); + strcat(dm_task_name, dm_id_buff); + osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128); + dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]); + } +} diff --git a/modules/motor/DMmotor/dm4310.h b/modules/motor/DMmotor/dm4310.h new file mode 100644 index 0000000..cbbbb78 --- /dev/null +++ b/modules/motor/DMmotor/dm4310.h @@ -0,0 +1,78 @@ +#ifndef DM4310_H +#define DM4310_H +#include +#include "bsp_can.h" +#include "controller.h" +#include "motor_def.h" +#include "daemon.h" + +#define DM_MOTOR_CNT 1 + +#define DM_P_MIN (-12.5f) +#define DM_P_MAX 12.5f +#define DM_V_MIN (-45.0f) +#define DM_V_MAX 45.0f +#define DM_T_MIN (-18.0f) +#define DM_T_MAX 18.0f +#define ECD_ANGLE_COEF_DM 57.324840f // (360/2PI),将编码器值转化为角度制 + +typedef struct +{ + uint8_t id; + uint8_t state; + float velocity; + float last_position; + float position; + float torque; + float T_Mos; + float T_Rotor; + float angle_single_round; + int32_t total_round; +}DM_Motor_Measure_s; + +typedef struct +{ + uint16_t position_des; + uint16_t velocity_des; + uint16_t torque_des; + uint16_t Kp; + uint16_t Kd; +}DMMotor_Send_s; +typedef struct +{ + DM_Motor_Measure_s 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_instance; + DaemonInstance* motor_daemon; + uint32_t lost_cnt; +}DMMotorInstance; + +typedef enum +{ + DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 + DM_CMD_RESET_MODE = 0xfd, // 停止 + DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位 + DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误 +}DMMotor_Mode_e; + +DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); + +void DMMotorSetRef(DMMotorInstance *motor, float ref); + +void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type); + +void DMMotorEnable(DMMotorInstance *motor); + +void DMMotorStop(DMMotorInstance *motor); +void DMMotorCaliEncoder(DMMotorInstance *motor); +void DMMotorControlInit(); +#endif // !DMMOTOR \ No newline at end of file diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 47ff232..cb21e22 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -13,7 +13,7 @@ void MotorControlTask() DJIMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ - LKMotorControl(); +// LKMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 @@ -21,7 +21,7 @@ void MotorControlTask() // HTMotorControl(); // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口 - ServeoMotorControl(); +// ServeoMotorControl(); // StepMotorControl(); }