Revision ae21730e
ID | ae21730ed52fe1d5e5638d166cca7dcef668ed08 |
Sim line can make turns now! yay
scout/libscout/src/LinesensorControl.cpp | ||
---|---|---|
91 | 91 |
|
92 | 92 |
bool LinesensorControl::fullline() |
93 | 93 |
{ |
94 |
return false; |
|
95 |
} |
|
96 |
|
|
97 |
void LinesensorControl::turnLeft() |
|
98 |
{ |
|
99 | 94 |
vector<uint32_t> readings = query(); |
95 |
for(int i=0; i<8; i++) |
|
96 |
{ |
|
97 |
if(readings[i] < 200) |
|
98 |
return false; |
|
99 |
} |
|
100 |
return true; |
|
100 | 101 |
} |
101 | 102 |
|
102 |
void LinesensorControl::turnRight() |
|
103 |
{ |
|
104 |
vector<uint32_t> readings = query(); |
|
105 |
|
|
106 |
} |
scout/libscout/src/LinesensorControl.h | ||
---|---|---|
52 | 52 |
/** Get line position */ |
53 | 53 |
double readline(); |
54 | 54 |
|
55 |
/** Implement turn functions */ |
|
56 |
void turnLeft(); |
|
57 |
void turnRight(); |
|
55 |
/** Check if we are at an intersecetion */ |
|
56 |
bool fullline(); |
|
58 | 57 |
|
59 | 58 |
private: |
60 | 59 |
/* ROS publisher and client declaration */ |
61 | 60 |
ros::ServiceClient query_client; |
62 | 61 |
ros::NodeHandle node; |
63 |
|
|
64 |
/** Check if we are at an intersecetion */ |
|
65 |
bool fullline(); |
|
66 | 62 |
}; |
67 | 63 |
|
68 | 64 |
#endif /* _LINESENSOR_CONTORL_H_ */ |
scout/libscout/src/behaviors/sim_line.cpp | ||
---|---|---|
29 | 29 |
|
30 | 30 |
static int motor_l; |
31 | 31 |
static int motor_r; |
32 |
static double last_ret; |
|
33 | 32 |
|
34 |
/** TODO: This should be part of the line sensor library */ |
|
35 |
double sim_line::get_line_pos(vector<uint32_t> readings) |
|
33 |
void sim_line::follow_line() |
|
36 | 34 |
{ |
37 |
unsigned int total_read = 0; |
|
38 |
unsigned int weighted_total = 0; |
|
39 |
|
|
40 |
for (int i = 0; i < 8; i++) |
|
35 |
while(!linesensor->fullline()) |
|
41 | 36 |
{ |
42 |
total_read += readings[i]; |
|
43 |
weighted_total += i * readings[i]; |
|
44 |
} |
|
37 |
double line_loc = linesensor->readline(); |
|
45 | 38 |
|
46 |
if (total_read == 0) |
|
47 |
{ |
|
48 |
return last_ret; |
|
39 |
cout << "line_loc: " << line_loc << endl; |
|
40 |
|
|
41 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
|
42 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
|
43 |
|
|
44 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
49 | 45 |
} |
50 |
|
|
51 |
double ret_val = last_ret = ((double) weighted_total / total_read) - 4; |
|
52 |
return ret_val; |
|
46 |
halt(); |
|
53 | 47 |
} |
54 | 48 |
|
55 |
void sim_line::run()
|
|
49 |
void sim_line::turn_left()
|
|
56 | 50 |
{ |
51 |
bool first = true; |
|
52 |
do |
|
53 |
{ |
|
57 | 54 |
motor_l = -MOTOR_BASE; |
58 |
motor_r = -MOTOR_BASE;
|
|
55 |
motor_r = MOTOR_BASE/8;
|
|
59 | 56 |
|
60 |
while(ok()) |
|
57 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
58 |
|
|
59 |
double line_loc = linesensor->readline(); |
|
60 |
cout << "line_loc: " << line_loc << endl; |
|
61 |
if(first) |
|
61 | 62 |
{ |
62 |
double line_loc = linesensor->readline(); |
|
63 |
loop_rate->sleep(); |
|
64 |
loop_rate->sleep(); |
|
65 |
loop_rate->sleep(); |
|
66 |
loop_rate->sleep(); |
|
67 |
first = false; |
|
68 |
} |
|
69 |
} |
|
70 |
while(linesensor->readline()); |
|
71 |
} |
|
63 | 72 |
|
64 |
cout << "line_loc: " << line_loc << endl; |
|
73 |
void sim_line::halt() |
|
74 |
{ |
|
75 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
|
76 |
} |
|
65 | 77 |
|
66 |
motor_l = -MOTOR_BASE + SCALE * line_loc; |
|
67 |
motor_r = -MOTOR_BASE - SCALE * line_loc; |
|
78 |
void sim_line::turn_right() |
|
79 |
{ |
|
80 |
bool first = true; |
|
81 |
do |
|
82 |
{ |
|
83 |
motor_l = MOTOR_BASE/8; |
|
84 |
motor_r = -MOTOR_BASE; |
|
68 | 85 |
|
69 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE);
|
|
86 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
70 | 87 |
|
71 |
spinOnce(); |
|
88 |
double line_loc = linesensor->readline(); |
|
89 |
cout << "line_loc: " << line_loc << endl; |
|
90 |
if(first) |
|
91 |
{ |
|
92 |
loop_rate->sleep(); |
|
93 |
loop_rate->sleep(); |
|
94 |
loop_rate->sleep(); |
|
72 | 95 |
loop_rate->sleep(); |
96 |
first = false; |
|
73 | 97 |
} |
98 |
} |
|
99 |
while(linesensor->readline()); |
|
100 |
} |
|
101 |
|
|
102 |
void sim_line::run() |
|
103 |
{ |
|
104 |
follow_line(); |
|
105 |
turn_right(); |
|
106 |
follow_line(); |
|
107 |
turn_left(); |
|
108 |
follow_line(); |
|
109 |
turn_right(); |
|
110 |
follow_line(); |
|
74 | 111 |
} |
scout/libscout/src/behaviors/sim_line.h | ||
---|---|---|
40 | 40 |
void run(); |
41 | 41 |
|
42 | 42 |
private: |
43 |
double get_line_pos(std::vector<uint32_t> readings); |
|
43 |
void turn_left(); |
|
44 |
void turn_right(); |
|
45 |
void follow_line(); |
|
46 |
void halt(); |
|
44 | 47 |
}; |
45 | 48 |
|
46 | 49 |
#endif |
Also available in: Unified diff