Hshine
/
sentry_left
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

联盟赛版本 修复卡弹检测...其他改了啥我也不记得了

This commit is contained in:
宋家齐 2024-04-16 16:52:10 +08:00
parent a32ac620ea
commit 1b4e26c50d
17 changed files with 665 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
application/chassis/chassis.c
View File

@ -43,8 +43,7 @@ static Subscriber_t *chassis_sub; // 用于订阅底盘的控
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令 static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据 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 SuperCapInstance *cap; // 超级电容
static PowerMeterInstance *power_meter; //功率计 static PowerMeterInstance *power_meter; //功率计
@ -110,8 +109,6 @@ void ChassisInit()
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
motor_rb = DJIMotorInit(&chassis_motor_config); motor_rb = DJIMotorInit(&chassis_motor_config);
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
SuperCap_Init_Config_s cap_conf = { SuperCap_Init_Config_s cap_conf = {
.can_config = { .can_config = {
.can_handle = &hcan2, .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); //chassis_cmd_recv.wz = 100.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
break; break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出 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; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = -5; chassis_cmd_recv.wz = -1.2f*PI;
break; break;
default: default:
break; break;
@ -318,10 +315,13 @@ void ChassisTask()
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
// chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0; // chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
// // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // //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.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 #ifdef ONE_BOARD

435
application/cmd/robot_cmd.c
View File

@ -9,14 +9,19 @@
#include "general_def.h" #include "general_def.h"
#include "dji_motor.h" #include "dji_motor.h"
#include "auto_aim.h" #include "auto_aim.h"
#include "referee_task.h"
// bsp // bsp
#include "bsp_dwt.h" #include "bsp_dwt.h"
#include "bsp_log.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 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应用包含的模块实例指针和交互信息存储*/ /* cmd应用包含的模块实例指针和交互信息存储*/
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
#include "can_comm.h" #include "can_comm.h"
@ -27,6 +32,10 @@ static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
#endif // ONE_BOARD #endif // ONE_BOARD
#include "can_comm.h"
static CANCommInstance *cmd_can_comm; //哨兵左右云台 双板通信
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 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; // 机器人整体工作状态 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协议串口需选用添加了反相器的那个 rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
vision_recv_data = VisionInit(&huart1); // 视觉通信串口 vision_recv_data = VisionInit(&huart1); // 视觉通信串口
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
@ -78,7 +91,20 @@ void RobotCMDInit()
}; };
cmd_can_comm = CANCommInit(&comm_conf); cmd_can_comm = CANCommInit(&comm_conf);
#endif // GIMBAL_BOARD #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; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
} }
@ -88,12 +114,17 @@ void RobotCMDInit()
* 单圈绝对角度的范围是0~360,说明文档中有图示 * 单圈绝对角度的范围是0~360,说明文档中有图示
* *
*/ */
static void CalcOffsetAngle() static void CalcOffsetAngle() {
{
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
//如果大yaw轴电机不在线 不计算底盘跟随
if (gimbal_fetch_data.big_yaw_online == 0) {
chassis_cmd_send.offset_angle = 0;
return;
}
static float angle; static float angle;
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 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) if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
else if (angle > 180.0f + YAW_ALIGN_ANGLE) else if (angle > 180.0f + YAW_ALIGN_ANGLE)
@ -114,20 +145,19 @@ static void CalcOffsetAngle()
* @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
* *
*/ */
static void RemoteControlSet() static void RemoteControlSet() {
{
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? // 控制底盘和云台运行模式,云台待添加,云台是否始终使用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; chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; 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; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; 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->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
@ -137,7 +167,8 @@ static void RemoteControlSet()
// 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制 // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
// ... // ...
gimbal_cmd_send.yaw -= 0.0005f * (float) rc_data[TEMP].rc.rocker_l_; 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_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 (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
@ -145,38 +176,41 @@ static void RemoteControlSet()
} }
//左侧开关状态为[下],视觉模式 //左侧开关状态为[下],视觉模式
// if (switch_is_down(rc_data[TEMP].rc.switch_left)) { if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
// trajectory_cal.v0 = 30; //弹速30 gimbal_cmd_send.control_mode = TEST_CONTROL;
// if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 trajectory_cal.v0 = 28; //弹速30
// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) { if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
// aim_select.suggest_fire = 0; && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
// //未发现目标 aim_select.suggest_fire = 0;
// no_find_cnt++; //未发现目标
// no_find_cnt++;
// if (no_find_cnt >= 2000) {
// //gimbal_scan_flag = 1; if (no_find_cnt >= 200) {
// //auto_aim_flag = 0; //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 //else
// aim_select.suggest_fire = 0; //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;
}
}
// 云台软件限位 // 云台软件限位
@ -184,31 +218,40 @@ static void RemoteControlSet()
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 0.001f * (float) rc_data[TEMP].rc.rocker_r1; // _水平方向 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.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) { // 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.vx = vision_recv_data->nav_vx;
chassis_cmd_send.vy = vision_recv_data->nav_vy; // chassis_cmd_send.vy = vision_recv_data->nav_vy;
chassis_cmd_send.wz = vision_recv_data->nav_wz; // chassis_cmd_send.wz = vision_recv_data->nav_wz;
} //
// }
// 发射参数 // 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
; // 弹舱舵机控制,待添加servo_motor模块,开启 ; // 弹舱舵机控制,待添加servo_motor模块,开启
else else; // 弹舱舵机控制,待添加servo_motor模块,关闭
; // 弹舱舵机控制,待添加servo_motor模块,关闭
// 摩擦轮控制,拨轮向上打为负,向下为正 // 摩擦轮控制,拨轮向上打为负,向下为正
if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮 if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
shoot_cmd_send.friction_mode = FRICTION_ON; shoot_cmd_send.friction_mode = FRICTION_ON;
//gimbal_cmd_send.friction_mode = FRICTION_ON;
else else
shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF;
//gimbal_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial < -500) if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.load_mode = LOAD_BURSTFIRE; shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
//gimbal_cmd_send.load_mode = LOAD_BURSTFIRE;
else else
shoot_cmd_send.load_mode = LOAD_STOP; 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发,后续可以根据左侧拨轮的值大小切换射频, // 射频控制,固定每秒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; shoot_cmd_send.load_mode = LOAD_REVERSE;
@ -218,130 +261,88 @@ static void RemoteControlSet()
* @brief 自动模式时模式和控制量设置 * @brief 自动模式时模式和控制量设置
* *
*/ */
static void AutoControlSet()
{ static uint8_t sentry_state;
// static int8_t gimbal_scan_flag = 1;
// static int8_t scan_dir = 1; 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 //trajectory_cal.v0 = 30; //弹速30
//
// //小云台扫描 //小云台扫描
// if(gimbal_scan_flag == 1) if (gimbal_scan_flag == 1) {
// {
// if (scan_dir == 1) gimbal_cmd_send.yaw += 0.001f; gimbal_cmd_send.pitch = PITCH_SCAN;
// 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(sentry_state > 1)
// if(gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 0.1f) scan_dir = -1; {
// } if (yaw_dir == 1) gimbal_cmd_send.yaw += 0.06f;
// else gimbal_cmd_send.yaw -= 0.06f;
// if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 if (gimbal_fetch_data.mini_yaw_encode_angle <= YAW_MIN_ENCODE_ANGLE + 2.0f) yaw_dir = 1;
// && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) { if (gimbal_fetch_data.mini_yaw_encode_angle >= YAW_MAX_ENCODE_ANGLE - 2.0f) yaw_dir = -1;
// aim_select.suggest_fire = 0; }
// //未发现目标
// no_find_cnt++; shoot_cmd_send.load_mode = LOAD_STOP;
// if (no_find_cnt >= 2000) { }
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; //gimbal_scan_flag = 1;
//auto_aim_flag = 0; //auto_aim_flag = 0;
// } }
//else //else
//auto_aim_flag = 1; //auto_aim_flag = 1;
// } else { } else {
// //弹道解算 //弹道解算
// no_find_cnt = 0; no_find_cnt = 0;
// auto_aim_flag = 1; 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;
// }
}
/** auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
* @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;
gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测 VisionSetAim(aim_select.aim_point[0], aim_select.aim_point[1], aim_select.aim_point[2]);
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速 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度
{ {
case 0: aim_select.suggest_fire = 1;
shoot_cmd_send.bullet_speed = 15; shoot_cmd_send.shoot_mode = SHOOT_ON;
break; shoot_cmd_send.shoot_rate = SHOOT_RATE;
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; shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
break; } else {
aim_select.suggest_fire = 0;
//shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
} }
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:
shoot_cmd_send.friction_mode = FRICTION_ON; shoot_cmd_send.friction_mode = FRICTION_ON;
break;
} }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度 // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
{ //shoot_cmd_send.shoot_rate = 8;
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:
break; shoot_cmd_send.friction_mode = FRICTION_ON;
default: if (shoot_fetch_data.stalled_flag == 1)
shoot_cmd_send.load_mode = LOAD_REVERSE;
break;
}
} }
/** /**
@ -351,8 +352,7 @@ static void MouseKeySet()
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现 * @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
* *
*/ */
static void EmergencyHandler() static void EmergencyHandler() {
{
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正 // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断 if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
{ {
@ -362,20 +362,22 @@ static void EmergencyHandler()
shoot_cmd_send.shoot_mode = SHOOT_OFF; shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF; shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP; shoot_cmd_send.load_mode = LOAD_STOP;
gimbal_cmd_send.control_mode = ZERO_FORCE;
LOGERROR("[CMD] emergency stop!"); 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; robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON; shoot_cmd_send.shoot_mode = SHOOT_ON;
LOGINFO("[CMD] reinstate, robot ready"); LOGINFO("[CMD] reinstate, robot ready");
} }
} }
uint8_t sentry_behave();
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask() void RobotCMDTask() {
{
// 从其他应用获取回传数据 // 从其他应用获取回传数据
#ifdef ONE_BOARD #ifdef ONE_BOARD
SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data); SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data);
@ -389,20 +391,59 @@ void RobotCMDTask()
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle(); CalcOffsetAngle();
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
// if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
// RemoteControlSet(); static control_mode_e last_gimbal_mode = 0;
// else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
// MouseKeySet();
if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式 if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式
{
gimbal_cmd_send.control_mode = AUTO_CONTROL;
AutoControlSet(); 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(); 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(); // 处理模块离线和遥控器急停等紧急情况 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) // 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中完成设置 // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
#ifdef ONE_BOARD #ifdef ONE_BOARD
@ -411,7 +452,55 @@ void RobotCMDTask()
#ifdef GIMBAL_BOARD #ifdef GIMBAL_BOARD
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send); CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
CANCommSend(cmd_can_comm, (void *) &gimbal_cmd_send); //哨兵左右云台双板通信
PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send); PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
VisionSend(&vision_send_data); 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;
}

58
application/gimbal/gimbal.c
View File

@ -30,6 +30,9 @@ static float big_yaw_angle = 0.0f; //大yaw轴绝对角度 通过小云台陀
static float big_yaw_speed = 0.0f;//大yaw轴绝对角速度 static float big_yaw_speed = 0.0f;//大yaw轴绝对角速度
sin_input_generate_t sinInputGenerate; sin_input_generate_t sinInputGenerate;
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
void GimbalInit() void GimbalInit()
{ {
@ -43,7 +46,7 @@ void GimbalInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 1.0f, // 8 .Kp = 0.5f, // 8
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.DeadBand = 0.1, .DeadBand = 0.1,
@ -82,7 +85,7 @@ void GimbalInit()
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 2.0f,//4.0f,//2.0f .Kp = 1.0f,//4.0f,//2.0f
.Ki = 0.0f,//1,//0 .Ki = 0.0f,//1,//0
.Kd = 0.0f,//0.0f .Kd = 0.0f,//0.0f
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
@ -90,12 +93,12 @@ void GimbalInit()
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = 800,//6000,//800 .Kp = 2500,//6000,//800
.Ki = 100,//500,//100 .Ki = 500,//500,//100
.Kd = 0,//0 .Kd = 0,//0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 5000, .IntegralLimit = 10000,
.MaxOut = 10000, .MaxOut = 30000,
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->Roll,//&gimba_IMU_data->Pitch, .other_angle_feedback_ptr = &gimba_IMU_data->Roll,//&gimba_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
@ -130,7 +133,7 @@ void GimbalInit()
.DeadBand = 0, .DeadBand = 0,
}, },
.speed_PID = { .speed_PID = {
.Kp = 300,//2500, // 50 .Kp = 150,//150,//这里KP尽可能小 防止震荡脱齿
.Ki = 0,//200, // 350 .Ki = 0,//200, // 350
.Kd = 0, // 0 .Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .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_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_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); //sin_input_frequency_init(&sinInputGenerate);
} }
@ -169,6 +174,10 @@ void GimbalTask()
// 获取云台控制数据 // 获取云台控制数据
// 后续增加未收到数据的处理 // 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv); SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
SubGetMessage(chassis_sub, &chassis_cmd_recv);
//gimbal_cmd_recv.big_yaw = 0;
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
@ -185,9 +194,6 @@ void GimbalTask()
DJIMotorEnable(yaw_motor); DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor); DJIMotorEnable(pitch_motor);
ECMotorEnable(big_yaw_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, ANGLE_LOOP, OTHER_FEED);
DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
@ -195,7 +201,9 @@ void GimbalTask()
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); 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; break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@ -208,7 +216,14 @@ void GimbalTask()
DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); 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; break;
default: default:
break; break;
@ -223,8 +238,20 @@ void GimbalTask()
//ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0); //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; 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); 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_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轴角速度 //yaw轴角速度
//重力补偿力矩 //重力补偿力矩
@ -246,10 +273,11 @@ void GimbalTask()
static float big_yaw_relative_angle; 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.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); PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);

33
application/robot_def.h
View File

@ -27,13 +27,13 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数 // 云台参数
#define YAW_CHASSIS_ALIGN_ECD 8012 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #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_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 39 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MAX_ANGLE 39 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define YAW_MAX_ENCODE_ANGLE 200.0f //小yaw编码器限位 #define YAW_MAX_ENCODE_ANGLE 139.0f //小yaw编码器限位
#define YAW_MIN_ENCODE_ANGLE 9.14f #define YAW_MIN_ENCODE_ANGLE 10.0f
// 发射参数 // 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
@ -99,6 +99,15 @@ typedef enum
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
} gimbal_mode_e; } gimbal_mode_e;
// 控制模式设置
typedef enum
{
RC_CONTROL = 0, // 遥控模式
AUTO_CONTROL, // 自动模式
TEST_CONTROL, // 测试模式
ZERO_FORCE, // 失能模式
} control_mode_e;
// 发射模式设置 // 发射模式设置
typedef enum typedef enum
{ {
@ -155,13 +164,23 @@ typedef struct
// cmd发布的云台控制数据,由gimbal订阅 // cmd发布的云台控制数据,由gimbal订阅
typedef struct typedef struct
{ // 云台角度控制 { // 云台角度控制
float big_yaw;
float yaw; float yaw;
float pitch; float pitch;
float chassis_rotate_wz; float chassis_rotate_wz;
control_mode_e control_mode;
gimbal_mode_e gimbal_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; } Gimbal_Ctrl_Cmd_s;
// cmd发布的发射控制数据,由shoot订阅 // cmd发布的发射控制数据,由shoot订阅
typedef struct typedef struct
{ {
@ -194,6 +213,9 @@ typedef struct
Bullet_Speed_e bullet_speed; // 弹速限制 Bullet_Speed_e bullet_speed; // 弹速限制
Enemy_Color_e enemy_color; // 0 for blue, 1 for red Enemy_Color_e enemy_color; // 0 for blue, 1 for red
uint8_t game_progress; //比赛阶段
uint16_t remain_HP; //当前血量
} Chassis_Upload_Data_s; } Chassis_Upload_Data_s;
@ -202,6 +224,7 @@ typedef struct
attitude_t gimbal_imu_data; attitude_t gimbal_imu_data;
float yaw_motor_single_round_angle; float yaw_motor_single_round_angle;
float mini_yaw_encode_angle; float mini_yaw_encode_angle;
uint8_t big_yaw_online;
} Gimbal_Upload_Data_s; } Gimbal_Upload_Data_s;
typedef struct typedef struct
@ -211,6 +234,8 @@ typedef struct
uint8_t stalled_flag; //堵转标志 uint8_t stalled_flag; //堵转标志
} Shoot_Upload_Data_s; } Shoot_Upload_Data_s;
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
#endif // !ROBOT_DEF_H #endif // !ROBOT_DEF_H

26
application/shoot/shoot.c
View File

@ -110,21 +110,25 @@ void stalled_detect()
static float last_total_angle = 0; static float last_total_angle = 0;
static uint8_t stalled_cnt = 0; static uint8_t stalled_cnt = 0;
last_detect_time = detect_time;
detect_time = DWT_GetTimeline_ms(); 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; return;
if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE) if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
{ {
float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f; if(abs(loader->measure.real_current)>=9500)
float real_angle = loader->measure.total_angle - last_total_angle;
if(real_angle < reference_angle * 0.2f)
{ {
//stalled_cnt ++; stalled_cnt++;
shoot_feedback_data.stalled_flag = 1; //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中独立出来 // 也有可能需要从switch-case中独立出来
case LOAD_REVERSE: case LOAD_REVERSE:
DJIMotorOuterLoop(loader, SPEED_LOOP); 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(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 500; // 翻转500ms dead_time = 200; // 翻转500ms
shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志 shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
// ... // ...
break; break;
@ -217,8 +221,8 @@ void ShootTask()
// DJIMotorSetRef(friction_r, 0); // DJIMotorSetRef(friction_r, 0);
// break; // break;
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 45000); //弹速27左右 不会超30 DJIMotorSetRef(friction_l, 42000); //弹速27左右 不会超30
DJIMotorSetRef(friction_r, 45000); DJIMotorSetRef(friction_r, 42000);
break; break;
} }
} }

191
modules/auto_aim/auto_aim.c
View File

@ -1,9 +1,4 @@
// //
// Created by SJQ on 2024/1/26.
//
#include "auto_aim.h"
//
// Created by sph on 2024/1/21. // Created by sph on 2024/1/21.
// //
#include "auto_aim.h" #include "auto_aim.h"
@ -15,7 +10,7 @@
* @param[in] trajectory_cal:弹道解算结构体 * @param[in] trajectory_cal:弹道解算结构体
* @retval 返回空 * @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->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; 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; 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 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); // float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
if (temp_yaw_diff < yaw_diff_min) { // if (temp_yaw_diff < yaw_diff_min) {
yaw_diff_min = temp_yaw_diff; // 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; idx = 1;
} }
} else if (aim_sel->target_state.armor_num == 3)//前哨站 } else if (aim_sel->target_state.armor_num == 3)//前哨站
{ {
for (i = 0; i < 3; i++) { 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; use_1 = !use_1;
} }
//计算枪管到目标装甲板yaw最小的那个装甲板 // //计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); // float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
for (i = 1; i < 3; i++) { // for (i = 1; i < 3; i++) {
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); // float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
if (temp_yaw_diff < yaw_diff_min) { // if (temp_yaw_diff < yaw_diff_min) {
yaw_diff_min = temp_yaw_diff; // yaw_diff_min = temp_yaw_diff;
idx = i; // 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]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->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 { } else {
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; 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; use_1 = !use_1;
} }
//计算枪管到目标装甲板yaw最小的那个装甲板 // //计算枪管到目标装甲板yaw最小的那个装甲板
float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); // float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
for (i = 1; i < 4; i++) { // for (i = 1; i < 4; i++) {
float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); // float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
if (temp_yaw_diff < yaw_diff_min) { // if (temp_yaw_diff < yaw_diff_min) {
yaw_diff_min = temp_yaw_diff; // yaw_diff_min = temp_yaw_diff;
idx = i; // 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]<distance[label_first]){
label_second = label_first;
label_first = 2;
}
else if(distance[2]<distance[label_second])
label_second = 2;
if(distance[3]<distance[label_first]){
label_second = label_first;
label_first = 3;
}
else if(distance[3]<distance[label_second])
label_second = 3;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->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[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[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; 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_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2);
arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis); 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->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis);
trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]); 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; 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) { int 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 trajectory_cal->extra_delay_time = 0.035;//0.025
aim_sel->target_state.armor_type = receive_packet->id; aim_sel->target_state.armor_type = receive_packet->id;
aim_sel->target_state.armor_num = receive_packet->armors_num; aim_sel->target_state.armor_num = receive_packet->armors_num;
// aim_sel->target_state.x = receive_packet->x; aim_sel->target_state.x = receive_packet->x;
// aim_sel->target_state.y = receive_packet->y; aim_sel->target_state.y = receive_packet->y;
// aim_sel->target_state.z = receive_packet->z; aim_sel->target_state.z = receive_packet->z;
// aim_sel->target_state.vx = receive_packet->vx; aim_sel->target_state.vx = receive_packet->vx;
// aim_sel->target_state.vy = receive_packet->vy; aim_sel->target_state.vy = receive_packet->vy;
// aim_sel->target_state.vz = receive_packet->vz; aim_sel->target_state.vz = receive_packet->vz;
// aim_sel->target_state.yaw = receive_packet->yaw; aim_sel->target_state.yaw = receive_packet->yaw;
// aim_sel->target_state.v_yaw = receive_packet->v_yaw; aim_sel->target_state.v_yaw = receive_packet->v_yaw;
// aim_sel->target_state.r1 = receive_packet->r1; aim_sel->target_state.r1 = receive_packet->r1;
// aim_sel->target_state.r2 = receive_packet->r2; aim_sel->target_state.r2 = receive_packet->r2;
// aim_sel->target_state.dz = receive_packet->dz; 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[0] = aim_sel->aim_point[0];
trajectory_cal->position_xy[1] = aim_sel->aim_point[1]; 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; trajectory_cal->velocity[2] = aim_sel->target_state.vz;
get_cmd_angle(trajectory_cal); get_cmd_angle(trajectory_cal);
return idx;
} }

18
modules/auto_aim/auto_aim.h
View File

@ -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. // Created by sph on 2024/1/21.
// //
#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
#define BASIC_FRAMEWORK_AUTO_AIM_H
#include "master_process.h" #include "master_process.h"
//弹道解算 //弹道解算
typedef struct typedef struct
@ -73,11 +69,9 @@ typedef struct
}Aim_Select_Type_t; }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); float get_fly_time(float x, float vx, float v_x0);
void get_cmd_angle(Trajectory_Type_t *trajectory_cal); 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 //BASIC_FRAMEWORK_AUTO_AIM_H
#endif //WHEEL_LEGGED_GIMBAL_AUTO_AIM_H

2
modules/imu/ins_task.c
View File

@ -169,7 +169,7 @@ void INS_Task(void)
INS.YawTotalAngle = QEKF_INS.YawTotalAngle; INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
//VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll); //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 // temperature control

8
modules/master_machine/master_process.c
View File

@ -25,9 +25,11 @@ static DaemonInstance *vision_daemon_instance;
// send_data.work_mode = work_mode; // send_data.work_mode = work_mode;
// send_data.bullet_speed = bullet_speed; // 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.detect_color = enemy_color;
// send_data.game_progress = game_progress;
// send_data.outpost_hp = outpost_hp;
send_data.reserved = 0; send_data.reserved = 0;
} }
@ -181,9 +183,9 @@ void VisionSend()
static uint8_t send_buffer[24]={0}; static uint8_t send_buffer[24]={0};
send_data.header = 0x5A; send_data.header = 0x5A;
VisionSetFlag(COLOR_BLUE); //VisionSetFlag(COLOR_BLUE);
//VisionSetAim(recv_data.x,recv_data.y,recv_data.z); //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); send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
memcpy(send_buffer,&send_data,sizeof(send_data)); memcpy(send_buffer,&send_data,sizeof(send_data));

35
modules/master_machine/master_process.h
View File

@ -83,6 +83,11 @@ typedef struct {
float aim_x; float aim_x;
float aim_y; float aim_y;
float aim_z; float aim_z;
// uint8_t game_progress;
// uint16_t outpost_hp;
// float target_x;
// float target_y;
uint16_t checksum; uint16_t checksum;
} SendPacket_t; } SendPacket_t;
@ -94,22 +99,22 @@ typedef struct {
uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal uint8_t armors_num: 3; // 2-balance 3-outpost 4-normal
uint8_t reserved: 1; uint8_t reserved: 1;
// float x; float x;
// float y; float y;
// float z; float z;
// float yaw; float yaw;
// float vx; float vx;
// float vy; float vy;
// float vz; float vz;
// float v_yaw; float v_yaw;
// float r1; float r1;
// float r2; float r2;
// float dz; float dz;
//导航数据 //导航数据
float nav_vx; // float nav_vx;
float nav_vy; // float nav_vy;
float nav_wz; // float nav_wz;
uint16_t checksum; uint16_t checksum;
} RecievePacket_t; } RecievePacket_t;
@ -137,7 +142,7 @@ void VisionSend();
* @param bullet_speed * @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, 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 设置发送数据的姿态部分 * @brief 设置发送数据的姿态部分

42
modules/motor/ECmotor/ECA8210.c
View File

@ -43,8 +43,22 @@ static void ECMotorDecode(CANInstance *_instance)
DaemonReload(motor->daemon); // 喂狗 DaemonReload(motor->daemon); // 喂狗
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt); measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
//处理一下首次上电的角度值反馈
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->last_angle = measure->angle_single_round;
measure->angle_single_round = (float)(ECMotor_Report.angle_raw-32768) * 0.0003814697f; //总角度 单位rad +-12.5f 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->speed_rads = (float)(ECMotor_Report.speed_raw-2048) * 0.008789f;
measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f; measure->real_current = (float)(ECMotor_Report.current_raw-2048) * 0.014648f;
measure->temperature = (ECMotor_Report.temperature_raw - 50) /2; 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 + measure->speed_rads = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_rads +
SPEED_SMOOTH_COEF * (float)(ECMotor_Report.speed_raw-2048) * 0.008789f; 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 -- ; 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_round ++;
measure->total_angle = measure->total_round * 25.0f + measure->angle_single_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.mode = 0b011;
ECMotor_TorCMD.reserve = 0b001; ECMotor_TorCMD.reserve = 0b001;
ECMotor_TorCMD.ack_type = 0b01; ECMotor_TorCMD.ack_type = 0b01;
//由于这个电机是响应式，只有收到命令帧才会发回反馈值
if(motor->stop_flag == MOTOR_STOP)
ECMotor_TorCMD.torque_target = 0;
else if (motor->stop_flag == MOTOR_ENALBED)
ECMotor_TorCMD.torque_target = set; ECMotor_TorCMD.torque_target = set;
CANSetDLC(motor->motor_can_ins,3); CANSetDLC(motor->motor_can_ins,3);
memcpy(motor->motor_can_ins->tx_buff,&ECMotor_TorCMD,sizeof(ECMotor_TorCMD)); 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); swap(motor->motor_can_ins->tx_buff+1,motor->motor_can_ins->tx_buff+2);
#endif #endif
if (motor->stop_flag == MOTOR_STOP) // if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零 // { // 若该电机处于停止状态,直接将发送buff置零
#ifdef SPEED_CONTROL //#ifdef SPEED_CONTROL
memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD)); // memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_SpdCMD));
#endif //#endif
#ifdef TORQUE_CONTROL //#ifdef TORQUE_CONTROL
memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_TorCMD)); // memcpy(motor->motor_can_ins->tx_buff,0,sizeof(ECMotor_TorCMD));
#endif //#endif
} // }
CANTransmit(motor->motor_can_ins,0.2); CANTransmit(motor->motor_can_ins,0.2);
} }

4
modules/motor/ECmotor/ECA8210.h
View File

@ -55,9 +55,13 @@ typedef struct
typedef struct // EC-A8120 typedef struct // EC-A8120
{ {
uint8_t init_flag;
uint16_t last_ecd; // 上一次读取的编码器值 uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值 uint16_t ecd; // 当前编码器值
float angle_single_round; // 单位rad +-12.5f float angle_single_round; // 单位rad +-12.5f
float offset_angle;
float last_angle; float last_angle;
float speed_rads; // speed rad/s float speed_rads; // speed rad/s
float real_current; // 实际电流 float real_current; // 实际电流

12
modules/motor/motor_task.c
View File

@ -8,14 +8,15 @@
void MotorControlTask() void MotorControlTask()
{ {
// static uint8_t cnt = 0; 设定不同电机的任务频率 static uint8_t cnt = 0; //设定不同电机的任务频率
// if(cnt%5==0) //200hz if(cnt%5==0) //200hz
ECMotorControl();
// if(cnt%10==0) //100hz // if(cnt%10==0) //100hz
DJIMotorControl(); DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl(); //LKMotorControl();
ECMotorControl();
// legacy support // legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
@ -25,4 +26,7 @@ void MotorControlTask()
ServeoMotorControl(); ServeoMotorControl();
// StepMotorControl(); // StepMotorControl();
cnt++;
if(cnt>=127)
cnt = 0;
} }

40
modules/referee/referee_protocol.h
View File

@ -107,12 +107,13 @@ typedef enum
/****************************接收数据的详细说明****************************/ /****************************接收数据的详细说明****************************/
/****************************接收数据的详细说明****************************/ /****************************接收数据的详细说明****************************/
/* ID: 0x0001 Byte: 3 比赛状态数据 */ /* ID: 0x0001 Byte: 3 V1.6.1 比赛状态数据 */
typedef struct typedef struct
{ {
uint8_t game_type : 4; uint8_t game_type : 4;
uint8_t game_progress : 4; uint8_t game_progress : 4;
uint16_t stage_remain_time; uint16_t stage_remain_time;
uint64_t SyncTimeStamp;
} ext_game_state_t; } ext_game_state_t;
/* ID: 0x0002 Byte: 1 比赛结果数据 */ /* ID: 0x0002 Byte: 1 比赛结果数据 */
@ -157,37 +158,32 @@ typedef struct
uint8_t supply_projectile_num; uint8_t supply_projectile_num;
} ext_supply_projectile_action_t; } ext_supply_projectile_action_t;
/* ID: 0X0201 Byte: 27 机器人状态数据 */ /* ID: 0X0201 Byte: 27 V1.6.1 机器人性能体系数据 */
typedef struct typedef struct
{ {
uint8_t robot_id; uint8_t robot_id;
uint8_t robot_level; uint8_t robot_level;
uint16_t remain_HP; uint16_t current_HP;
uint16_t max_HP; uint16_t maximum_HP;
uint16_t shooter_id1_17mm_cooling_rate; uint16_t shooter_barrel_cooling_value;
uint16_t shooter_id1_17mm_cooling_limit; uint16_t shooter_barrel_heat_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; uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
uint8_t mains_power_chassis_output : 1; uint8_t power_management_gimbal_output : 1;
uint8_t mains_power_shooter_output : 1; uint8_t power_management_chassis_output : 1;
uint8_t power_management_shooter_output : 1;
} ext_game_robot_state_t; } ext_game_robot_state_t;
/* ID: 0X0202 Byte: 14 实时功率热量数据 */ /* ID: 0X0202 Byte: 14 V1.6.1 实时功率热量数据 */
typedef struct typedef struct
{ {
uint16_t chassis_volt; uint16_t chassis_voltage;
uint16_t chassis_current; uint16_t chassis_current;
float chassis_power; // 瞬时功率 float chassis_power;
uint16_t chassis_power_buffer; // 60焦耳缓冲能量 uint16_t buffer_energy;
uint16_t shooter_heat0; // 17mm uint16_t shooter_17mm_1_barrel_heat;
uint16_t shooter_heat1; uint16_t shooter_17mm_2_barrel_heat;
uint16_t shooter_42mm_barrel_heat;
} ext_power_heat_data_t; } ext_power_heat_data_t;
/* ID: 0x0203 Byte: 16 机器人位置数据 */ /* ID: 0x0203 Byte: 16 机器人位置数据 */

2
modules/vofa/vofa.c
View File

@ -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 + 2] = 0x80;
send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴 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); //CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
} }

1
sentry_left.jdebug
View File

@ -33,7 +33,6 @@ void OnProjectLoad (void) {
// //
// User settings // User settings
// //
Edit.SysVar (VAR_HSS_SPEED, FREQ_100_HZ);
File.Open ("$(ProjectDir)/cmake-build-debug/sentry_left.elf"); File.Open ("$(ProjectDir)/cmake-build-debug/sentry_left.elf");
Project.SetOSPlugin ("FreeRTOSPlugin_CM4"); Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
} }

54
sentry_left.jdebug.user
View File

@ -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=D:/CLion/Project/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:362:9, State=BP_STATE_DISABLED Breakpoint=D:/CLion/Project/sentry_left/modules/motor/DJImotor/dji_motor.c:328, State=BP_STATE_DISABLED
OpenDocument="master_process.c", FilePath="C:/Users/sph/Desktop/sentry_left/modules/master_machine/master_process.c", Line=126 Breakpoint=D:/CLion/Project/sentry_left/modules/motor/DJImotor/dji_motor.c:362:9, State=BP_STATE_DISABLED
OpenDocument="main.c", FilePath="C:/Users/sph/Desktop/sentry_left/Src/main.c", Line=56 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 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="Call Stack", DockArea=RIGHT, x=0, y=0, w=300, h=253, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=720, h=487, 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="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=370, h=302, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x20009694
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=0, w=510, h=622, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=720, h=486, 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="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1292, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 OpenWindow="Console", DockArea=BOTTOM, x=0, y=0, w=517, h=302, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=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
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;296] 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="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="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;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="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="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="INS", DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1
WatchedExpression="pitch_motor", RefreshRate=5, 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 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_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="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="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="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="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="pid_ref", DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1
WatchedExpression="send_data", RefreshRate=5, DisplayFormat=DISPLAY_FORMAT_HEX, Window=Watched Data 1 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