Revision af7e0f94
Added new at_destination functionality.
Maze turns are buggy and are the next order of business.
scout/libscout/src/LinesensorControl.cpp | ||
---|---|---|
61 | 61 |
ROS_ERROR("LinesensorControl query failed."); |
62 | 62 |
} |
63 | 63 |
|
64 |
if (srv.response.readings.size() != 8) |
|
65 |
{ |
|
66 |
ROS_WARN("Linesensor reading vector has %d readings, 8 expected.", |
|
67 |
int(srv.response.readings.size())); |
|
68 |
} |
|
69 |
|
|
64 | 70 |
return srv.response.readings; |
65 | 71 |
} |
66 | 72 |
|
... | ... | |
94 | 100 |
return ret_val; |
95 | 101 |
} |
96 | 102 |
|
97 |
bool LinesensorControl::fullline() |
|
103 |
bool LinesensorControl::fullline(vector<uint32_t> readings)
|
|
98 | 104 |
{ |
99 |
vector<uint32_t> readings = query(); |
|
105 |
if (readings.size() < 8) |
|
106 |
{ |
|
107 |
return false; |
|
108 |
} |
|
109 |
|
|
100 | 110 |
for(int i=0; i<8; i++) |
101 | 111 |
{ |
102 | 112 |
if(readings[i] < 200) |
... | ... | |
105 | 115 |
return true; |
106 | 116 |
} |
107 | 117 |
|
118 |
/** |
|
119 |
* We define a destination as two gaps of white between the line and patches |
|
120 |
* of 15-shade color (light grey) on either side. |
|
121 |
* I use 15-shade color in an attempt to not throw off the line localization. |
|
122 |
*/ |
|
123 |
bool LinesensorControl::destination(vector<uint32_t> readings) |
|
124 |
{ |
|
125 |
if (readings.size() < 8) |
|
126 |
{ |
|
127 |
return false; |
|
128 |
} |
|
129 |
|
|
130 |
ROS_INFO("Destination call. Readings: %d %d %d %d %d %d %d %d", |
|
131 |
readings[0], readings[1], readings[2], readings[3], |
|
132 |
readings[4], readings[5], readings[6], readings[7]); |
|
133 |
|
|
134 |
// Try to match this pattern to what we see |
|
135 |
int expected_pattern[] = {15, 0, 255, 0, 15}; |
|
136 |
int pat_idx = 0; |
|
137 |
|
|
138 |
for (int i = 0; i < 8; i++) |
|
139 |
{ |
|
140 |
// If the difference between what we have and expect is less than 5 |
|
141 |
if (abs(readings[i] - expected_pattern[pat_idx]) < 5) |
|
142 |
{ |
|
143 |
// Check. Keep looking for the rest of the pattern. |
|
144 |
pat_idx++; |
|
145 |
} |
|
146 |
|
|
147 |
// The whole pattern was found |
|
148 |
if (pat_idx > 4) |
|
149 |
{ |
|
150 |
ROS_INFO("FOUND DESTINATION"); |
|
151 |
return true; |
|
152 |
} |
|
153 |
} |
|
154 |
return false; |
|
155 |
} |
scout/libscout/src/LinesensorControl.h | ||
---|---|---|
53 | 53 |
double readline(); |
54 | 54 |
|
55 | 55 |
/** Check if we are at an intersecetion */ |
56 |
bool fullline(); |
|
56 |
bool fullline(std::vector<uint32_t> readings); |
|
57 |
|
|
58 |
/** Check if we are at the destination for a maze |
|
59 |
* (a special line pattern) */ |
|
60 |
bool destination(std::vector<uint32_t> readings); |
|
57 | 61 |
|
58 | 62 |
private: |
59 | 63 |
/* ROS publisher and client declaration */ |
scout/libscout/src/behaviors/line_follow.cpp | ||
---|---|---|
34 | 34 |
|
35 | 35 |
void line_follow::follow_line() |
36 | 36 |
{ |
37 |
vector<uint32_t> readings; |
|
37 | 38 |
do |
38 | 39 |
{ |
39 | 40 |
double line_loc = linesensor->readline(); |
41 |
ROS_INFO("Line loc: %lf", line_loc); |
|
40 | 42 |
|
41 | 43 |
if (line_loc == 0.0) |
42 | 44 |
{ |
... | ... | |
48 | 50 |
motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127); |
49 | 51 |
|
50 | 52 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
53 |
readings = linesensor->query(); |
|
51 | 54 |
} |
52 |
while(!linesensor->fullline()); |
|
55 |
while(!linesensor->fullline(readings) && |
|
56 |
!linesensor->destination(readings) && |
|
57 |
ok()); |
|
53 | 58 |
halt(); |
54 | 59 |
} |
55 | 60 |
|
56 | 61 |
void line_follow::turn_straight() |
57 | 62 |
{ |
63 |
vector<uint32_t> readings; |
|
58 | 64 |
do |
59 | 65 |
{ |
60 | 66 |
motor_l = -MOTOR_BASE; |
61 | 67 |
motor_r = -MOTOR_BASE; |
62 | 68 |
|
63 | 69 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
70 |
readings = linesensor->query(); |
|
64 | 71 |
} |
65 |
while(linesensor->fullline()); |
|
72 |
while(!linesensor->fullline(readings) && |
|
73 |
!linesensor->destination(readings) && |
|
74 |
ok()); |
|
66 | 75 |
} |
67 | 76 |
|
68 | 77 |
void line_follow::turn_left() |
... | ... | |
84 | 93 |
|
85 | 94 |
line_loc = linesensor->readline(); |
86 | 95 |
} |
87 |
while(!(-1 < line_loc && line_loc < 1)); |
|
96 |
while(!(-1 < line_loc && line_loc < 1) && ok());
|
|
88 | 97 |
} |
89 | 98 |
|
90 | 99 |
void line_follow::halt() |
... | ... | |
109 | 118 |
{ |
110 | 119 |
line_loc = linesensor->readline(); |
111 | 120 |
} |
112 |
while(line_loc < 2 && line_loc > -2); |
|
121 |
while(line_loc < 2 && line_loc > -2 && ok());
|
|
113 | 122 |
|
114 | 123 |
first = false; |
115 | 124 |
} |
116 | 125 |
line_loc = linesensor->readline(); |
117 | 126 |
} |
118 |
while(!(-1 < line_loc && line_loc < 1)); |
|
127 |
while(!(-1 < line_loc && line_loc < 1) && ok());
|
|
119 | 128 |
} |
120 | 129 |
|
121 | 130 |
void line_follow::turn_right() |
... | ... | |
137 | 146 |
|
138 | 147 |
line_loc = linesensor->readline(); |
139 | 148 |
} |
140 |
while(!(-1 < line_loc && line_loc < 1)); |
|
149 |
while(!(-1 < line_loc && line_loc < 1) && ok());
|
|
141 | 150 |
} |
142 | 151 |
|
143 | 152 |
void line_follow::u_turn() |
Also available in: Unified diff