144 lines
4.0 KiB
C++
144 lines
4.0 KiB
C++
//
|
|
// Created by xixiliadorabarry on 1/24/19.
|
|
//
|
|
#include <iostream>
|
|
#include <opencv2/core/core.hpp>
|
|
|
|
#include <energy/energy.h>
|
|
#include <uart/uart.h>
|
|
#include <energy/param_struct_define.h>
|
|
#include <energy/constant.h>
|
|
#include <camera/camera_wrapper.h>
|
|
#include <camera/video_wrapper.h>
|
|
#include <camera/wrapper_head.h>
|
|
#include <armor_finder/armor_finder.h>
|
|
#include <options/options.h>
|
|
#include <thread>
|
|
|
|
//#define DO_NOT_CNT_TIME
|
|
#include <log.h>
|
|
|
|
#define PROJECT_DIR PATH
|
|
#define ENERGY_STATE 1
|
|
#define ARMOR_STATE 0
|
|
|
|
using namespace cv;
|
|
using namespace std;
|
|
|
|
|
|
int state = ARMOR_STATE;
|
|
float curr_yaw = 0, curr_pitch = 0;
|
|
float mark_yaw = 0, mark_pitch = 0;
|
|
int mark = 0;
|
|
|
|
void uartReceive(Uart *uart);
|
|
|
|
int main(int argc, char *argv[]) {
|
|
process_options(argc, argv);
|
|
Uart uart;
|
|
thread receive(uartReceive, &uart);
|
|
bool flag = true;
|
|
|
|
while (flag) {
|
|
int ally_color = ALLY_RED;
|
|
int energy_part_rotation = CLOCKWISE;
|
|
|
|
int from_camera = 1;
|
|
if (!run_with_camera) {
|
|
cout << "Input 1 for camera, 0 for video files" << endl;
|
|
cin >> from_camera;
|
|
}
|
|
|
|
WrapperHead *video_armor;
|
|
WrapperHead *video_energy;
|
|
if (from_camera) {
|
|
video_armor = new CameraWrapper(0);
|
|
video_energy = new CameraWrapper(1);
|
|
} else {
|
|
video_armor = new VideoWrapper("/home/xinyang/Desktop/Video.mp4");
|
|
video_energy = new VideoWrapper("/home/xinyang/Desktop/Video.mp4");
|
|
}
|
|
if (video_armor->init()) {
|
|
cout << "Video source initialization successfully." << endl;
|
|
}
|
|
|
|
Mat energy_src, armor_src;
|
|
ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/");
|
|
|
|
Energy energy(uart);
|
|
energy.setAllyColor(ally_color);
|
|
energy.setRotation(energy_part_rotation);
|
|
|
|
bool ok = true;
|
|
|
|
while (ok) {
|
|
CNT_TIME(WORD_LIGHT_CYAN, "Total", {
|
|
if (state == ENERGY_STATE) {
|
|
ok = video_energy->read(energy_src);
|
|
if (show_origin) {
|
|
imshow("energy src", energy_src);
|
|
}
|
|
if (from_camera == 0) {
|
|
energy.extract(energy_src);
|
|
}
|
|
energy.run(energy_src);
|
|
} else {
|
|
ok = video_armor->read(armor_src);
|
|
if (show_origin) {
|
|
imshow("armor src", armor_src);
|
|
}
|
|
CNT_TIME(WORD_LIGHT_BLUE, "Armor Time", {
|
|
armorFinder.run(armor_src);
|
|
});
|
|
}
|
|
if (waitKey(1) == 'q') {
|
|
flag = false;
|
|
break;
|
|
}
|
|
});
|
|
}
|
|
delete video_armor;
|
|
cout << "Program fails. Restarting" << endl;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
#define RECEIVE_LOG_LEVEL LOG_NOTHING
|
|
|
|
void uartReceive(Uart *uart) {
|
|
char buffer[100];
|
|
int cnt = 0;
|
|
while (true) {
|
|
char data;
|
|
while ((data = uart->receive()) != '\n') {
|
|
buffer[cnt++] = data;
|
|
if (cnt >= 100) {
|
|
LOG(RECEIVE_LOG_LEVEL, "data receive over flow!");
|
|
cnt = 0;
|
|
}
|
|
}
|
|
if (cnt == 10) {
|
|
if (buffer[8] == 'e') {
|
|
state = ENERGY_STATE;
|
|
LOG(RECEIVE_LOG_LEVEL, "Energy state");
|
|
} else if (buffer[8] == 'a') {
|
|
state = ARMOR_STATE;
|
|
LOG(RECEIVE_LOG_LEVEL, "Armor state");
|
|
}
|
|
memcpy(&curr_yaw, buffer, 4);
|
|
memcpy(&curr_pitch, buffer + 4, 4);
|
|
LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
|
|
if (buffer[9] == 1) {
|
|
if (mark == 0) {
|
|
mark = 1;
|
|
mark_yaw = curr_yaw;
|
|
mark_pitch = curr_pitch;
|
|
}
|
|
LOG(RECEIVE_LOG_LEVEL, "Marked");
|
|
}
|
|
}
|
|
cnt = 0;
|
|
}
|
|
}
|