Repository navigation
Expand file tree
/
Copy pathall_tests.cpp
More file actions
86 lines (77 loc) · 2.41 KB
/
Copy pathall_tests.cpp
File metadata and controls
86 lines (77 loc) · 2.41 KB
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
#include "DriveTrain.h"
#include <unistd.h>
#include <utility> // pair
#include <math.h>
class RoomeTest {
public:
ArduinoSerial get_serial() {
return d_train.serial;
}
private:
DriveTrain d_train;
};
std::pair<int, int> get_odm(const std::string& odm_str) {
auto no_markers_str = odm_str.substr(1, odm_str.find('>') - 1);
int l_odm = stoi(no_markers_str.substr(0, no_markers_str.find(',')));
int r_odm = stoi(no_markers_str.substr(no_markers_str.find(',')+1));
return std::make_pair(l_odm, r_odm);
}
//Arduino follows format <State,forwards/backwards,left/right,emergencystop,distance,angle,piconfirmation>
//State can be 0 for standby, 1 for turn, and 2 for drive straight
bool drive_go_straight(RoomeTest& test_c) {
auto serial = test_c.get_serial();
serial.write_string("<2,0,0,0,0.1,0,1>");
auto odm_str = serial.read_string();
auto odm_p = get_odm(odm_str);
bool pass = std::abs(odm_p.first - 6) < 2 && std::abs(odm_p.second - 6) < 2;
if (!pass) {
std::cout << "Straight ODM: " << odm_str << std::endl;
}
return pass;
}
bool drive_turn_right(RoomeTest& test_c) {
auto serial = test_c.get_serial();
serial.write_string("<1,0,1,0,0,90,1>");
auto odm_str = serial.read_string();
auto odm_p = get_odm(odm_str);
bool pass = std::abs(odm_p.first - 20) < 2 && odm_p.second == 0;
if (!pass) {
std::cout << "Right turn ODM: " << odm_str << std::endl;
}
return pass;
}
bool drive_turn_left(RoomeTest& test_c) {
auto serial = test_c.get_serial();
serial.write_string("<1,0,0,0,0,90,1>");
auto odm_str = serial.read_string();
auto odm_p = get_odm(odm_str);
bool pass = odm_p.first == 0 && std::abs(odm_p.second - 20) < 2;
if (!pass) {
std::cout << "Left turn ODM: " << odm_str << std::endl;
}
return pass;
}
int main() {
RoomeTest test_c;
std::cout << "Test Straight: ";
if (drive_go_straight(test_c)) {
std::cout << "Pass!" << std::endl;
} else {
std::cout << "Fail!" << std::endl;
}
sleep(2);
std::cout << "Test Right: ";
if (drive_turn_right(test_c)) {
std::cout << "Pass!" << std::endl;
} else {
std::cout << "Fail!" << std::endl;
}
sleep(2);
std::cout << "Test Left: ";
if (drive_turn_left(test_c)) {
std::cout << "Pass!" << std::endl;
} else {
std::cout << "Fail!" << std::endl;
}
return 0;
}