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

Auto Aim

Browse Source
This commit is contained in:
JiatongSun
2019-05-07 09:33:28 +08:00
parent 7fcd8e6c84
commit 508004c548
9 changed files with 191 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
main.cpp
View File

@@ -26,116 +26,115 @@ 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 state = ENERGY_STATE;
float curr_yaw=0, curr_pitch=0;
float mark_yaw=0, mark_pitch=0;
int mark = 0;
void uartReceive(Uart *uart);
void uartReceive(Uart* uart);
int main(int argc, char *argv[]) {
int main(int argc, char *argv[]){
process_options(argc, argv);
Uart uart;
thread receive(uartReceive, &uart);
bool flag = true;
bool flag = true;
while (flag) {
while (flag){
int ally_color = ALLY_RED;
int energy_part_rotation = CLOCKWISE;
int from_camera = 1;
if (!run_with_camera) {
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_armor;
WrapperHead *video_energy;
if (from_camera) {
video_armor = new CameraWrapper(0);
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;
}else {
video_armor = new VideoWrapper("r_l_640.avi");
video_energy = new VideoWrapper("r_l_640.avi");
}
if (video_energy->init()) {
cout << "Video source initialization successfully." << endl;
}
Mat energy_src, armor_src;
ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/");
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;
bool ok=true;
while (ok) {
CNT_TIME(WORD_LIGHT_CYAN, "Total", {
while (ok){
// CNT_TIME(WORD_LIGHT_CYAN, "Total", {
// ok = video_energy->read(energy_src) && video_energy->read(armor_src);
ok = video_energy->read(energy_src);
if (show_origin) {
imshow("enery src", energy_src);
imshow("armor src", armor_src);
}
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);
});
// 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;
}
// });
}
delete video_energy;
cout << "Program fails. Restarting" << endl;
}
return 0;
return 0;
}
#define RECEIVE_LOG_LEVEL LOG_NOTHING
#define RECEIVE_LOG_LEVEL LOG_MSG
void uartReceive(Uart *uart) {
void uartReceive(Uart* uart){
char buffer[100];
int cnt = 0;
while (true) {
int cnt=0;
while(true){
char data;
while ((data = uart->receive()) != '\n') {
while((data=uart->receive()) != '\n'){
buffer[cnt++] = data;
if (cnt >= 100) {
if(cnt >= 100){
LOG(RECEIVE_LOG_LEVEL, "data receive over flow!");
cnt = 0;
cnt = 0;
}
}
if (cnt == 10) {
if (buffer[8] == 'e') {
if(cnt == 10){
if(buffer[8] == 'e'){
state = ENERGY_STATE;
LOG(RECEIVE_LOG_LEVEL, "Energy state");
} else if (buffer[8] == 'a') {
// LOG(RECEIVE_LOG_LEVEL, "Energy state");
}else if(buffer[8] == 'a'){
state = ARMOR_STATE;
LOG(RECEIVE_LOG_LEVEL, "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) {
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");
// LOG(RECEIVE_LOG_LEVEL, "Marked");
}
}
cnt = 0;