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/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