From 845942b3d0a246efdcb157a25fd6f2182f686e3c Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Thu, 23 May 2024 13:41:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=8F=E9=99=80=E8=9E=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 15 +++++++++++---- application/cmd/robot_cmd.c | 36 ++++++++++++++++++++--------------- application/gimbal/gimbal.c | 20 +++++++++---------- application/robot_def.h | 5 +++-- 4 files changed, 45 insertions(+), 31 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 3d6fae9..a26b707 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -394,9 +394,16 @@ static void ParamAssemble() static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */ { - - float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw); - PIDCalculate(&steer_v_pid, chassis.wz, p_ref); + if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || + chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) + { + float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw); + PIDCalculate(&steer_v_pid, chassis.wz, p_ref); + } + else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) + { + PIDCalculate(&steer_v_pid, chassis.wz, 6); + } l_side.T_wheel -= steer_v_pid.Output; r_side.T_wheel += steer_v_pid.Output; @@ -467,6 +474,6 @@ void BalanceTask() return; // 复位模态或急停,直接退出 // 运动模态,电机输出映射和限幅 - // WattLimitSet(); + WattLimitSet(); // CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data); } \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 163cede..e3b2f6e 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -72,6 +72,7 @@ void RobotCMDInit() shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.load_mode = LOAD_STOP; shoot_cmd_send.friction_mode = FRICTION_OFF; + chassis_direction = CHASSIS_ALIGN; robot_state = ROBOT_STOP; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -85,6 +86,14 @@ static void CalcOffsetAngle() { //@todo:相差一整圈时会出问题，待修复 // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 + if(chassis_direction == CHASSIS_ALIGN) + { + yaw_chassis_align_ecd = 2716; + } + else if(chassis_direction == CHASSIS_SIDLE) + { + yaw_chassis_align_ecd = 765; + } static float angle; yaw_align_angle = yaw_chassis_align_ecd * ECD_ANGLE_COEF_DJI; // 从底盘获取的yaw电机对齐角度 angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 @@ -127,12 +136,10 @@ static void RemoteControlSet() if (abs(rc_data[TEMP].rc.rocker_r1) > 500) { chassis_direction = CHASSIS_ALIGN; - yaw_chassis_align_ecd = 2716; } else if (abs(rc_data[TEMP].rc.rocker_r_) > 500) { chassis_direction = CHASSIS_SIDLE; - yaw_chassis_align_ecd = 765; } // 右拨杆拨下去,底盘旋转 @@ -151,7 +158,7 @@ static void RemoteControlSet() gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; // 摇杆控制的软件限位 gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); - + // gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -90.0f, 90+.0f); // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 摩擦轮控制,拨轮向上打为负,向下为正 @@ -188,23 +195,22 @@ static void MouseKeySet() { gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - gimbal_cmd_send.yaw -= 0.1f * rc_data[TEMP].mouse.x; - gimbal_cmd_send.pitch -= 0.1f * rc_data[TEMP].mouse.y; + gimbal_cmd_send.yaw -= (float)(rc_data[TEMP].mouse.x + rc_data[LAST].mouse.x) / 660.0f * 4.0f ; // 系数待测 + gimbal_cmd_send.pitch += (float)(rc_data[TEMP].mouse.y + rc_data[TEMP].mouse.y) / 660.0f * 4.0f; gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); - chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - + rc_data[TEMP].key[KEY_PRESS].a - rc_data[TEMP].key[KEY_PRESS].d); + chassis_cmd_send.vx = BALANCE_MAX_SPEED * (float)(rc_data[TEMP].key[KEY_PRESS].w - rc_data[TEMP].key[KEY_PRESS].s - rc_data[TEMP].key[KEY_PRESS].a + rc_data[TEMP].key[KEY_PRESS].d); chassis_cmd_send.delta_leglen = (float)(rc_data[TEMP].key[KEY_PRESS].e - rc_data[TEMP].key[KEY_PRESS].c) * 0.001f; if (rc_data[TEMP].key[KEY_PRESS].w || rc_data[TEMP].key[KEY_PRESS].s) { - chassis_cmd_send.direction = CHASSIS_ALIGN; + chassis_direction = CHASSIS_ALIGN; } else if (rc_data[TEMP].key[KEY_PRESS].a || rc_data[TEMP].key[KEY_PRESS].d) { - chassis_cmd_send.direction = CHASSIS_SIDLE; + chassis_direction = CHASSIS_SIDLE; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺 @@ -227,13 +233,11 @@ static void MouseKeySet() break; } - if(rc_data[TEMP].mouse.press_r) + if (rc_data[TEMP].mouse.press_r) { - gimbal_cmd_send.yaw = ( vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw ); - gimbal_cmd_send.pitch =( vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch ); + gimbal_cmd_send.yaw = (vision_recv_data->yaw == 0 ? gimbal_cmd_send.yaw : vision_recv_data->yaw); + gimbal_cmd_send.pitch = (vision_recv_data->pitch == 0 ? gimbal_cmd_send.pitch : vision_recv_data->pitch); } - - } /** @@ -278,7 +282,6 @@ static void EmergencyHandler() shoot_cmd_send.shoot_mode = SHOOT_ON; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - chassis_cmd_send.direction = CHASSIS_ALIGN; break; default: @@ -288,6 +291,7 @@ static void EmergencyHandler() shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.load_mode = LOAD_STOP; + chassis_cmd_send.direction = CHASSIS_ALIGN; gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle; // 急停时设定值保持与实际值同步，避免恢复时疯转 gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch; @@ -313,6 +317,8 @@ void RobotCMDTask() EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 + chassis_cmd_send.direction = chassis_direction; + CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index af3aa0e..9b1851c 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -26,26 +26,26 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 0.8, //0.5 - .Ki = 6.0,//0 - .Kd = 0.0,//0 - .CoefA =10.0,//0 - .CoefB = 0.5,//0 + .Kp = 0.5, //1.0 + .Ki = 0.0,//0.8 + .Kd = 0.0,//0.005 + .CoefA =0.0,//4.0 + .CoefB = 0.0,//0.5 .Output_LPF_RC = 0, .DeadBand = 0.0,//0 - .Derivative_LPF_RC=0.0,//0 + .Derivative_LPF_RC=0,//0.01 .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter, - .IntegralLimit = 5.0, + .IntegralLimit = 4.0, .MaxOut = 20, }, .speed_PID = { - .Kp = 25000,//14000 + .Kp = 14000,//22500 .Ki = 0,// .Kd =0, // .CoefA = 0.8, // .CoefB = 0.1, - .Output_LPF_RC = 0.0,//0 + .Output_LPF_RC = 0.002,//0 .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter, .IntegralLimit = 0, .MaxOut = 20000, @@ -84,7 +84,7 @@ void GimbalInit() .MaxOut = 10,//600 }, .speed_PID = { - .Kp=13000,//10000 + .Kp=10000,//10000 .Ki =0,//0 .Kd =0.0,//0 .CoefA =0,//0 diff --git a/application/robot_def.h b/application/robot_def.h index 6dcaf02..f837b1b 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -18,8 +18,8 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ // #define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 -#define GIMBAL_BOARD //云台板 +#define CHASSIS_BOARD //底盘板 +// #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 // #define VISION_USE_UART // 使用串口发送视觉数据 @@ -27,6 +27,7 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 2716 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_CHASSIS_SIDE_ECD 716 // 云台和底盘侧面对齐时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE (30.0f) // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)