Revision 414d2b48
| ID | 414d2b48e22681108c656cd57dbf2177efd5bbfc |
Updated line follow code to make turns for Lab 2.
Also upgraded at_destination and LineSensor::destination() for Intro Lab 2.
| scout/libscout/src/Behavior.cpp | ||
|---|---|---|
| 66 | 66 |
Behavior::~Behavior() |
| 67 | 67 |
{
|
| 68 | 68 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
| 69 |
ROS_INFO("I get called");
|
|
| 70 | 69 |
spinOnce(); |
| 71 | 70 |
loop_rate->sleep(); |
| 72 | 71 |
delete wl_receiver; |
| scout/libscout/src/LinesensorControl.cpp | ||
|---|---|---|
| 117 | 117 |
|
| 118 | 118 |
/** |
| 119 | 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.
|
|
| 120 |
* of 95-shade color (dark grey) on either side.
|
|
| 121 |
* I use 95-shade color in an attempt to not throw off the line localization.
|
|
| 122 | 122 |
*/ |
| 123 | 123 |
bool LinesensorControl::destination(vector<uint32_t> readings) |
| 124 | 124 |
{
|
| ... | ... | |
| 127 | 127 |
return false; |
| 128 | 128 |
} |
| 129 | 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 | 130 |
// Try to match this pattern to what we see |
| 135 |
int expected_pattern[] = {15, 0, 255, 0, 15};
|
|
| 131 |
int expected_pattern[] = {95, 0, 255, 0, 95};
|
|
| 136 | 132 |
int pat_idx = 0; |
| 137 | 133 |
|
| 138 | 134 |
for (int i = 0; i < 8; i++) |
| ... | ... | |
| 147 | 143 |
// The whole pattern was found |
| 148 | 144 |
if (pat_idx > 4) |
| 149 | 145 |
{
|
| 150 |
ROS_INFO("FOUND DESTINATION");
|
|
| 151 | 146 |
return true; |
| 152 | 147 |
} |
| 153 | 148 |
} |
| scout/libscout/src/behaviors/line_follow.cpp | ||
|---|---|---|
| 30 | 30 |
static int motor_l; |
| 31 | 31 |
static int motor_r; |
| 32 | 32 |
|
| 33 |
Duration init_turn_time(0.4);
|
|
| 33 |
Duration init_turn_time(0.8);
|
|
| 34 | 34 |
|
| 35 | 35 |
void line_follow::follow_line() |
| 36 | 36 |
{
|
| ... | ... | |
| 38 | 38 |
do |
| 39 | 39 |
{
|
| 40 | 40 |
double line_loc = linesensor->readline(); |
| 41 |
ROS_INFO("Line loc: %lf", line_loc);
|
|
| 42 | 41 |
|
| 43 | 42 |
if (line_loc == 0.0) |
| 44 | 43 |
{
|
| 45 | 44 |
motors->set_sides(-60, -60, MOTOR_ABSOLUTE); |
| 46 |
continue; |
|
| 47 | 45 |
} |
| 46 |
else |
|
| 47 |
{
|
|
| 48 |
motor_l = min(max((int) (-MOTOR_BASE + KP * line_loc), -128), 127); |
|
| 49 |
motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127); |
|
| 48 | 50 |
|
| 49 |
motor_l = min(max((int) (-MOTOR_BASE + KP * line_loc), -128), 127); |
|
| 50 |
motor_r = min(max((int) (-MOTOR_BASE - KP * line_loc), -128), 127); |
|
| 51 |
|
|
| 52 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
| 51 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
| 52 |
} |
|
| 53 | 53 |
readings = linesensor->query(); |
| 54 | 54 |
} |
| 55 | 55 |
while(!linesensor->fullline(readings) && |
| ... | ... | |
| 63 | 63 |
vector<uint32_t> readings; |
| 64 | 64 |
do |
| 65 | 65 |
{
|
| 66 |
motor_l = -MOTOR_BASE; |
|
| 67 |
motor_r = -MOTOR_BASE; |
|
| 68 |
|
|
| 69 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
| 66 |
motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE); |
|
| 70 | 67 |
readings = linesensor->query(); |
| 71 | 68 |
} |
| 72 | 69 |
while(!linesensor->fullline(readings) && |
| ... | ... | |
| 81 | 78 |
do |
| 82 | 79 |
{
|
| 83 | 80 |
motor_l = -MOTOR_BASE; |
| 84 |
motor_r = 2*MOTOR_BASE/5;
|
|
| 81 |
motor_r = 0;
|
|
| 85 | 82 |
|
| 86 | 83 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
| 87 | 84 |
|
| ... | ... | |
| 133 | 130 |
float line_loc; |
| 134 | 131 |
do |
| 135 | 132 |
{
|
| 136 |
motor_l = 2*MOTOR_BASE/5;
|
|
| 133 |
motor_l = 0;
|
|
| 137 | 134 |
motor_r = -MOTOR_BASE; |
| 138 | 135 |
|
| 139 | 136 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
Also available in: Unified diff