NOTOS_RM_AutoAim/AutoAim.cpp at main · notos-robomaster/NOTOS_RM_AutoAim · 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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#include "Armor/Armor.h"
#include "General/General.h"
#include "AngleSolver/AngleSolver.h"
#include "Serial/serialport.h"
#include "Filter/Kalman.h"
#include "GraphWindow/MotionDetection.h"

ArmorDetector detector;
AngleSolver angleSolver;
Kalman kalman;
#ifdef SHOW_PLOT
int argc;
char **argv = nullptr;
QApplication a(argc, argv);
MotionDetect motionDetect;
#endif // SHOW_PLOT

Color ENEMYCOLOR = BLUE;
//Color ENEMYCOLOR = RED;

int targetNum = 3;
unsigned char readData[3];
int length = 10;
int rLength = 4;

bool bRun = true;
bool firstTime = true;

char ttyUSB_path[] = "/dev/ttyUSB0";    //设置串口名称
SerialPort port(ttyUSB_path);   //创建串口类对象

void autoaimRun()
{
    detector.loadSVM("../General/123svm.xml");  // todo SVM update

    angleSolver.setCameraParam("../General/camera_params.xml", 3);
    angleSolver.setArmorSize(SMALL_ARMOR, 135, 125);
    angleSolver.setArmorSize(BIG_ARMOR, 230, 127);
    angleSolver.setBulletSpeed(274032);

    port.initSerialPort();  //串口初始化

    imageInit();

    do
    {
        fpsInit();

        // Serial
#ifdef WAIT_RECEIVE
        port.receive(readData, rLength);
//        for (int i = 0; i < sizeof(readData)/sizeof(readData[0]); i++)
//        {
//            cout << (int)readData[i] << endl;
//        }

        // Wait for the referee system to read the color
        if (firstTime)
        {
            if (readData[3] == 0x03)
            {
                firstTime = false;
                // if data is 0, mean our COLOR is RED; enemy COLOR is BLUE
                if (readData[2] == 0)   ENEMYCOLOR = BLUE;
                if (readData[2] == 1)   ENEMYCOLOR = RED;
            }
        }
#endif // WAIT_RECEIVE

        detector.setEnemyColor(ENEMYCOLOR);
        imageUpdating();
        detector.setImg(src);
        detector.run(src);

        double yaw = 0, pitch = 0, distance = 0;
        Point2f centerPoint;
        if (detector.isFoundArmor())
        {
            vector<Point2f> contourPoints;
            ArmorType type;
            detector.getTargetInfo(contourPoints, centerPoint, type);
#ifdef KALMAN
            centerPoint = kalman.predictRun(centerPoint);
#endif // KALMAN
            angleSolver.getAngle(contourPoints, centerPoint, SMALL_ARMOR, yaw, pitch, distance);
        }

        // Serial
        port.TransformData_Part(detector.isFoundArmor(), yaw, pitch);
        port.send(length);
#ifdef WAIT_RECEIVE
        if (readData[3] == 3)
        {
            if (readData[1] == 1)       targetNum = 1;
            else if (readData[1] == 2)  targetNum = 2;
            else if (readData[1] == 3)  targetNum = 3;
            else if (readData[1] == 4)  targetNum = 4;
            else if (readData[1] == 5)  targetNum = 5;
            else if (readData[1] == 6)  targetNum = 6;
        }
#endif // WAIT_RECEIVE

        detector.setTargetNum(targetNum);

        fpsUpdating();

#ifdef ALL_DEBUG_MOOD
#ifdef SHOW_PLOT
        motionDetect.setPoint(yaw, 0);
        motionDetect.setPoint(pitch, 1);
#endif // SHOW_PLOT
        detector.showDebugInfo();
        if (detector.isFoundArmor())
        {
            angleSolver.showDebugInfo();
            kalman.showDebugInfo(src);
        }
#endif // ALL_DEBUG_MOOD

        char chKey = waitKey(1);
        switch (chKey)
        {
            case '0':
                targetNum = 0;
            case '1':
                targetNum = 1;
                break;
            case '2':
                targetNum = 2;
                break;
            case '3':
                targetNum = 3;
                break;
            case 'i':
            case 'I':
                targetNum = 4;
                break;
            case 'b':
            case 'B':
                ENEMYCOLOR = BLUE;
                break;
            case 'r':
            case 'R':
                ENEMYCOLOR = RED;
                break;
            case 'q':
            case 'Q':
            case 27:
                bRun = false;
                break;
            default:
                break;
        }
    } while (bRun);
}