From 1b4e26c50d3a0d6264715f966748a6e2732bc43e Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Tue, 16 Apr 2024 16:52:10 +0800 Subject: [PATCH] =?UTF-8?q?=E8=81=94=E7=9B=9F=E8=B5=9B=E7=89=88=E6=9C=AC?= =?UTF-8?q?=20=20=E4=BF=AE=E5=A4=8D=E5=8D=A1=E5=BC=B9=E6=A3=80=E6=B5=8B...?= =?UTF-8?q?=E5=85=B6=E4=BB=96=E6=94=B9=E4=BA=86=E5=95=A5=E6=88=91=E4=B9=9F?= =?UTF-8?q?=E4=B8=8D=E8=AE=B0=E5=BE=97=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 20 +- application/cmd/robot_cmd.c | 467 ++++++++++++++---------- application/gimbal/gimbal.c | 60 ++- application/robot_def.h | 33 +- application/shoot/shoot.c | 26 +- modules/auto_aim/auto_aim.c | 195 +++++++--- modules/auto_aim/auto_aim.h | 18 +- modules/imu/ins_task.c | 2 +- modules/master_machine/master_process.c | 8 +- modules/master_machine/master_process.h | 35 +- modules/motor/ECmotor/ECA8210.c | 48 ++- modules/motor/ECmotor/ECA8210.h | 4 + modules/motor/motor_task.c | 12 +- modules/referee/referee_protocol.h | 52 ++- modules/vofa/vofa.c | 2 +- sentry_left.jdebug | 1 - sentry_left.jdebug.user | 56 +-- 17 files changed, 665 insertions(+), 374 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 3f53d1a..c4c08f9 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -43,8 +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 Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI + static SuperCapInstance *cap; // 超级电容 static PowerMeterInstance *power_meter; //功率计 @@ -110,8 +109,6 @@ void ChassisInit() chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; motor_rb = DJIMotorInit(&chassis_motor_config); - referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI - SuperCap_Init_Config_s cap_conf = { .can_config = { .can_handle = &hcan2, @@ -270,10 +267,10 @@ void ChassisTask() //chassis_cmd_recv.wz = 100.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); break; case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 - chassis_cmd_recv.wz = 0.05f * chassis_cmd_recv.offset_angle; + chassis_cmd_recv.wz = 0.1f * chassis_cmd_recv.offset_angle; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - chassis_cmd_recv.wz = -5; + chassis_cmd_recv.wz = -1.2f*PI; break; default: break; @@ -318,10 +315,13 @@ void ChassisTask() // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red - // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; - // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 - // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; - // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; +// 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; +// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; +// +// chassis_feedback_data.game_progress = referee_data->GameState.game_progress; +// chassis_feedback_data.remain_HP = referee_data->GameRobotState.current_HP; // 推送反馈消息 #ifdef ONE_BOARD diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 99b0ef3..51b9a7a 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -9,14 +9,19 @@ #include "general_def.h" #include "dji_motor.h" #include "auto_aim.h" +#include "referee_task.h" // bsp #include "bsp_dwt.h" #include "bsp_log.h" // 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE 272.799561F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 +#define YAW_ALIGN_ANGLE 60.0F //(YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360 +#define PITCH_SCAN -10.0f //扫描阶段PITCH固定角度 + +#define SHOOT_RATE 10.0f //射频 + /* cmd应用包含的模块实例指针和交互信息存储*/ #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #include "can_comm.h" @@ -27,6 +32,10 @@ static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 #endif // ONE_BOARD +#include "can_comm.h" + +static CANCommInstance *cmd_can_comm; //哨兵左右云台 双板通信 + static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 @@ -52,11 +61,15 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 static Robot_Status_e robot_state; // 机器人整体工作状态 -void RobotCMDInit() -{ +static referee_info_t* referee_data; // 用于获取裁判系统的数据 +static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI + +void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 vision_recv_data = VisionInit(&huart1); // 视觉通信串口 + referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI + gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); @@ -78,7 +91,20 @@ void RobotCMDInit() }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD - gimbal_cmd_send.pitch = 0; + + CANComm_Init_Config_s comm_conf = { + .can_config = { + .can_handle = &hcan1, + .tx_id = 0x312, + .rx_id = 0x311, + }, + .recv_data_len = sizeof(Gimbal_Upload_Data_s), + .send_data_len = sizeof(Gimbal_Ctrl_Cmd_s), + }; + cmd_can_comm = CANCommInit(&comm_conf); + + gimbal_cmd_send.pitch = PITCH_SCAN; + gimbal_cmd_send.yaw = 0; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -88,12 +114,17 @@ void RobotCMDInit() * 单圈绝对角度的范围是0~360,说明文档中有图示 * */ -static void CalcOffsetAngle() -{ +static void CalcOffsetAngle() { // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 + + //如果大yaw轴电机不在线 不计算底盘跟随 + if (gimbal_fetch_data.big_yaw_online == 0) { + chassis_cmd_send.offset_angle = 0; + return; + } static float angle; angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 -#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 +#if 1 // 如果大于180度 if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE) chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; else if (angle > 180.0f + YAW_ALIGN_ANGLE) @@ -114,22 +145,21 @@ static void CalcOffsetAngle() * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * */ -static void RemoteControlSet() -{ +static void RemoteControlSet() { // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? - if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台 + if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺 { chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; - } - else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台 { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } + // 云台参数,确定云台控制数据 - if (switch_is_mid(rc_data[TEMP].rc.switch_left) ) + if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // (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)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制 @@ -137,80 +167,93 @@ static void RemoteControlSet() // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制 // ... gimbal_cmd_send.yaw -= 0.0005f * (float) rc_data[TEMP].rc.rocker_l_; - gimbal_cmd_send.pitch -= 0.0005f * (float) rc_data[TEMP].rc.rocker_l1; + gimbal_cmd_send.big_yaw -= 0.0005f * (float) rc_data[TEMP].rc.rocker_l_; + 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; if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE; } - // 左侧开关状态为[下],视觉模式 -// if (switch_is_down(rc_data[TEMP].rc.switch_left)) { -// trajectory_cal.v0 = 30; //弹速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; -// //未发现目标 -// no_find_cnt++; -// -// if (no_find_cnt >= 2000) { -// //gimbal_scan_flag = 1; -// //auto_aim_flag = 0; -// } -// //else -// //auto_aim_flag = 1; -// } else { -// //弹道解算 -// no_find_cnt = 0; -// auto_aim_flag = 1; -// -// auto_aim(&aim_select, &trajectory_cal, vision_recv_data); -// -// gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI; -// -// gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI; -// -// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); -// if (yaw_err <= 0.008) //3度 -// aim_select.suggest_fire = 1; -// else -// aim_select.suggest_fire = 0; -// } -// } + //左侧开关状态为[下],视觉模式 + if (switch_is_down(rc_data[TEMP].rc.switch_left)) { + gimbal_cmd_send.control_mode = TEST_CONTROL; + trajectory_cal.v0 = 28; //弹速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; + //未发现目标 + no_find_cnt++; + + if (no_find_cnt >= 200) { + //gimbal_scan_flag = 1; + //auto_aim_flag = 0; + } + //else + //auto_aim_flag = 1; + } else { + //弹道解算 + no_find_cnt = 0; + auto_aim_flag = 1; + + auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + + 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; + + gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI; + + float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); + if (yaw_err <= 0.008) //3度 + aim_select.suggest_fire = 1; + else + aim_select.suggest_fire = 0; + } + } // 云台软件限位 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vx = 0.001f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向 - chassis_cmd_send.vy = -0.001f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向 - - if (vision_recv_data->nav_vx != 0 || vision_recv_data->nav_vy != 0 || vision_recv_data->nav_wz != 0) { - chassis_cmd_send.vx = vision_recv_data->nav_vx; - chassis_cmd_send.vy = vision_recv_data->nav_vy; - chassis_cmd_send.wz = vision_recv_data->nav_wz; - } + chassis_cmd_send.vx = 0.001f * (float) rc_data[TEMP].rc.rocker_r1; // _水平方向 + chassis_cmd_send.vy = -0.001f * (float) rc_data[TEMP].rc.rocker_r_; // 1数值方向 + //chassis_cmd_send.wz = -0.001f * (float)rc_data[TEMP].rc.rocker_l_; // 1数值方向 +// if (vision_recv_data->nav_vx != 0 || vision_recv_data->nav_vy != 0 || vision_recv_data->nav_wz != 0) { +// chassis_cmd_send.vx = vision_recv_data->nav_vx; +// chassis_cmd_send.vy = vision_recv_data->nav_vy; +// chassis_cmd_send.wz = vision_recv_data->nav_wz; +// +// } // 发射参数 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 ; // 弹舱舵机控制,待添加servo_motor模块,开启 - else - ; // 弹舱舵机控制,待添加servo_motor模块,关闭 + else; // 弹舱舵机控制,待添加servo_motor模块,关闭 // 摩擦轮控制,拨轮向上打为负,向下为正 if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 shoot_cmd_send.friction_mode = FRICTION_ON; + //gimbal_cmd_send.friction_mode = FRICTION_ON; else shoot_cmd_send.friction_mode = FRICTION_OFF; + //gimbal_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 if (rc_data[TEMP].rc.dial < -500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + //gimbal_cmd_send.load_mode = LOAD_BURSTFIRE; else shoot_cmd_send.load_mode = LOAD_STOP; + //gimbal_cmd_send.load_mode = LOAD_STOP; + + gimbal_cmd_send.shoot_mode = shoot_cmd_send.shoot_mode;//遥控发射指令同步发送给右边 + gimbal_cmd_send.load_mode = shoot_cmd_send.load_mode;//遥控发射指令同步发送给右边 + gimbal_cmd_send.friction_mode = shoot_cmd_send.friction_mode;//遥控发射指令同步发送给右边 + // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, - shoot_cmd_send.shoot_rate = 8; + shoot_cmd_send.shoot_rate = SHOOT_RATE; //检测到卡弹 拨弹盘反转 - if(shoot_fetch_data.stalled_flag == 1) + if (shoot_fetch_data.stalled_flag == 1) shoot_cmd_send.load_mode = LOAD_REVERSE; } @@ -218,130 +261,88 @@ static void RemoteControlSet() * @brief 自动模式时模式和控制量设置 * */ -static void AutoControlSet() -{ -// static int8_t gimbal_scan_flag = 1; -// static int8_t scan_dir = 1; -// trajectory_cal.v0 = 30; //弹速30 -// -// //小云台扫描 -// if(gimbal_scan_flag == 1) -// { -// if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f; -// else gimbal_cmd_send.yaw -= 0.001f; -// if(gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 0.1f) scan_dir = 1; -// if(gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 0.1f) scan_dir = -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) { -// aim_select.suggest_fire = 0; -// //未发现目标 -// no_find_cnt++; -// if (no_find_cnt >= 2000) { -// gimbal_scan_flag = 1; -// auto_aim_flag = 0; -// } -// else -// auto_aim_flag = 1; -// } else { -// //弹道解算 -// no_find_cnt = 0; -// auto_aim_flag = 1; -// -// auto_aim(&aim_select, &trajectory_cal, vision_recv_data); -// gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI; -// gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI; -// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); -// if (yaw_err <= 0.008) //3度 -// aim_select.suggest_fire = 1; -// else -// aim_select.suggest_fire = 0; -// } -} -/** - * @brief 输入为键鼠时模式和控制量设置 - * - */ -static void MouseKeySet() -{ - chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测 - chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300; +static uint8_t sentry_state; - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10; +static void AutoControlSet() { + chassis_cmd_send.chassis_mode = CHASSIS_ROTATE; + +// chassis_cmd_send.vx = vision_recv_data->nav_vx; +// chassis_cmd_send.vy = vision_recv_data->nav_vy; + //云台保持陀螺仪控制 + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; + + + static int8_t gimbal_scan_flag = 1; + static int8_t yaw_dir = 1; + static int8_t pitch_dir = 1; + //trajectory_cal.v0 = 30; //弹速30 + + //小云台扫描 + if (gimbal_scan_flag == 1) { + + gimbal_cmd_send.pitch = PITCH_SCAN; + + //if(sentry_state > 1) + { + if (yaw_dir == 1) gimbal_cmd_send.yaw += 0.06f; + else gimbal_cmd_send.yaw -= 0.06f; + if (gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 2.0f) yaw_dir = 1; + if (gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 2.0f) yaw_dir = -1; + } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 - { - case 0: - shoot_cmd_send.bullet_speed = 15; - break; - case 1: - shoot_cmd_send.bullet_speed = 18; - break; - default: - shoot_cmd_send.bullet_speed = 30; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式 - { - case 0: shoot_cmd_send.load_mode = LOAD_STOP; - break; - case 1: - shoot_cmd_send.load_mode = LOAD_1_BULLET; - break; - case 2: - shoot_cmd_send.load_mode = LOAD_3_BULLET; - break; - default: - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; - break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 - { - case 0: - shoot_cmd_send.lid_mode = LID_OPEN; - break; - default: - shoot_cmd_send.lid_mode = LID_CLOSE; - break; - } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 - { - case 0: - shoot_cmd_send.friction_mode = FRICTION_OFF; - break; - default: + + trajectory_cal.v0 = 35; //弹速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; + //未发现目标 + no_find_cnt++; + + if (no_find_cnt >= 200) { + //gimbal_scan_flag = 1; + //auto_aim_flag = 0; + } + //else + //auto_aim_flag = 1; + } else { + //弹道解算 + no_find_cnt = 0; + auto_aim_flag = 1; + + auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + + 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; + + gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI; + + float yaw_err = fabsf(gimbal_cmd_send.yaw - gimbal_fetch_data.gimbal_imu_data.Yaw); + if (yaw_err <= 5) //3度 + { + aim_select.suggest_fire = 1; + shoot_cmd_send.shoot_mode = SHOOT_ON; + shoot_cmd_send.shoot_rate = SHOOT_RATE; + shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + } else { + aim_select.suggest_fire = 0; + //shoot_cmd_send.shoot_mode = SHOOT_OFF; + shoot_cmd_send.load_mode = LOAD_STOP; + } + //摩擦轮始终开启 shoot_cmd_send.friction_mode = FRICTION_ON; - break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 - { - case 0: - chassis_cmd_send.chassis_speed_buff = 40; - break; - case 1: - chassis_cmd_send.chassis_speed_buff = 60; - break; - case 2: - chassis_cmd_send.chassis_speed_buff = 80; - break; - default: - chassis_cmd_send.chassis_speed_buff = 100; - break; - } - switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量 - { - case 1: + // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, + //shoot_cmd_send.shoot_rate = 8; + //检测到卡弹 拨弹盘反转 - break; + shoot_cmd_send.friction_mode = FRICTION_ON; - default: - - break; - } + if (shoot_fetch_data.stalled_flag == 1) + shoot_cmd_send.load_mode = LOAD_REVERSE; } /** @@ -351,8 +352,7 @@ static void MouseKeySet() * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * */ -static void EmergencyHandler() -{ +static void EmergencyHandler() { // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { @@ -362,23 +362,25 @@ static void EmergencyHandler() shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.load_mode = LOAD_STOP; + + gimbal_cmd_send.control_mode = ZERO_FORCE; LOGERROR("[CMD] emergency stop!"); } // 遥控器右侧开关为[上],恢复正常运行 - if (switch_is_up(rc_data[TEMP].rc.switch_right)) - { + if (switch_is_up(rc_data[TEMP].rc.switch_right)) { robot_state = ROBOT_READY; shoot_cmd_send.shoot_mode = SHOOT_ON; LOGINFO("[CMD] reinstate, robot ready"); } } +uint8_t sentry_behave(); + /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ -void RobotCMDTask() -{ +void RobotCMDTask() { // 从其他应用获取回传数据 #ifdef ONE_BOARD - SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data); + SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data); #endif // ONE_BOARD #ifdef GIMBAL_BOARD chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm); @@ -389,29 +391,116 @@ void RobotCMDTask() // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 -// if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制 -// RemoteControlSet(); -// else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 -// MouseKeySet(); + + static control_mode_e last_gimbal_mode = 0; if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式 + { + gimbal_cmd_send.control_mode = AUTO_CONTROL; AutoControlSet(); - else + } + + else { + if (last_gimbal_mode == AUTO_CONTROL) { + gimbal_cmd_send.yaw = 0; + gimbal_cmd_send.pitch = 0; + } + gimbal_cmd_send.control_mode = RC_CONTROL; RemoteControlSet(); + } + gimbal_cmd_send.enemy_color = !referee_data->referee_id.Robot_Color; + gimbal_cmd_send.rest_heat = referee_data->PowerHeatData.shooter_17mm_2_barrel_heat; + + + + last_gimbal_mode = gimbal_cmd_send.control_mode; + + //热量控制 + if(referee_data->PowerHeatData.shooter_17mm_1_barrel_heat>350) + shoot_cmd_send.load_mode = LOAD_STOP; EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 +// 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; +// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; +// chassis_feedback_data.game_progress = referee_data->GameState.game_progress; +// chassis_feedback_data.remain_HP = referee_data->GameRobotState.current_HP; // 设置视觉发送数据,还需增加加速度和角速度数据 // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed) + //sentry_state = sentry_behave(); + + if(referee_data->GameState.game_progress == 4) + { + if(referee_data->GameState.stage_remain_time >= (300-60)) + sentry_state = 1;//前一分钟 + else if(referee_data->GameState.stage_remain_time <= 60) + sentry_state = 3;//后一分钟 + else + sentry_state = 2;//中间阶段 + } + + gimbal_cmd_send.game_state = sentry_state; + + VisionSetFlag(!referee_data->referee_id.Robot_Color,sentry_state,referee_data->GameRobotState.current_HP); // 推送消息,双板通信,视觉通信等 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 #ifdef ONE_BOARD - PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send); + PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send); #endif // ONE_BOARD #ifdef GIMBAL_BOARD CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); #endif // GIMBAL_BOARD - PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send); - PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send); + + CANCommSend(cmd_can_comm, (void *) &gimbal_cmd_send); //哨兵左右云台双板通信 + PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send); + PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); VisionSend(&vision_send_data); } + +uint8_t sentry_behave() +{ + static uint8_t behave_flag = 0; + static uint8_t last_behave_flag = 0; + static uint8_t recover_cnt = 0; + // 比赛阶段 0准备阶段 1前1分钟 2中间阶段 3后一分钟 4比赛结束 9补给区回血 10保留（无响应） + if(referee_data->GameState.game_progress < 4) + behave_flag = 0; + else if(referee_data->GameState.game_progress == 4) + { + if(referee_data->GameState.stage_remain_time >= (300-60)) + behave_flag = 1;//前一分钟 + else if(referee_data->GameState.stage_remain_time <= 60) + behave_flag = 3;//后一分钟 + else + behave_flag = 2;//中间阶段 + } + else if (referee_data->GameState.game_progress == 5) + behave_flag = 4; + + if(referee_data->GameRobotState.current_HP < 200 && referee_data->GameState.stage_remain_time >= 60 && recover_cnt <= 1) + { + behave_flag = 9; + } + + + + static uint16_t last_HP = 600; + static uint16_t recover_start_T = 0; + + if(behave_flag == 9 ) //回去回血了 + { + if(last_behave_flag != 9) + { + recover_start_T = referee_data->GameState.stage_remain_time; + recover_cnt ++ ; + } + + if((referee_data->GameState.stage_remain_time - recover_start_T)>10) + behave_flag = 10;//随便给一个值 + } + + last_behave_flag = behave_flag; + return behave_flag; +} diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 64f9497..77cc43b 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -30,6 +30,9 @@ static float big_yaw_angle = 0.0f; //大yaw轴绝对角度 通过小云台陀 static float big_yaw_speed = 0.0f;//大yaw轴绝对角速度 sin_input_generate_t sinInputGenerate; +static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令 +static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 + void GimbalInit() { @@ -43,7 +46,7 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 1.0f, // 8 + .Kp = 0.5f, // 8 .Ki = 0, .Kd = 0, .DeadBand = 0.1, @@ -82,7 +85,7 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 2.0f,//4.0f,//2.0f + .Kp = 1.0f,//4.0f,//2.0f .Ki = 0.0f,//1,//0 .Kd = 0.0f,//0.0f .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, @@ -90,12 +93,12 @@ void GimbalInit() .MaxOut = 500, }, .speed_PID = { - .Kp = 800,//6000,//800 - .Ki = 100,//500,//100 + .Kp = 2500,//6000,//800 + .Ki = 500,//500,//100 .Kd = 0,//0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, - .IntegralLimit = 5000, - .MaxOut = 10000, + .IntegralLimit = 10000, + .MaxOut = 30000, }, .other_angle_feedback_ptr = &gimba_IMU_data->Roll,//&gimba_IMU_data->Pitch, // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 @@ -130,7 +133,7 @@ void GimbalInit() .DeadBand = 0, }, .speed_PID = { - .Kp = 300,//2500, // 50 + .Kp = 150,//150,//这里KP尽可能小 防止震荡脱齿 .Ki = 0,//200, // 350 .Kd = 0, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, @@ -160,6 +163,8 @@ void GimbalInit() gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); + chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); + //sin_input_frequency_init(&sinInputGenerate); } @@ -169,6 +174,10 @@ void GimbalTask() // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + SubGetMessage(chassis_sub, &chassis_cmd_recv); + + + //gimbal_cmd_recv.big_yaw = 0; // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref @@ -185,9 +194,6 @@ void GimbalTask() DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); ECMotorEnable(big_yaw_motor); -// DJIMotorStop(yaw_motor); -// DJIMotorStop(pitch_motor); -// ECMotorStop(big_yaw_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); @@ -195,7 +201,9 @@ void GimbalTask() DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - ECMotorSetRef(big_yaw_motor,gimbal_cmd_recv.yaw); + + + ECMotorSetRef(big_yaw_motor,gimbal_cmd_recv.big_yaw); break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) @@ -208,7 +216,14 @@ void GimbalTask() DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); - ECMotorSetRef(big_yaw_motor,gimbal_cmd_recv.yaw); +// big_yaw_angle = chassis_cmd_recv.offset_angle; +// big_yaw_speed = big_yaw_motor->measure.speed_rads; +// +// ECMotorSetRef(big_yaw_motor,0); +// big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44); +// big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; + + ECMotorSetRef(big_yaw_motor,gimbal_cmd_recv.big_yaw); break; default: break; @@ -223,8 +238,20 @@ void GimbalTask() //ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0); float theta = gimba_IMU_data->Roll/180*PI;//(pitch_motor->measure.angle_single_round - 5505 * ECD_ANGLE_COEF_DJI)/180*PI; yaw_speed = gimba_IMU_data->Gyro[2] * arm_cos_f32(theta) - gimba_IMU_data->Gyro[0] * arm_sin_f32(theta); + //big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44); + //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; + + big_yaw_angle = gimba_IMU_data->YawTotalAngle - (yaw_motor->measure.total_angle - 44); - big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; + //big_yaw_speed = yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD; + + big_yaw_speed = (1.0f - 0.75f) * big_yaw_speed + + 0.75f * (yaw_speed - yaw_motor->measure.speed_aps * DEGREE_2_RAD); + + +// big_yaw_angle = chassis_cmd_recv.offset_angle; +// //loop_float_constrain(big_yaw_motor->measure.total_angle * (4.0f/3.0f),0,2*PI) * RAD_2_DEGREE - 340.0f; +// big_yaw_speed = big_yaw_motor->measure.speed_rads; //yaw轴角速度 //重力补偿力矩 @@ -245,11 +272,12 @@ void GimbalTask() gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; static float big_yaw_relative_angle; - - big_yaw_relative_angle = big_yaw_motor->measure.total_angle * (4.0f/3.0f); + + big_yaw_relative_angle = big_yaw_motor->measure.total_angle ; gimbal_feedback_data.yaw_motor_single_round_angle = loop_float_constrain(big_yaw_relative_angle,0,2*PI) * RAD_2_DEGREE; - gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.angle_single_round; + gimbal_feedback_data.mini_yaw_encode_angle = yaw_motor->measure.total_angle; + gimbal_feedback_data.big_yaw_online = ECMotorIsOnline(big_yaw_motor); // 推送消息 PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); diff --git a/application/robot_def.h b/application/robot_def.h index 6126437..9dc2f92 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -27,13 +27,13 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 8012 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 -#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 +#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 39 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) -#define YAW_MAX_ENCODE_ANGLE 200.0f //小yaw编码器限位 -#define YAW_MIN_ENCODE_ANGLE 9.14f +#define YAW_MAX_ENCODE_ANGLE 139.0f //小yaw编码器限位 +#define YAW_MIN_ENCODE_ANGLE 10.0f // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f @@ -99,6 +99,15 @@ typedef enum GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 } gimbal_mode_e; +// 控制模式设置 +typedef enum +{ + RC_CONTROL = 0, // 遥控模式 + AUTO_CONTROL, // 自动模式 + TEST_CONTROL, // 测试模式 + ZERO_FORCE, // 失能模式 +} control_mode_e; + // 发射模式设置 typedef enum { @@ -155,13 +164,23 @@ typedef struct // cmd发布的云台控制数据,由gimbal订阅 typedef struct { // 云台角度控制 + float big_yaw; float yaw; float pitch; float chassis_rotate_wz; - + control_mode_e control_mode; gimbal_mode_e gimbal_mode; + shoot_mode_e shoot_mode; + loader_mode_e load_mode; + friction_mode_e friction_mode; + + uint16_t rest_heat; // 剩余枪口热量 + Enemy_Color_e enemy_color; // 0 for blue, 1 for red + + uint8_t game_state;//比赛阶段 } Gimbal_Ctrl_Cmd_s; + // cmd发布的发射控制数据,由shoot订阅 typedef struct { @@ -194,6 +213,9 @@ typedef struct Bullet_Speed_e bullet_speed; // 弹速限制 Enemy_Color_e enemy_color; // 0 for blue, 1 for red + uint8_t game_progress; //比赛阶段 + uint16_t remain_HP; //当前血量 + } Chassis_Upload_Data_s; @@ -202,6 +224,7 @@ typedef struct attitude_t gimbal_imu_data; float yaw_motor_single_round_angle; float mini_yaw_encode_angle; + uint8_t big_yaw_online; } Gimbal_Upload_Data_s; typedef struct @@ -211,6 +234,8 @@ typedef struct uint8_t stalled_flag; //堵转标志 } Shoot_Upload_Data_s; + + #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) #endif // !ROBOT_DEF_H \ No newline at end of file diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 75750fc..c68b8a2 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -110,21 +110,25 @@ void stalled_detect() static float last_total_angle = 0; static uint8_t stalled_cnt = 0; - last_detect_time = detect_time; detect_time = DWT_GetTimeline_ms(); - if(detect_time - last_detect_time < 200) // 200ms 检测一次 + //last_detect_time + 200 > detect_time + if(detect_time - last_detect_time < 300) // 200ms 检测一次 return; if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE) { - float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f; - float real_angle = loader->measure.total_angle - last_total_angle; - if(real_angle < reference_angle * 0.2f) + if(abs(loader->measure.real_current)>=9500) { - //stalled_cnt ++; - shoot_feedback_data.stalled_flag = 1; + stalled_cnt++; + //shoot_feedback_data.stalled_flag = 1; } + else + stalled_cnt = 0; + + if(stalled_cnt >= 3) + shoot_feedback_data.stalled_flag = 1; + last_detect_time = DWT_GetTimeline_ms(); } } @@ -187,9 +191,9 @@ void ShootTask() // 也有可能需要从switch-case中独立出来 case LOAD_REVERSE: DJIMotorOuterLoop(loader, SPEED_LOOP); - DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转 + DJIMotorSetRef(loader, -8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 - dead_time = 500; // 翻转500ms + dead_time = 200; // 翻转500ms shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志 // ... break; @@ -217,8 +221,8 @@ void ShootTask() // DJIMotorSetRef(friction_r, 0); // break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 45000); //弹速27左右 不会超30 - DJIMotorSetRef(friction_r, 45000); + DJIMotorSetRef(friction_l, 42000); //弹速27左右 不会超30 + DJIMotorSetRef(friction_r, 42000); break; } } diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 6d6be58..90fc927 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -1,9 +1,4 @@ // -// Created by SJQ on 2024/1/26. -// - -#include "auto_aim.h" -// // Created by sph on 2024/1/21. // #include "auto_aim.h" @@ -15,7 +10,7 @@ * @param[in] trajectory_cal:弹道解算结构体 * @retval 返回空 */ -void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { +int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time; aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time; @@ -35,14 +30,22 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ 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; +// 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); + if (distance_temp < distance_min) { + distance_min = distance_temp; idx = 1; } + } else if (aim_sel->target_state.armor_num == 3)//前哨站 { for (i = 0; i < 3; i++) { @@ -60,15 +63,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); - for (i = 1; i < 3; i++) { - float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); - if (temp_yaw_diff < yaw_diff_min) { - yaw_diff_min = temp_yaw_diff; - idx = i; - } +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 3; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 +// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); +// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); +// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2); + +// int label_first = 0; +// int label_second = 1; +// if (distance_temp0 > distance_temp1) { +// label_first = 1; +// if (distance_temp0 > distance_temp2) { +// label_second = 2; +// } else label_second = 0; +// } else { +// label_first = 0; +// if (distance_temp1 > distance_temp2) { +// label_second = 2; +// } else label_second = 1; +// } + + // 选择两块较近的装甲板 + float distance[3]; + for (i = 0; i < 3; i++) { + distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); } + + int label_first = 0; + int label_second = 1; + + if(distance[label_first] > distance[label_second]) + { + label_first = 1; + label_second = 0; + } + + if(distance[2]target_state.x, 2) + powf(aim_sel->target_state.y, 2)); + + float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; + } else { for (i = 0; i < 4; i++) { float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; @@ -82,19 +140,68 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); - for (i = 1; i < 4; i++) { - float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); - if (temp_yaw_diff < yaw_diff_min) { - yaw_diff_min = temp_yaw_diff; - idx = i; - } +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 4; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + + // 选择两块较近的装甲板 + float distance[4]; + for (i = 0; i < 4; i++) { + distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2); } + + int label_first = 0; + int label_second = 1; + + if(distance[label_first] > distance[label_second]) + { + label_first = 1; + label_second = 0; + } + + if(distance[2]target_state.x, 2) + powf(aim_sel->target_state.y, 2)); + + float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; } aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time; aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time; aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time; + return idx; } /** @@ -128,7 +235,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2); arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis); - trajectory_cal->dis = trajectory_cal->dis - 0.20; + trajectory_cal->dis = trajectory_cal->dis - 0.05f; trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis); trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]); @@ -162,24 +269,24 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { trajectory_cal->cmd_pitch = trajectory_cal->theta_k; } -void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { - trajectory_cal->extra_delay_time = 0.025;//0.025 +int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { + trajectory_cal->extra_delay_time = 0.035;//0.025 aim_sel->target_state.armor_type = receive_packet->id; aim_sel->target_state.armor_num = receive_packet->armors_num; -// aim_sel->target_state.x = receive_packet->x; -// aim_sel->target_state.y = receive_packet->y; -// aim_sel->target_state.z = receive_packet->z; -// aim_sel->target_state.vx = receive_packet->vx; -// aim_sel->target_state.vy = receive_packet->vy; -// aim_sel->target_state.vz = receive_packet->vz; -// aim_sel->target_state.yaw = receive_packet->yaw; -// aim_sel->target_state.v_yaw = receive_packet->v_yaw; -// aim_sel->target_state.r1 = receive_packet->r1; -// aim_sel->target_state.r2 = receive_packet->r2; -// aim_sel->target_state.dz = receive_packet->dz; + aim_sel->target_state.x = receive_packet->x; + aim_sel->target_state.y = receive_packet->y; + aim_sel->target_state.z = receive_packet->z; + aim_sel->target_state.vx = receive_packet->vx; + aim_sel->target_state.vy = receive_packet->vy; + aim_sel->target_state.vz = receive_packet->vz; + aim_sel->target_state.yaw = receive_packet->yaw; + aim_sel->target_state.v_yaw = receive_packet->v_yaw; + aim_sel->target_state.r1 = receive_packet->r1; + aim_sel->target_state.r2 = receive_packet->r2; + aim_sel->target_state.dz = receive_packet->dz; - aim_armor_select(aim_sel, trajectory_cal); + int idx = aim_armor_select(aim_sel, trajectory_cal); trajectory_cal->position_xy[0] = aim_sel->aim_point[0]; trajectory_cal->position_xy[1] = aim_sel->aim_point[1]; @@ -189,5 +296,5 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec trajectory_cal->velocity[2] = aim_sel->target_state.vz; get_cmd_angle(trajectory_cal); - + return idx; } diff --git a/modules/auto_aim/auto_aim.h b/modules/auto_aim/auto_aim.h index d836d5a..ce151c1 100644 --- a/modules/auto_aim/auto_aim.h +++ b/modules/auto_aim/auto_aim.h @@ -1,14 +1,10 @@ -// -// Created by SJQ on 2024/1/26. -// - -#ifndef WHEEL_LEGGED_GIMBAL_AUTO_AIM_H -#define WHEEL_LEGGED_GIMBAL_AUTO_AIM_H - // // Created by sph on 2024/1/21. // +#ifndef BASIC_FRAMEWORK_AUTO_AIM_H +#define BASIC_FRAMEWORK_AUTO_AIM_H + #include "master_process.h" //弹道解算 typedef struct @@ -73,11 +69,9 @@ typedef struct }Aim_Select_Type_t; -void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal); +int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal); float get_fly_time(float x, float vx, float v_x0); void get_cmd_angle(Trajectory_Type_t *trajectory_cal); -void auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet); +int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet); - - -#endif //WHEEL_LEGGED_GIMBAL_AUTO_AIM_H +#endif //BASIC_FRAMEWORK_AUTO_AIM_H diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 40518d8..a4d505c 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -169,7 +169,7 @@ void INS_Task(void) INS.YawTotalAngle = QEKF_INS.YawTotalAngle; //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); - VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180); + VisionSetAltitude(INS.Yaw * PI / 180, INS.Roll * PI / 180); } // temperature control diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index b13730c..36c2efc 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -25,9 +25,11 @@ static DaemonInstance *vision_daemon_instance; // send_data.work_mode = work_mode; // send_data.bullet_speed = bullet_speed; //} -void VisionSetFlag(Enemy_Color_e enemy_color) +void VisionSetFlag(Enemy_Color_e enemy_color,uint8_t game_progress,uint16_t outpost_hp) { send_data.detect_color = enemy_color; +// send_data.game_progress = game_progress; +// send_data.outpost_hp = outpost_hp; send_data.reserved = 0; } @@ -181,9 +183,9 @@ void VisionSend() static uint8_t send_buffer[24]={0}; send_data.header = 0x5A; - VisionSetFlag(COLOR_BLUE); + //VisionSetFlag(COLOR_BLUE); //VisionSetAim(recv_data.x,recv_data.y,recv_data.z); - VisionSetAim(0,0,0); + //VisionSetAim(0,0,0); send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2); memcpy(send_buffer,&send_data,sizeof(send_data)); diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index dcb1641..fada1e7 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -83,6 +83,11 @@ typedef struct { float aim_x; float aim_y; float aim_z; + +// uint8_t game_progress; +// uint16_t outpost_hp; +// float target_x; +// float target_y; uint16_t checksum; } SendPacket_t; @@ -94,22 +99,22 @@ typedef struct { uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal uint8_t reserved: 1; -// float x; -// float y; -// float z; -// float yaw; -// float vx; -// float vy; -// float vz; -// float v_yaw; -// float r1; -// float r2; -// float dz; + float x; + float y; + float z; + float yaw; + float vx; + float vy; + float vz; + float v_yaw; + float r1; + float r2; + float dz; //导航数据 - float nav_vx; - float nav_vy; - float nav_wz; +// float nav_vx; +// float nav_vy; +// float nav_wz; uint16_t checksum; } RecievePacket_t; @@ -137,7 +142,7 @@ void VisionSend(); * @param bullet_speed */ //void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed); -void VisionSetFlag(Enemy_Color_e enemy_color); +void VisionSetFlag(Enemy_Color_e enemy_color,uint8_t game_progress,uint16_t outpost_hp); /** * @brief 设置发送数据的姿态部分 diff --git a/modules/motor/ECmotor/ECA8210.c b/modules/motor/ECmotor/ECA8210.c index 44f1ffe..11766b0 100644 --- a/modules/motor/ECmotor/ECA8210.c +++ b/modules/motor/ECmotor/ECA8210.c @@ -43,8 +43,22 @@ static void ECMotorDecode(CANInstance *_instance) DaemonReload(motor->daemon); // 喂狗 measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); - measure->last_angle = measure->angle_single_round; - measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; //总角度 单位rad +-12.5f + + //处理一下首次上电的角度值反馈 + if(measure->init_flag == 0) + { + measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; //总角度 单位rad +-12.5f + measure->last_angle = measure->angle_single_round; + measure->offset_angle = measure->angle_single_round; + measure->init_flag = 1; + } + else + { + measure->last_angle = measure->angle_single_round; + measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; //总角度 单位rad +-12.5f + } + + //measure->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f; measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; @@ -52,9 +66,9 @@ static void ECMotorDecode(CANInstance *_instance) measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads + SPEED_SMOOTH_COEF * (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; - if((measure->angle_single_round - measure->last_angle) >= 20.0f) + if((measure->angle_single_round - measure->last_angle) >= 6.0f) measure->total_round -- ; - else if ((measure->angle_single_round - measure->last_angle) <= -20.0f) + else if ((measure->angle_single_round - measure->last_angle) <= -6.0f) measure->total_round ++; measure->total_angle = measure->total_round * 25.0f + measure->angle_single_round; } @@ -164,21 +178,27 @@ void ECMotorControl() ECMotor_TorCMD.mode = 0b011; ECMotor_TorCMD.reserve = 0b001; ECMotor_TorCMD.ack_type = 0b01; - ECMotor_TorCMD.torque_target = set; + + //由于这个电机是响应式，只有收到命令帧才会发回反馈值 + if(motor->stop_flag == MOTOR_STOP) + ECMotor_TorCMD.torque_target = 0; + else if (motor->stop_flag == MOTOR_ENALBED) + ECMotor_TorCMD.torque_target = set; + CANSetDLC(motor->motor_can_ins,3); memcpy(motor->motor_can_ins->tx_buff,&ECMotor_TorCMD,sizeof(ECMotor_TorCMD)); swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+2); #endif - if (motor->stop_flag == MOTOR_STOP) - { // 若该电机处于停止状态,直接将发送buff置零 -#ifdef SPEED_CONTROL - memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD)); -#endif -#ifdef TORQUE_CONTROL - memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_TorCMD)); -#endif - } +// if (motor->stop_flag == MOTOR_STOP) +// { // 若该电机处于停止状态,直接将发送buff置零 +//#ifdef SPEED_CONTROL +// memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD)); +//#endif +//#ifdef TORQUE_CONTROL +// memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_TorCMD)); +//#endif +// } CANTransmit(motor->motor_can_ins,0.2); } diff --git a/modules/motor/ECmotor/ECA8210.h b/modules/motor/ECmotor/ECA8210.h index f22ac9b..11271d9 100644 --- a/modules/motor/ECmotor/ECA8210.h +++ b/modules/motor/ECmotor/ECA8210.h @@ -55,9 +55,13 @@ typedef struct typedef struct // EC-A8120 { + uint8_t init_flag; + uint16_t last_ecd; // 上一次读取的编码器值 uint16_t ecd; // 当前编码器值 float angle_single_round; // 单位rad +-12.5f + + float offset_angle; float last_angle; float speed_rads; // speed rad/s float real_current; // 实际电流 diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 807957b..bca6b5c 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -8,14 +8,15 @@ void MotorControlTask() { - // static uint8_t cnt = 0; 设定不同电机的任务频率 - // if(cnt%5==0) //200hz + static uint8_t cnt = 0; //设定不同电机的任务频率 + if(cnt%5==0) //200hz + ECMotorControl(); // if(cnt%10==0) //100hz DJIMotorControl(); /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ - LKMotorControl(); - ECMotorControl(); + //LKMotorControl(); + // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 @@ -25,4 +26,7 @@ void MotorControlTask() ServeoMotorControl(); // StepMotorControl(); + cnt++; + if(cnt>=127) + cnt = 0; } diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 22416b3..840150a 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -107,12 +107,13 @@ typedef enum /****************************接收数据的详细说明****************************/ /****************************接收数据的详细说明****************************/ -/* ID: 0x0001 Byte: 3 比赛状态数据 */ +/* ID: 0x0001 Byte: 3 V1.6.1 比赛状态数据 */ typedef struct { uint8_t game_type : 4; - uint8_t game_progress : 4; - uint16_t stage_remain_time; + uint8_t game_progress : 4; + uint16_t stage_remain_time; + uint64_t SyncTimeStamp; } ext_game_state_t; /* ID: 0x0002 Byte: 1 比赛结果数据 */ @@ -157,37 +158,32 @@ typedef struct uint8_t supply_projectile_num; } ext_supply_projectile_action_t; -/* ID: 0X0201 Byte: 27 机器人状态数据 */ +/* ID: 0X0201 Byte: 27 V1.6.1 机器人性能体系数据 */ typedef struct { - uint8_t robot_id; - uint8_t robot_level; - uint16_t remain_HP; - uint16_t max_HP; - uint16_t shooter_id1_17mm_cooling_rate; - uint16_t shooter_id1_17mm_cooling_limit; - uint16_t shooter_id1_17mm_speed_limit; - uint16_t shooter_id2_17mm_cooling_rate; - uint16_t shooter_id2_17mm_cooling_limit; - uint16_t shooter_id2_17mm_speed_limit; - uint16_t shooter_id1_42mm_cooling_rate; - uint16_t shooter_id1_42mm_cooling_limit; - uint16_t shooter_id1_42mm_speed_limit; - uint16_t chassis_power_limit; - uint8_t mains_power_gimbal_output : 1; - uint8_t mains_power_chassis_output : 1; - uint8_t mains_power_shooter_output : 1; + uint8_t robot_id; + uint8_t robot_level; + uint16_t current_HP; + uint16_t maximum_HP; + uint16_t shooter_barrel_cooling_value; + uint16_t shooter_barrel_heat_limit; + uint16_t chassis_power_limit; + + uint8_t power_management_gimbal_output : 1; + uint8_t power_management_chassis_output : 1; + uint8_t power_management_shooter_output : 1; } ext_game_robot_state_t; -/* ID: 0X0202 Byte: 14 实时功率热量数据 */ +/* ID: 0X0202 Byte: 14 V1.6.1 实时功率热量数据 */ typedef struct { - uint16_t chassis_volt; - uint16_t chassis_current; - float chassis_power; // 瞬时功率 - uint16_t chassis_power_buffer; // 60焦耳缓冲能量 - uint16_t shooter_heat0; // 17mm - uint16_t shooter_heat1; + uint16_t chassis_voltage; + uint16_t chassis_current; + float chassis_power; + uint16_t buffer_energy; + uint16_t shooter_17mm_1_barrel_heat; + uint16_t shooter_17mm_2_barrel_heat; + uint16_t shooter_42mm_barrel_heat; } ext_power_heat_data_t; /* ID: 0x0203 Byte: 16 机器人位置数据 */ diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c index 34850a8..74a723e 100644 --- a/modules/vofa/vofa.c +++ b/modules/vofa/vofa.c @@ -27,7 +27,7 @@ void vofa_justfloat_output(float *data, uint8_t num, UART_HandleTypeDef *huart) send_data[4 * num + 2] = 0x80; send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 - HAL_UART_Transmit(huart, (uint8_t *) send_data, 4 * num + 4, 100); + //HAL_UART_Transmit(huart, (uint8_t *) send_data, 4 * num + 4, 100); //CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4); } diff --git a/sentry_left.jdebug b/sentry_left.jdebug index 2ef1348..32d0361 100644 --- a/sentry_left.jdebug +++ b/sentry_left.jdebug @@ -33,7 +33,6 @@ void OnProjectLoad (void) { // // User settings // - Edit.SysVar (VAR_HSS_SPEED, FREQ_100_HZ); File.Open ("$(ProjectDir)/cmake-build-debug/sentry_left.elf"); Project.SetOSPlugin ("FreeRTOSPlugin_CM4"); } diff --git a/sentry_left.jdebug.user b/sentry_left.jdebug.user index 348afe9..8815b99 100644 --- a/sentry_left.jdebug.user +++ b/sentry_left.jdebug.user @@ -1,30 +1,36 @@ -Breakpoint=C:/Users/sph/Desktop/sentry_left/modules/motor/DJImotor/dji_motor.c:316, State=BP_STATE_DISABLED -Breakpoint=C:/Users/sph/Desktop/sentry_left/modules/motor/DJImotor/dji_motor.c:328, State=BP_STATE_DISABLED -Breakpoint=C:/Users/sph/Desktop/sentry_left/modules/motor/DJImotor/dji_motor.c:362:9, State=BP_STATE_DISABLED -OpenDocument="master_process.c", FilePath="C:/Users/sph/Desktop/sentry_left/modules/master_machine/master_process.c", Line=126 -OpenDocument="main.c", FilePath="C:/Users/sph/Desktop/sentry_left/Src/main.c", Line=56 + +Breakpoint=D:/CLion/Project/sentry_left/modules/motor/DJImotor/dji_motor.c:316, State=BP_STATE_DISABLED +Breakpoint=D:/CLion/Project/sentry_left/modules/motor/DJImotor/dji_motor.c:328, State=BP_STATE_DISABLED +Breakpoint=D:/CLion/Project/sentry_left/modules/motor/DJImotor/dji_motor.c:362:9, State=BP_STATE_DISABLED +GraphedExpression="sentry_state", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f +OpenDocument="main.c", FilePath="D:/CLion/Project/sentry_left/Src/main.c", Line=69 +OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/sentry_left/bsp/dwt/bsp_dwt.c", Line=81 +OpenDocument="tasks.c", FilePath="D:/CLion/Project/sentry_left/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3399 +OpenDocument="usbd_def.h", FilePath="D:/CLion/Project/sentry_left/Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h", Line=0 +OpenDocument="referee_task.c", FilePath="D:/CLion/Project/sentry_left/modules/referee/referee_task.c", Line=0 +OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/sentry_left/application/cmd/robot_cmd.c", Line=292 +OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/sentry_left/Startup/startup_stm32f407ighx.s", Line=52 +OpenDocument="shoot.c", FilePath="D:/CLion/Project/sentry_left/application/shoot/shoot.c", Line=0 +OpenDocument="chassis.c", FilePath="D:/CLion/Project/sentry_left/application/chassis/chassis.c", Line=298 +OpenDocument="usbd_ctlreq.h", FilePath="D:/CLion/Project/sentry_left/Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h", Line=0 OpenToolbar="Debug", Floating=0, x=0, y=0 -OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=591, h=632, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 -OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=720, h=487, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=591, h=322, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Memory 1", DockArea=BOTTOM, x=2, y=0, w=545, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20009694 -OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=720, h=486, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 -OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1292, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 -OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=591, h=632, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="8;47", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="831;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="847;0" -OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=721, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Call Stack", DockArea=RIGHT, x=0, y=0, w=300, h=253, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=300, h=368, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Memory 1", DockArea=BOTTOM, x=2, y=0, w=370, h=302, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20009694 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=0, w=510, h=622, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1031, h=302, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;98", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="833;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="847;0" +OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=517, h=302, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" -TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;296] -TableHeader="Source Files", SortCol="File", SortOrder="DESCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[233;100;100;100;878] -TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] -TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[380;100;100;100;100;100;110;126;176] +TableHeader="Call Stack", SortCol="Function", SortOrder="ASCENDING", VisibleCols=["Function";"Stack Frame";"Source";"PC";"Return Address";"Stack Used"], ColWidths=[100;100;100;100;112;100] TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110] -TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[237;238;100;119] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;259] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[166;134;100;110] TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] -TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[26;26;26;26] +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351] WatchedExpression="INS", DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="pitch_motor", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="gimbal_cmd_recv", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 @@ -34,8 +40,16 @@ WatchedExpression="motor_can_ins", DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watc WatchedExpression="big_yaw_motor", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="big_yaw_angle", RefreshRate=2, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="friction_l", RefreshRate=2, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 -WatchedExpression="gimbal_feedback_data", RefreshRate=2, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 +WatchedExpression="gimbal_feedback_data", RefreshRate=1, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="chassis_cmd_recv", RefreshRate=2, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="big_yaw_relative_angle", RefreshRate=2, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 WatchedExpression="pid_ref", DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 -WatchedExpression="send_data", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 \ No newline at end of file +WatchedExpression="send_data", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 +WatchedExpression="vision_recv_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="chassis_fetch_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="sentry_state", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="referee_recv_info", Window=Watched Data 1 +WatchedExpression="loader", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="gimbal_cmd_send", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="recover_start_T", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="vision_recv_data", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file