From 4b17e1e1932636fb153ddf68166fc676442856f3 Mon Sep 17 00:00:00 2001 From: Zorua <3540226929@qq.com> Date: Sun, 15 Mar 2026 12:52:05 +0800 Subject: [PATCH] damiao motor driver init_1 --- application/chassis/chassis.c | 10 +++---- application/cmd/robot_cmd.c | 28 +++++++++--------- application/gimbal/gimbal.c | 15 ++-------- application/robot_def.h | 4 --- application/shoot/shoot.c | 2 +- modules/motor/DJImotor/dji_motor.c | 47 ++++++++++++++++++++---------- modules/motor/DJImotor/dji_motor.h | 1 - 7 files changed, 53 insertions(+), 54 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 06428c7..e43e52a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -113,7 +113,7 @@ void ChassisInit() .can_init_config.can_handle = &hcan1, .controller_param_init_config = { .angle_PID = { - .Kp = 50,//15 + .Kp = 12, .Ki = 0.2, .Kd = 0, .CoefA = 3, @@ -125,7 +125,7 @@ void ChassisInit() .DeadBand = 0.5, }, .speed_PID = { - .Kp = 50,//25 + .Kp = 40, .Ki = 3, .Kd = 0, .Improve = PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate | PID_OutputFilter, @@ -154,7 +154,7 @@ void ChassisInit() .Kp = 1.05, // 6 .Ki = 0, .Kd = 0.1, // 0.495 - // .DeadBand = 0.5f,// 启用死区，值为0.5度 + .DeadBand = 0.5f,// 启用死区，值为0.5度 //.CoefA = 0.2, //.CoefB = 0.3, //.Improve = PID_Trapezoid_Intergral | PID_DerivativeFilter | PID_DerivativeFilter | PID_Derivative_On_Measurement | PID_Integral_Limit | PID_Derivative_On_Measurement, @@ -412,9 +412,7 @@ void ChassisTask() { single_round_offset += 360.0f; } - - chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100; - // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100; + chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, single_round_offset, 0)/100; // chassis_cmd_recv.wz = PIDCalculate(&chassis_follow_pid, chassis_cmd_recv.offset_angle, 0)/100; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 128d024..1c0e786 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -84,9 +84,9 @@ void RobotCMDInit() static void CalcOffsetAngle() { static float last_offset_angle = 0; - const float smooth_factor = 0.2f; // 平滑因子,0~1之间,值越大平滑效果越明显 + const float smooth_factor = 0.3f; // 平滑因子,0~1之间,值越大平滑效果越明显 - //const float deadband = 3.0f; // 死区值 + const float deadband = 3.0f; // 死区值 // 新增：跟踪模式变化，用于重置基准角度 // static chassis_mode_e last_chassis_mode = CHASSIS_NO_FOLLOW;// // @@ -108,14 +108,14 @@ static void CalcOffsetAngle() else if (angle_diff < -180.0f) new_offset += 360.0f; ///// - // // 应用死区 - // if (fabs(new_offset) < deadband) { - // new_offset = 0.0f; - // } else if (new_offset > 0) { - // new_offset -= deadband; - // } else { - // new_offset += deadband; - // }////// + // 应用死区 + if (fabs(new_offset) < deadband) { + new_offset = 0.0f; + } else if (new_offset > 0) { + new_offset -= deadband; + } else { + new_offset += deadband; + }////// chassis_cmd_send.offset_angle = last_offset_angle *(1.0f - smooth_factor) + new_offset * smooth_factor; last_offset_angle = chassis_cmd_send.offset_angle; // // 更新模式状态 @@ -203,14 +203,14 @@ static void RemoteControlSet() // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制 if (switch_is_down(rc_data[TEMP].rc.switch_left) ) { // 按照摇杆的输出大小进行角度增量,增益系数需调整 - gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_r_; - gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_r1; + gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; } // 云台软件限位 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_l_; // _水平方向 - chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_l1; // 1数值方向 + chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 // // 发射参数 // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index eb98b6a..cbaea8a 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -66,7 +66,7 @@ void GimbalInit() Motor_Init_Config_s pitch_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 2,//2 + .tx_id = 4,//2 }, .controller_param_init_config = { .angle_PID = { @@ -79,20 +79,12 @@ void GimbalInit() }, .speed_PID = { .Kp = 50, // 50 - .Ki = 175, // 350 + .Ki = 350, // 350 .Kd = 0, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 2500, - .MaxOut = 12000, + .MaxOut = 20000, }, - // .current_PID = { // 添加current_PID配置 - // .Kp = 0, - // .Ki = 0, - // .Kd = 0, - // .Improve = PID_Integral_Limit, - // .IntegralLimit = 5000, - // .MaxOut = 15000, - // }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]), @@ -103,7 +95,6 @@ void GimbalInit() .outer_loop_type = ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, - .feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE, }, .motor_type = DM4310, }; diff --git a/application/robot_def.h b/application/robot_def.h index 6412201..1465ca5 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -208,10 +208,6 @@ typedef struct attitude_t gimbal_imu_data; uint16_t yaw_motor_single_round_angle; uint16_t yaw_motor_total_angle; - //uint16_t pitch_motor_single_round_angle; - //uint16_t pitch_motor_total_angle; - // 后续增加云台的其他数据 - // ... } Gimbal_Upload_Data_s; typedef struct diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 7a89e8e..514ea36 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -55,7 +55,7 @@ void ShootInit() friction_config.can_init_config.tx_id = 1, friction_l = DJIMotorInit(&friction_config); - friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行 + friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; friction_r = DJIMotorInit(&friction_config); diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index b9c76e0..7e328f3 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -20,7 +20,6 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF */ static CANInstance sender_assignment[6] = { - //[0] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [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}}, @@ -52,12 +51,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf if (motor_id < 4) // 根据ID分组 { motor_send_num = motor_id; - motor_grouping = config->can_handle == &hcan2 ? 1 : 4; + motor_grouping = config->can_handle == &hcan1 ? 1 : 4; } else { motor_send_num = motor_id - 4; - motor_grouping = config->can_handle == &hcan2 ? 0 : 3; + motor_grouping = config->can_handle == &hcan1 ? 0 : 3; } // 计算接收id并设置分组发送id @@ -72,7 +71,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf 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 == &hcan2 ? 1 : 2; + 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); } @@ -91,6 +90,22 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf motor_grouping = config->can_handle == &hcan1 ? 2 : 5; } + 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 DM4310: if (motor_id < 4) // 根据ID分组 { @@ -292,34 +307,34 @@ void DJIMotorControl() 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->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_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + pid_ref *= -1; // 获取最终输出 set = (int16_t)pid_ref; + // 分组填入发送数据 group = motor->sender_group; num = motor->message_num; - //DM电机单独处理,因为它的发送格式与DJI电机不同,但仍然使用sender_assignment进行分组发送 + if (i == 1) { sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位 sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位 } - else { sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位 - sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位 + sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位 } // 若该电机处于停止状态,直接将buff置零 diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index 1419199..c4c878d 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -105,7 +105,6 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 */ void DJIMotorControl(); -void DM_MotorControl(); /** * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 *