From caec6bb497ce3195e24986e342ae7c62d3587dc9 Mon Sep 17 00:00:00 2001 From: Zorua <3540226929@qq.com> Date: Tue, 17 Mar 2026 19:44:51 +0800 Subject: [PATCH] 6020pitch driver --- application/cmd/robot_cmd.c | 12 +++++++++++- application/gimbal/gimbal.c | 22 ++++++++++------------ application/robot_def.h | 6 +++--- application/shoot/shoot.c | 4 ++-- modules/algorithm/QuaternionEKF.c | 4 ++-- modules/motor/DJImotor/dji_motor.c | 18 ++++-------------- 6 files changed, 32 insertions(+), 34 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 1c0e786..ffee775 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -207,6 +207,10 @@ static void RemoteControlSet() gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1; } // 云台软件限位 + if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE) + gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; + else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE) + gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 @@ -243,6 +247,12 @@ static void MouseKeySet() gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; + + // 云台软件限位 + if (gimbal_cmd_send.pitch > PITCH_MAX_ANGLE) + gimbal_cmd_send.pitch = PITCH_MAX_ANGLE; + else if (gimbal_cmd_send.pitch < PITCH_MIN_ANGLE) + gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 { @@ -382,4 +392,4 @@ void RobotCMDTask() PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); // VisionSend(&vision_send_data); -} +} \ No newline at end of file diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index e881bd2..8626f24 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -66,24 +66,20 @@ void GimbalInit() Motor_Init_Config_s pitch_config = { .can_init_config = { .can_handle = &hcan2, - .tx_id = 4,//2 + .tx_id = 2,//2 }, .controller_param_init_config = { .angle_PID = { - .Kp = 10, // 10 + .Kp = 40, // 10 .Ki = 0, .Kd = 0, - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 100, - .MaxOut = 500, + .MaxOut = 1000, }, .speed_PID = { - .Kp = 50, // 50 - .Ki = 350, // 350 - .Kd = 0, // 0 - .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 2500, - .MaxOut = 8000,//20000 + .Kp = 80, // 50 + .Ki = 15, // 350 + .Kd = 0.6, // 0 + .MaxOut = 30000,//20000 }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 @@ -94,9 +90,11 @@ void GimbalInit() .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, + // .outer_loop_type = SPEED_LOOP,//调试用 + // .close_loop_type = SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, - .motor_type = DM4310, + .motor_type = GM6020, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); diff --git a/application/robot_def.h b/application/robot_def.h index 1465ca5..21e72aa 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -29,8 +29,8 @@ #define YAW_CHASSIS_ALIGN_ECD 1190 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 -#define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) -#define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE -20 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 底盘参数 // #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘 // #define CHASSIS_MCNAMEE_WHEEL // 是否为麦克纳姆轮底盘 @@ -42,7 +42,7 @@ #define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度 // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f +#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f #define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) #define WHEEL_BASE 350 // 纵向轴距(前进后退方向) diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 514ea36..4063116 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -52,10 +52,10 @@ void ShootInit() .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; - friction_config.can_init_config.tx_id = 1, + friction_config.can_init_config.tx_id = 3, friction_l = DJIMotorInit(&friction_config); - friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 + 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); diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 27c06e5..7fd7d3b 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -199,8 +199,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, // 利用四元数反解欧拉角 QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f; - QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; - QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Pitch = -asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f; + QEKF_INS.Roll = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f; // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺) if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f) diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 7e328f3..556aedc 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -24,7 +24,7 @@ static CANInstance sender_assignment[6] = { [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 = 0x3FE, .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}}, }; @@ -321,25 +321,15 @@ void DJIMotorControl() // 获取最终输出 set = (int16_t)pid_ref; - // 分组填入发送数据 group = motor->sender_group; num = motor->message_num; - - 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] = (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, 2u); + memset(sender_assignment[group].tx_buff + 2 * num, 0, sizeof(uint16_t)); } // 遍历flag,检查是否要发送这一帧报文