From b2687cafe31121443efafed01e061f84c8f28782 Mon Sep 17 00:00:00 2001 From: zcj <2487150395@qq.com> Date: Tue, 26 Mar 2024 22:50:17 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=AE=AD=E7=BB=83=E8=B5=9B?= =?UTF-8?q?=E7=9A=84=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 6 +- application/cmd/robot_cmd.c | 157 +++++++++++++++++++++++----------- application/gimbal/gimbal.c | 24 +++--- application/shoot/shoot.c | 4 +- modules/auto_aim/auto_aim.c | 10 +-- 5 files changed, 127 insertions(+), 74 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index d761d78..1123af7 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -43,7 +43,7 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 -static referee_info_t* referee_data; // 用于获取裁判系统的数据 + static SuperCapInstance *cap; // 超级电容 @@ -110,7 +110,7 @@ void ChassisInit() chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rb = DJIMotorInit(&chassis_motor_config); - referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI + SuperCap_Init_Config_s cap_conf = { .can_config = { @@ -289,7 +289,7 @@ void ChassisTask() // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; + //chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index d10dc5b..48b2445 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -27,6 +27,8 @@ static CANCommInstance *cmd_can_comm; // 双板通信 static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 #endif // ONE_BOARD +static float temp_yaw; //for test +static float temp_index; //for test static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 @@ -54,8 +56,9 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 +static referee_info_t* referee_data; // 用于获取裁判系统的数据 extern float horizontal_angle; -int target_index = -1; +static int target_index = -1; void RobotCMDInit() { @@ -83,6 +86,7 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD + referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI gimbal_cmd_send.pitch = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 @@ -119,6 +123,15 @@ static void CalcOffsetAngle() // chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle + 360.0f; // else // chassis_cmd_send.offset_angle = chassis_cmd_send.offset_angle; +} +static void death_check() +{ + if(referee_data->GameRobotState.remain_HP <= 0) + { + chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; + } + } static void update_ui_data() { @@ -136,15 +149,9 @@ static void pitch_limit() if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18; if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38; } -static void hand_aim_mode() -{ - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9; - pitch_limit(); -} static void auto_aim_mode() { - trajectory_cal.v0 = 30; //弹速30 + trajectory_cal.v0 = 15.8f; //弹速30 if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) { aim_select.suggest_fire = 0; @@ -163,14 +170,27 @@ static void auto_aim_mode() auto_aim_flag = 1; target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + temp_index = target_index; VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]); - gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI; + float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw; + float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now; + if(diff_yaw>180) + diff_yaw -= 360; + else if(diff_yaw<-180) + diff_yaw += 360; + + gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw; gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI; - float target_yaw = atan2f(aim_select.armor_pose[target_index].x, - aim_select.armor_pose[target_index].y); +// float target_yaw = -atan2f(aim_select.armor_pose[target_index].x, +// aim_select.armor_pose[target_index].y); + + //float target_yaw = aim_select.armor_pose[target_index].yaw; + float target_yaw = trajectory_cal.cmd_yaw * 180 / PI; + temp_yaw = target_yaw; //for test + float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw); if (yaw_err <= 3) //3度 { @@ -214,7 +234,7 @@ static void RemoteControlSet() vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制 { - gimbal_cmd_send.yaw += 0.003f * (float)rc_data[TEMP].rc.rocker_l_; + gimbal_cmd_send.yaw -= 0.003f * (float)rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.pitch += 0.0007f * (float)rc_data[TEMP].rc.rocker_l1; pitch_limit(); @@ -259,6 +279,41 @@ static void MouseKeySet() chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 20000 - rc_data[TEMP].key[KEY_PRESS].d * 20000; // 系数待测 chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 20000 - rc_data[TEMP].key[KEY_PRESS].s * 20000; + gimbal_cmd_send.yaw -= (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测 + gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 *5 ; + + + if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式 + { + if (shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.load_mode = LOAD_1_BULLET; + } + } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r)) + { + shoot_cmd_send.load_mode = LOAD_STOP; + + } + if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式 + { + + if ((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 + && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && + vision_recv_data->vz == 0)) + { + shoot_cmd_send.load_mode = LOAD_STOP; + } + + + else { + auto_aim_mode(); + if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l && + shoot_cmd_send.friction_mode == FRICTION_ON) { + shoot_cmd_send.load_mode = LOAD_1_BULLET; + } else { + shoot_cmd_send.load_mode = LOAD_STOP; + } + } + } // switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速// // { @@ -273,43 +328,44 @@ static void MouseKeySet() // break; // } - switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式 - { - case 1: - if((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 - && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && - vision_recv_data->vz == 0)) - hand_aim_mode(); - else - auto_aim_mode(); - break; - default: - hand_aim_mode(); - break; - } - switch (rc_data[TEMP].mouse.press_l) // 左键发射模式 - { - case 1: - if(shoot_cmd_send.friction_mode == FRICTION_ON) - { - if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l) - { - if(aim_select.suggest_fire == 1) - shoot_cmd_send.load_mode = LOAD_1_BULLET; - else - shoot_cmd_send.load_mode = LOAD_STOP; - break; - } - shoot_cmd_send.load_mode = LOAD_1_BULLET; - } - else - shoot_cmd_send.load_mode = LOAD_STOP; - break; - default: - shoot_cmd_send.load_mode = LOAD_STOP; - break; - } +// switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式 +// { +// case 1: +// if((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 +// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && +// vision_recv_data->vz == 0)) +// hand_aim_mode(); +// else +// auto_aim_mode(); +// break; +// default: +// hand_aim_mode(); +// break; +// } +// +// switch (rc_data[TEMP].mouse.press_l) // 左键发射模式 +// { +// case 1: +// if(shoot_cmd_send.friction_mode == FRICTION_ON) +// { +// if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l) +// { +// if(aim_select.suggest_fire == 1) +// shoot_cmd_send.load_mode = LOAD_1_BULLET; +// else +// shoot_cmd_send.load_mode = LOAD_STOP; +// break; +// } +// shoot_cmd_send.load_mode = LOAD_1_BULLET; +// } +// else +// shoot_cmd_send.load_mode = LOAD_STOP; +// break; +// default: +// shoot_cmd_send.load_mode = LOAD_STOP; +// break; +// } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI { @@ -355,6 +411,7 @@ static void MouseKeySet() // chassis_cmd_send.chassis_speed_buff = 100; // break; // } + switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量 { case 1: @@ -365,6 +422,10 @@ static void MouseKeySet() break; } + + pitch_limit(); + death_check(); + } /** diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 1062122..de760c6 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -27,17 +27,17 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 200, // 8 + .Kp = 4, // 8 .Ki = 0, - .Kd = 20, + .Kd = 0.3, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, .MaxOut = 500, }, .speed_PID = { - .Kp = 70, // 70 - .Ki = 0, // 200 + .Kp = 5000, // 70 + .Ki = 1000, // 200 .Kd = 0,//10 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 3000, @@ -52,7 +52,7 @@ void GimbalInit() .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP |SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = GM6020 }; @@ -64,20 +64,20 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 20, // 65 + .Kp = 0.3f,//20, // 65 .Ki = 0, - .Kd = 1, + .Kd = 0,//1, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100,//100 .MaxOut = 500,//500 }, .speed_PID = { - .Kp = 30, // 50 - .Ki = 0, // 350 - .Kd = 2, // 0 + .Kp = 2500,//11000, // 50 + .Ki = 1000, // 350 + .Kd = 0,//15000, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 150,//2500 - .MaxOut = 2000,//20000 + .IntegralLimit = 2500, + .MaxOut = 20000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 6f75ceb..95bfd55 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -193,9 +193,9 @@ void ShootTask() // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) if (shoot_cmd_recv.friction_mode == FRICTION_ON) { - DJIMotorSetRef(friction_l, 36000); + DJIMotorSetRef(friction_l, 35000); // DJIMotorSetRef(friction_r, 39000); - DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500 + DJIMotorSetRef(friction_z, 35000);//39000/6 = 6500 } else // 关闭摩擦轮 { diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 6e78c58..ddcc590 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -30,14 +30,6 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI; } -// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); -// -// //因为是平衡步兵 只需判断两块装甲板即可 -// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw); -// if (temp_yaw_diff < yaw_diff_min) { -// yaw_diff_min = temp_yaw_diff; -// idx = 1; -// } // 平衡步兵选择两块装甲板中较近的装甲板 float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); @@ -395,7 +387,7 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ * @param[in] v_x0:弹速水平分量 * @retval 返回空 */ -const float k1 = 0.0761; //标准大气压25度下空气阻力系数 +const float k1 = 0.015; //标准大气压25度下空气阻力系数 float get_fly_time(float x, float vx, float v_x0) { float t = 0; float f_ti = 0, df_ti = 0;