联盟赛版本 修复卡弹检测...其他改了啥我也不记得了
This commit is contained in:
parent
a32ac620ea
commit
1b4e26c50d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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]<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 {
|
||||
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]<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[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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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 设置发送数据的姿态部分
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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; // 实际电流
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 机器人位置数据 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
Loading…
Reference in New Issue