Robomaster-Auto-Aim/main.cpp at master · JiatongSun/Robomaster-Auto-Aim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
//
// Created by xixiliadorabarry on 1/24/19.
//
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "energy/energy.h"
#include "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 <time.h>
#include <thread>

using namespace cv;
using namespace std;

#define ENERGY_STATE 1
#define ARMOR_STATE 0

int state = ENERGY_STATE;
float yaw=0;
float pitch=0;


void uartReceive(Uart* uart);

int main()
{
    Uart uart;
	bool flag = true;
	short done = 0;//用于检测是否已经读完初始激光中心时的角度

	while (flag)
	{
        int ally_color = ALLY_RED;
        int energy_part_rotation = CLOCKWISE;

	    int from_camera = 1;
	    cout<<"Input 1 for camera, 0 for video files"<<endl;
	    cin>>from_camera;

		WrapperHead *video;
		if(from_camera)
		    video = new CameraWrapper;
		else
		    video = new VideoWrapper("r_l_640.avi", "fan_640.avi");

		if (video->init()) {
			cout << "Video source initialization successfully." << endl;
		}

		Mat src, src_none;

		ArmorFinder armorFinder(ENEMY_BLUE, uart);

        Energy energy(uart);
        energy.setAllyColor(ally_color);
        energy.setRotation(energy_part_rotation);

        static thread receive(uartReceive, &uart);

        if(state==1 && done == 0){
            energy.uart.receive_data();
            done = 1;
        }

//        energy.sendTargetByUart(-8,-8,-8);


		time_t t1 = time(nullptr), t2 = time(nullptr);

		while (video->read(src, src_none))
		{
//			if(!from_camera)energy.extract(src);
            if(state == 1){
                imshow("src", src);
                energy.run(src);
            }else{
                armorFinder.run(src_none);
            }

			if (waitKey(10) == 'q') {
				flag = false;
				break;
			}
		}
		delete video;
		cout << "Program fails. Restarting" << endl;
	}

	return 0;
}


void uartReceive(Uart* uart){
    char buffer[10];
    int cnt=0;
    while(true){
        char data;
        while((data=uart->receive()) != '\n'){
            buffer[cnt++] = data;
        }
        if(cnt==1 && buffer[0]=='e'){
            state = ENERGY_STATE;
        }else if(cnt==1 && buffer[0]=='a'){
            state = ARMOR_STATE;
        }else if(cnt==8){
            memcpy(&yaw, buffer, 4);
            memcpy(&pitch, buffer+4, 4);
        }
        cnt = 0;
    }
}