Revision 68845e8e
ParkScout (2nd half of AutoParking Behavior) in Simular is working without end brakes.
scout/libscout/src/test_behaviors/ParkScout.cpp | ||
---|---|---|
24 | 24 |
*/ |
25 | 25 |
|
26 | 26 |
#include "ParkScout.h" |
27 |
#define LEFT_EMITTER_ID 110
|
|
28 |
#define RIGHT_EMITTER_ID 111
|
|
27 |
#define LEFT_EMITTER_ID 1 |
|
28 |
#define RIGHT_EMITTER_ID 0
|
|
29 | 29 |
|
30 | 30 |
|
31 |
#define LEFT 0 |
|
32 |
#define RIGHT 1 |
|
31 |
#define LEFT 1 |
|
32 |
#define RIGHT 2 |
|
33 |
#define NEITHER 0 |
|
33 | 34 |
|
34 | 35 |
using namespace std; |
35 | 36 |
|
... | ... | |
41 | 42 |
|
42 | 43 |
void ParkScout::run() { |
43 | 44 |
|
44 |
/* update_readings();
|
|
45 |
start_side = get_side(readings);
|
|
45 |
update_readings(); |
|
46 |
start_side = get_side(); |
|
46 | 47 |
|
47 |
while ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) || |
|
48 |
(start_side == RIGHT && checkBOM(6, RIGHT_EMITTER_ID))) { |
|
49 |
motors->set_sides(100, 100, MOTOR_PERCENT); |
|
50 |
bomR = bom->query(); |
|
48 |
while (ok() && ((start_side == LEFT && checkBOM(3, RIGHT_EMITTER_ID)) || |
|
49 |
(start_side == RIGHT && checkBOM(6, LEFT_EMITTER_ID)))) { |
|
50 |
motors->set_sides(25, 25, MOTOR_PERCENT); |
|
51 | 51 |
update_readings(); |
52 |
} |
|
52 |
|
|
53 |
} |
|
53 | 54 |
|
54 | 55 |
motors->set_sides(0, 0, MOTOR_PERCENT); |
55 | 56 |
|
56 | 57 |
|
57 |
while(ok) { |
|
58 |
reverse_park(); |
|
59 |
} */ |
|
58 |
while(ok()) { |
|
59 |
update_readings(); |
|
60 |
reverse_park(); |
|
61 |
} |
|
62 |
|
|
60 | 63 |
|
61 | 64 |
|
62 | 65 |
} |
63 |
/* |
|
64 |
void reverse_park() {
|
|
65 |
if(checkBOM(5, LEFT_EMITTER_ID) ||
|
|
66 |
|
|
67 |
bool ParkScout::reverse_park() {
|
|
68 |
if(checkBOM(5, LEFT_EMITTER_ID) || |
|
66 | 69 |
checkBOM(4, RIGHT_EMITTER_ID)) { //move forward |
67 | 70 |
motors->set_sides(10, 10, MOTOR_PERCENT); |
68 | 71 |
} |
69 | 72 |
else if(checkBOM(4, LEFT_EMITTER_ID) && |
70 | 73 |
checkBOM(5, RIGHT_EMITTER_ID)) { |
71 | 74 |
motors->set_sides(-20, -20, MOTOR_PERCENT); |
75 |
return true; |
|
72 | 76 |
} |
73 | 77 |
else { |
74 | 78 |
if (start_side == RIGHT) { //reverseright |
75 |
motors->set_sides(100, 20, MOTOR_PERCENT);
|
|
79 |
motors->set_sides(-40, -10, MOTOR_PERCENT);
|
|
76 | 80 |
} |
77 | 81 |
else { //reverseleft |
78 |
motors->set_sides(20, 100, MOTOR_PERCENT);
|
|
82 |
motors->set_sides(-10, -40, MOTOR_PERCENT);
|
|
79 | 83 |
|
80 | 84 |
} |
81 | 85 |
} |
82 |
|
|
86 |
return false; |
|
83 | 87 |
} |
84 | 88 |
|
85 |
int get_side() { |
|
89 |
int ParkScout::get_side() {
|
|
86 | 90 |
|
87 |
return RIGHT; |
|
91 |
if ((readings[1] > 0 || readings[2] > 0|| readings[3] > 0) && |
|
92 |
(readings[6] == 0 && readings[7] == 0 && readings[8] == 0)) { |
|
93 |
return LEFT; |
|
94 |
} |
|
95 |
else if ((readings[6] > 0 || readings[7] > 0|| readings[8] > 0) && |
|
96 |
(readings[1] == 0 && readings[2] == 0 && readings[3] == 0)) { |
|
97 |
return RIGHT; |
|
98 |
} |
|
99 |
else { |
|
100 |
return NEITHER; |
|
101 |
} |
|
88 | 102 |
} |
89 | 103 |
|
90 |
bool checkBOM(int bNum, int ID) {
|
|
104 |
bool ParkScout::checkBOM(int bNum, unsigned int ID) {
|
|
91 | 105 |
if(readings[bNum] == true) { |
92 |
ROS_INFO("Emitter ID: %d", senders[bNum]); |
|
106 |
//ROS_INFO("Emitter ID: %d", senders[bNum]);
|
|
93 | 107 |
return senders[bNum] == ID; |
94 | 108 |
} |
95 | 109 |
return false; |
96 | 110 |
|
97 | 111 |
} |
98 | 112 |
|
99 |
void update_readings() { |
|
113 |
void ParkScout::update_readings() {
|
|
100 | 114 |
BomReadings bomR = bom->query(); |
101 | 115 |
readings = bomR.readings; |
102 | 116 |
senders = bomR.senders; |
103 | 117 |
|
104 |
}*/ |
|
118 |
} |
scout/libscout/src/test_behaviors/ParkScout.h | ||
---|---|---|
47 | 47 |
private: |
48 | 48 |
std::string name; |
49 | 49 |
pos* scout_pos; |
50 |
/*int start_side;
|
|
50 |
int start_side; |
|
51 | 51 |
std::vector<uint32_t> readings; |
52 | 52 |
std::vector<uint32_t> senders; |
53 | 53 |
|
54 |
void reverse_park();
|
|
54 |
bool reverse_park();
|
|
55 | 55 |
int get_side(); |
56 |
bool checkBOM(int bNum, int ID); |
|
57 |
void update_readings(); */
|
|
56 |
bool checkBOM(int bNum, unsigned int ID);
|
|
57 |
void update_readings(); |
|
58 | 58 |
|
59 | 59 |
}; |
60 | 60 |
#endif |
Also available in: Unified diff