Revision a3539955
| ID | a353995525face04a9cc4ad87cf56f13380cc543 |
Fully working linefollowing. Instructions:
roscore
rosrun scoutsim scoutsim_node race
rosrun libscout libscout scout1 6
| scout/libscout/src/BehaviorList.cpp | ||
|---|---|---|
| 6 | 6 |
behavior_list.push_back((Behavior*)new draw_ccw_circle(scoutname)); |
| 7 | 7 |
behavior_list.push_back((Behavior*)new Odometry(scoutname)); |
| 8 | 8 |
behavior_list.push_back((Behavior*)new navigationMap(scoutname)); |
| 9 |
//behavior_list.push_back((Behavior*)new sim_line(scoutname)); |
|
| 10 | 9 |
behavior_list.push_back((Behavior*)new Scheduler(scoutname)); |
| 11 | 10 |
behavior_list.push_back((Behavior*)new WH_Robot(scoutname)); |
| 11 |
behavior_list.push_back((Behavior*)new sim_line(scoutname)); |
|
| 12 | 12 |
return; |
| 13 | 13 |
} |
| 14 | 14 |
|
| scout/libscout/src/behaviors/sim_line.cpp | ||
|---|---|---|
| 27 | 27 |
|
| 28 | 28 |
using namespace std; |
| 29 | 29 |
|
| 30 |
int motor_l; |
|
| 31 |
int motor_r; |
|
| 32 |
|
|
| 33 |
double sim_line::get_line_pos(vector<uint32_t> readings) |
|
| 34 |
{
|
|
| 35 |
unsigned int total_read = 0; |
|
| 36 |
unsigned int weighted_total = 0; |
|
| 37 |
|
|
| 38 |
for (int i = 0; i < 8; i++) |
|
| 39 |
{
|
|
| 40 |
total_read += readings[i]; |
|
| 41 |
weighted_total += i * readings[i]; |
|
| 42 |
} |
|
| 43 |
|
|
| 44 |
if (total_read == 0) |
|
| 45 |
{
|
|
| 46 |
return 0; |
|
| 47 |
} |
|
| 48 |
|
|
| 49 |
return ((double) weighted_total / total_read) - 4; |
|
| 50 |
} |
|
| 51 |
|
|
| 30 | 52 |
void sim_line::run() |
| 31 | 53 |
{
|
| 54 |
motor_l = -MOTOR_MAX; |
|
| 55 |
motor_r = -MOTOR_MAX; |
|
| 56 |
|
|
| 32 | 57 |
while(ok()) |
| 33 | 58 |
{
|
| 34 |
motors->set_sides(10, 20, MOTOR_ABSOLUTE);
|
|
| 59 |
double line_loc = get_line_pos(linesensor->query());
|
|
| 35 | 60 |
|
| 36 |
vector<uint32_t> readings = linesensor->query();
|
|
| 61 |
cout << "line_loc: " << line_loc << endl;
|
|
| 37 | 62 |
|
| 38 |
for (int i = 0; i < 8; i++)
|
|
| 63 |
if (line_loc < 0)
|
|
| 39 | 64 |
{
|
| 40 |
cout << "[" << i << "]" << ": " << readings[i] << ", "; |
|
| 65 |
line_loc = abs(line_loc); |
|
| 66 |
motor_l = -MOTOR_MAX; |
|
| 67 |
motor_r = -MOTOR_MAX + (MOTOR_MAX / 2) * line_loc; |
|
| 41 | 68 |
} |
| 42 |
cout << endl; |
|
| 69 |
else |
|
| 70 |
{
|
|
| 71 |
motor_l = -MOTOR_MAX + (MOTOR_MAX / 2) * line_loc; |
|
| 72 |
motor_r = -MOTOR_MAX; |
|
| 73 |
} |
|
| 74 |
|
|
| 75 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
| 43 | 76 |
|
| 44 | 77 |
spinOnce(); |
| 45 | 78 |
loop_rate->sleep(); |
| scout/libscout/src/behaviors/sim_line.h | ||
|---|---|---|
| 28 | 28 |
|
| 29 | 29 |
#include "../Behavior.h" |
| 30 | 30 |
|
| 31 |
#define MOTOR_MAX 50 |
|
| 32 |
|
|
| 31 | 33 |
class sim_line : Behavior |
| 32 | 34 |
{
|
| 33 | 35 |
public: |
| ... | ... | |
| 35 | 37 |
|
| 36 | 38 |
/** Actually executes the behavior. */ |
| 37 | 39 |
void run(); |
| 40 |
|
|
| 41 |
private: |
|
| 42 |
double get_line_pos(std::vector<uint32_t> readings); |
|
| 38 | 43 |
}; |
| 39 | 44 |
|
| 40 | 45 |
#endif |
Also available in: Unified diff