Revision af7e0f94
Added new at_destination functionality.
Maze turns are buggy and are the next order of business.
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