VSAG/vision_sjtu_19
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

3.28

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-28 04:23:17 +08:00
parent 12a02f382a
commit 460ef760fa
4 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -38,6 +38,14 @@ void ArmorFinder::antiTop() {
// 通过两次装甲角度为零的时间差计算陀螺旋转周期
// 根据旋转周期计算下一次装甲出现在角度为零的时间点
if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
/* PID相关控制 */
sum_yaw = sum_pitch = 0;
double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
double dy = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
double yaw = dx;
double pitch = dy;
/* PID相关控制 */
auto front_time = getFrontTime(time_seq, angle_seq);
auto once_periodms = getTimeIntervalms(front_time, last_front_time);
// if (abs(once_periodms - top_periodms[-1]) > 50) {

16
armor/src/armor_finder/armor_finder.cpp
View File

@@ -54,7 +54,11 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
anti_switch_cnt(0),
classifier(paras_folder),
contour_area(0),
tracking_cnt(0) {
tracking_cnt(0),
last_yaw(0),
last_pitch(0),
sum_yaw(0),
sum_pitch(0) {
}
void ArmorFinder::run(cv::Mat &src) {
@@ -75,6 +79,9 @@ void ArmorFinder::run(cv::Mat &src) {
tracker->init(src, target_box.rect);
state = TRACKING_STATE;
tracking_cnt = 0;
last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
sum_yaw = sum_pitch = 0;
LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
}
}
@@ -82,6 +89,8 @@ void ArmorFinder::run(cv::Mat &src) {
case TRACKING_STATE:
if (!stateTrackingTarget(src) || ++tracking_cnt > 100) { // 最多追踪100帧图像
state = SEARCHING_STATE;
last_yaw = last_pitch = 0;
sum_yaw = sum_pitch = 0;
LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
}
break;
@@ -93,6 +102,11 @@ end:
if(is_anti_top) { // 判断当前是否为反陀螺模式
antiTop();
}else if(target_box.rect != cv::Rect2d()) {
if(last_box.rect == cv::Rect2d()) { // 如果上一帧没有目标装甲板
sum_yaw = sum_pitch = 0;
last_yaw = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X;
last_pitch = target_box.rect.y + target_box.rect.height / 2 - IMAGE_CENTER_Y;
}
anti_top_cnt = 0;
time_seq.clear();
angle_seq.clear();

4
main.cpp
View File

@@ -44,7 +44,9 @@ WrapperHead *video = nullptr; // 云台摄像头视频源
Serial serial(115200); // 串口对象
uint8_t last_state = ARMOR_STATE; // 上次状态，用于初始化
// 自瞄主程序对象
ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
uint8_t enemy_color = ENEMY_BLUE;//RED ro BLUE
uint8_t forced_anti_top = 1;
ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
// 能量机关主程序对象
Energy energy(serial, mcu_data.enemy_color);