From b42e603e0e5cd0790df36c82761cd9d6f3f28b43 Mon Sep 17 00:00:00 2001 From: chenfu <2412777093@qq.com> Date: Thu, 23 May 2024 16:27:59 +0800 Subject: [PATCH] =?UTF-8?q?=E9=81=A5=E6=8E=A7=E5=99=A8=E9=94=AE=E9=BC=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 67 +++++++++++----------- application/cmd/robot_cmd.c | 101 ++++++++++++++++++++++++++++------ application/gimbal/gimbal.c | 40 +++++++------- application/robot_def.h | 7 ++- 4 files changed, 144 insertions(+), 71 deletions(-) diff --git a/application/chassis/balance.c b/application/chassis/balance.c index c408787..3d6fae9 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -63,6 +63,7 @@ void BalanceInit() .tx_id = 0x311, .rx_id = 0x312, }, + .daemon_count = 100, .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s), .send_data_len = sizeof(Chassis_Upload_Data_s), }; @@ -225,35 +226,37 @@ static uint8_t DrivenMotorIsLost() /* 切换底盘遥控器控制和云台双板控制 */ static void ControlSwitch() { - // 根据裁判系统底盘输出电压设定底盘状态 - float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001; - if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost()) - { - chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 - return; - } + // // 根据裁判系统底盘输出电压设定底盘状态 + // float chassis_vol = referee_data->PowerHeatData.chassis_voltage * 0.001; + // if (chassis_vol < 15.0f || JointMotorIsLost() || DrivenMotorIsLost()) + // { + // chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE; // 皆离线,急停 + // return; + // } + + // // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 + // if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) + // { + // if (switch_is_up(rc_data->rc.switch_left)) + // { + // chassis_cmd_recv.chassis_mode = CHASSIS_RESET; + // chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s + // chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_; + // } + // else + // { + // chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 + // chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s + // chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial; + // chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_; + // } + // } + // else + // { + // chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); + // } + chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); - // 右侧拨杆向下,进入遥控器底盘控制,此时不响应云台控制指令 - if (switch_is_down(rc_data->rc.switch_right) && RemoteControlIsOnline()) - { - if (switch_is_up(rc_data->rc.switch_left)) - { - chassis_cmd_recv.chassis_mode = CHASSIS_RESET; - chassis_cmd_recv.vx = 0.5 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - chassis_cmd_recv.rotate_w = 0.5 * (float)rc_data[TEMP].rc.rocker_r_; - } - else - { - chassis_cmd_recv.chassis_mode = CHASSIS_FREE_DEBUG; // 自由转动&前后 - chassis_cmd_recv.vx = 0.003 * (float)rc_data[TEMP].rc.rocker_r1; // speed x, unit m/s - chassis_cmd_recv.delta_leglen = -0.0000005f * (float)rc_data[TEMP].rc.dial; - chassis_cmd_recv.offset_angle -= 0.000005 * (float)rc_data[TEMP].rc.rocker_r_; - } - } - else - { - chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm); - } } /* 腿缩回复位,只允许驱动轮电机移动 */ @@ -267,7 +270,7 @@ static void ResetChassis() chassis.dist = chassis.target_dist = 0; l_side.target_len = r_side.target_len = 0.12; // 角度输入为当前角度 - chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; + // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; // 撞墙时前后移动保证能重新站立,执行速度输入 LKMotorSetRef(l_driven, chassis_cmd_recv.vx + chassis_cmd_recv.rotate_w); @@ -320,7 +323,7 @@ static void WokingStateSet() l_side.target_len = r_side.target_len = 0.12; chassis.dist = chassis.target_dist = 0; // 角度输入为当前角度 - chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; + // chassis_cmd_recv.offset_angle = chassis.target_yaw = chassis.yaw; for (uint8_t i = 0; i < JOINT_CNT; i++) HTMotorStop(joint[i]); @@ -464,6 +467,6 @@ void BalanceTask() return; // 复位模态或急停,直接退出 // 运动模态,电机输出映射和限幅 - WattLimitSet(); - CANCommSend(cmd_can_comm, (void *)&chassis_feedback_data); + // 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 5be3b0a..163cede 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -69,6 +69,9 @@ void RobotCMDInit() gimbal_cmd_send.yaw = 0; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; + shoot_cmd_send.shoot_mode = SHOOT_OFF; + shoot_cmd_send.load_mode = LOAD_STOP; + shoot_cmd_send.friction_mode = FRICTION_OFF; robot_state = ROBOT_STOP; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -112,6 +115,8 @@ static void CalcOffsetAngle() static void RemoteControlSet() { shoot_cmd_send.bullet_speed = 30; + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; // // 云台参数,确定云台控制数据 // if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式 // { @@ -123,34 +128,46 @@ static void RemoteControlSet() { 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; } - - chassis_cmd_send.vx = 0.003f * ((float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1); + + // 右拨杆拨下去,底盘旋转 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) + { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + } + + if (switch_is_mid(rc_data[TEMP].rc.switch_left)) + { + chassis_cmd_send.delta_leglen = -0.000001f * (abs(rc_data[TEMP].rc.dial) > 100 ? (float)rc_data[TEMP].rc.dial : 0); + } + chassis_cmd_send.vx = 0.003f * ((float)rc_data[TEMP].rc.rocker_r_ + (float)rc_data[TEMP].rc.rocker_r1); gimbal_cmd_send.yaw -= 0.001f * (float)rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch -= 0.0004f * (float)rc_data[TEMP].rc.rocker_l1; + gimbal_cmd_send.pitch -= 0.0006f * (float)rc_data[TEMP].rc.rocker_l1; // 摇杆控制的软件限位 - gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, -35.0f, 30.0f); - // gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw, -180.0f, 180.0f); + gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE, PITCH_MAX_ANGLE); // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 摩擦轮控制,拨轮向上打为负,向下为正 - if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 - shoot_cmd_send.friction_mode = FRICTION_ON; - else - shoot_cmd_send.friction_mode = FRICTION_OFF; - // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 - if (rc_data[TEMP].rc.dial < -400) - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; - else - shoot_cmd_send.load_mode = LOAD_STOP; + if (switch_is_down(rc_data[TEMP].rc.switch_left)) + { + if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 + shoot_cmd_send.friction_mode = FRICTION_ON; + else + shoot_cmd_send.friction_mode = FRICTION_OFF; + // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 + if (rc_data[TEMP].rc.dial < -400) + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + else + shoot_cmd_send.load_mode = LOAD_STOP; + } + // // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, // if( rc_data[TEMP].rc.switch_left == 3) // { @@ -169,6 +186,54 @@ static void RemoteControlSet() */ 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.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.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; + } + else if (rc_data[TEMP].key[KEY_PRESS].a || rc_data[TEMP].key[KEY_PRESS].d) + { + chassis_cmd_send.direction = CHASSIS_SIDLE; + } + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q 小陀螺 + { + case 0: + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + break; + default: + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + break; + } + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F 摩擦轮 + { + case 0: + shoot_cmd_send.friction_mode = FRICTION_OFF; + break; + default: + shoot_cmd_send.friction_mode = FRICTION_ON; + break; + } + + 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 ); + } + + } /** @@ -183,7 +248,7 @@ static void EmergencyHandler() if (switch_is_down(rc_data[TEMP].rc.switch_left) || switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 { - if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 + if ((rc_data[TEMP].rc.dial > 300 && switch_is_down(rc_data[TEMP].rc.switch_left)) || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { robot_state = ROBOT_STOP; gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; @@ -201,6 +266,7 @@ 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; } } else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 @@ -210,6 +276,9 @@ static void EmergencyHandler() case 0: robot_state = ROBOT_READY; 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: diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index aae4b61..af3aa0e 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.5, //0.55 - .Ki = 0.0,//0.05 - .Kd = 0.0,//0.02 - .CoefA =0.0,//0.5 - .CoefB = 0.0,//0.6 + .Kp = 0.8, //0.5 + .Ki = 6.0,//0 + .Kd = 0.0,//0 + .CoefA =10.0,//0 + .CoefB = 0.5,//0 .Output_LPF_RC = 0, - .DeadBand = 0.0,//0.02 - .Derivative_LPF_RC=0.0,//0.008 + .DeadBand = 0.0,//0 + .Derivative_LPF_RC=0.0,//0 .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter |PID_DerivativeFilter, - .IntegralLimit = 0.0, + .IntegralLimit = 5.0, - .MaxOut = 400, + .MaxOut = 20, }, .speed_PID = { - .Kp = 15000,//22000 + .Kp = 25000,//14000 .Ki = 0,// .Kd =0, // .CoefA = 0.8, // .CoefB = 0.1, - .Output_LPF_RC = 0.0,//0.002 + .Output_LPF_RC = 0.0,//0 .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement | PID_OutputFilter, .IntegralLimit = 0, .MaxOut = 20000, @@ -72,7 +72,7 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp =0.4,//0.4 + .Kp =0.6,//0.4 .Ki = 0.0,//0.15 .Kd = 0.0,//0.006 .CoefA = 0.0,//0.5 @@ -81,17 +81,17 @@ void GimbalInit() .Output_LPF_RC = 0.0,//0.01 .Improve = PID_Trapezoid_Intergral |PID_ChangingIntegrationRate| PID_Integral_Limit| PID_OutputFilter|PID_DerivativeFilter, .IntegralLimit =1,//1 - .MaxOut = 600,//600 + .MaxOut = 10,//600 }, .speed_PID = { - .Kp=10000,//14000 + .Kp=13000,//10000 .Ki =0,//0 - .Kd =0.0,//0.0005 - .CoefA =0,//1500 - .CoefB =0,//2000 - .Output_LPF_RC = 0.0,//0.005 + .Kd =0.0,//0 + .CoefA =0,//0 + .CoefB =0,//0 + .Output_LPF_RC = 0.0,//0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_OutputFilter, - .IntegralLimit =3000,//3000 + .IntegralLimit =3000,//0 .MaxOut = 20000,//20000 }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, @@ -137,7 +137,7 @@ void GimbalTask() // DJIMotorSetFeedfoward(yaw_motor,SPEED_FEEDFORWARD); DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); - DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); + //DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); diff --git a/application/robot_def.h b/application/robot_def.h index c937da3..6dcaf02 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -18,7 +18,7 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ // #define ONE_BOARD // 单板控制整车 -//#define CHASSIS_BOARD //底盘板 +// #define CHASSIS_BOARD //底盘板 #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 @@ -29,8 +29,8 @@ #define YAW_CHASSIS_ALIGN_ECD 2716 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #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.0f) // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) +#define PITCH_MIN_ANGLE (-30.0f) // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 @@ -42,6 +42,7 @@ #define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反 #define GYRO2GIMBAL_DIR_ROLL 1 // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反 +#define BALANCE_MAX_SPEED 2.0f // 底盘最大速度,单位m/s // 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \