Revision af7e0f94
| ID | af7e0f941c6ec3ce22eb88a8e49af36147359107 |
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