HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

修改训练赛的问题

This commit is contained in:
zcj 2024-03-26 22:50:17 +08:00
parent 2b402788d1
commit b2687cafe3
5 changed files with 127 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
application/chassis/chassis.c
View File

@ -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;

157
application/cmd/robot_cmd.c
View File

@ -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();
}
/**

24
application/gimbal/gimbal.c
View File

@ -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坐标系说明

4
application/shoot/shoot.c
View File

@ -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 // 关闭摩擦轮
{

10
modules/auto_aim/auto_aim.c
View File

@ -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;