Revision afa9104d
Ok Scheduler warehouse behaviour working! and linefollowing works better now! Onto better demo.
scout/libscout/src/behaviors/WH_Robot.cpp | ||
---|---|---|
34 | 34 |
switch(t) |
35 | 35 |
{ |
36 | 36 |
case ISTRAIGHT: |
37 |
turn_straight(); |
|
37 | 38 |
follow_line(); |
38 | 39 |
break; |
39 | 40 |
case ILEFT: |
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.5); |
|
34 |
|
|
33 | 35 |
void line_follow::follow_line() |
34 | 36 |
{ |
35 |
while(!linesensor->fullline())
|
|
37 |
do
|
|
36 | 38 |
{ |
37 | 39 |
double line_loc = linesensor->readline(); |
38 | 40 |
|
... | ... | |
41 | 43 |
|
42 | 44 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
43 | 45 |
} |
46 |
while(!linesensor->fullline()); |
|
44 | 47 |
halt(); |
45 | 48 |
ROS_INFO("Intersection reached!"); |
46 | 49 |
} |
47 | 50 |
|
51 |
void line_follow::turn_straight() |
|
52 |
{ |
|
53 |
do |
|
54 |
{ |
|
55 |
motor_l = -MOTOR_BASE; |
|
56 |
motor_r = -MOTOR_BASE; |
|
57 |
|
|
58 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
59 |
} |
|
60 |
while(linesensor->fullline()); |
|
61 |
} |
|
62 |
|
|
48 | 63 |
void line_follow::turn_left() |
49 | 64 |
{ |
50 | 65 |
bool first = true; |
66 |
float line_loc; |
|
51 | 67 |
do |
52 | 68 |
{ |
53 | 69 |
motor_l = -MOTOR_BASE; |
... | ... | |
57 | 73 |
|
58 | 74 |
if(first) |
59 | 75 |
{ |
60 |
loop_rate->sleep(); |
|
61 |
loop_rate->sleep(); |
|
62 |
loop_rate->sleep(); |
|
76 |
init_turn_time.sleep(); |
|
63 | 77 |
first = false; |
64 | 78 |
} |
79 |
|
|
80 |
line_loc = linesensor->readline(); |
|
65 | 81 |
} |
66 |
while(linesensor->readline());
|
|
82 |
while(!(-.5 < line_loc && line_loc < .5));
|
|
67 | 83 |
} |
68 | 84 |
|
69 | 85 |
void line_follow::halt() |
... | ... | |
71 | 87 |
motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
72 | 88 |
} |
73 | 89 |
|
90 |
void line_follow::spot_turn() |
|
91 |
{ |
|
92 |
bool first = true; |
|
93 |
float line_loc; |
|
94 |
do |
|
95 |
{ |
|
96 |
motor_l = MOTOR_BASE; |
|
97 |
motor_r = -MOTOR_BASE; |
|
98 |
|
|
99 |
motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
|
100 |
|
|
101 |
if(first) |
|
102 |
{ |
|
103 |
while(line_loc < 2 && line_loc > -2) |
|
104 |
line_loc = linesensor->readline(); |
|
105 |
|
|
106 |
first = false; |
|
107 |
} |
|
108 |
line_loc = linesensor->readline(); |
|
109 |
} |
|
110 |
while(!(-1 < line_loc && line_loc < 1)); |
|
111 |
} |
|
112 |
|
|
74 | 113 |
void line_follow::turn_right() |
75 | 114 |
{ |
76 | 115 |
bool first = true; |
116 |
float line_loc; |
|
77 | 117 |
do |
78 | 118 |
{ |
79 | 119 |
motor_l = MOTOR_BASE/8; |
... | ... | |
83 | 123 |
|
84 | 124 |
if(first) |
85 | 125 |
{ |
86 |
loop_rate->sleep(); |
|
87 |
loop_rate->sleep(); |
|
88 |
loop_rate->sleep(); |
|
126 |
init_turn_time.sleep(); |
|
89 | 127 |
first = false; |
90 | 128 |
} |
129 |
|
|
130 |
line_loc = linesensor->readline(); |
|
91 | 131 |
} |
92 |
while(linesensor->readline());
|
|
132 |
while(!(-1 < line_loc && line_loc < 1));
|
|
93 | 133 |
} |
94 | 134 |
|
95 | 135 |
void line_follow::u_turn() |
... | ... | |
102 | 142 |
void line_follow::run() |
103 | 143 |
{ |
104 | 144 |
follow_line(); |
145 |
turn_right(); |
|
105 | 146 |
follow_line(); |
106 |
/* |
|
107 |
follow_line(); |
|
108 |
u_turn(); |
|
109 |
follow_line(); |
|
110 |
u_turn(); |
|
111 |
follow_line(); |
|
112 |
follow_line(); |
|
113 |
u_turn(); |
|
147 |
turn_straight(); |
|
148 |
motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE); |
|
149 |
Duration meow(2); |
|
150 |
meow.sleep(); |
|
151 |
spot_turn(); |
|
114 | 152 |
follow_line(); |
115 |
*/ |
|
116 | 153 |
} |
scout/libscout/src/behaviors/line_follow.h | ||
---|---|---|
42 | 42 |
protected: |
43 | 43 |
void turn_left(); |
44 | 44 |
void turn_right(); |
45 |
void turn_straight(); |
|
46 |
void spot_turn(); |
|
45 | 47 |
void u_turn(); |
46 | 48 |
void follow_line(); |
47 | 49 |
void halt(); |
scout/libscout/src/behaviors/navigationMap.cpp | ||
---|---|---|
94 | 94 |
Edge* a7 = new Edge[ARRAY_SIZE]; |
95 | 95 |
a7[0] = MAKE_EDGE(ISTRAIGHT, 6, 0); |
96 | 96 |
a7[1] = MAKE_EDGE(ILEFT, 11, 40); |
97 |
a7[2] = MAKE_EDGE(IUTURN, 3, 10);
|
|
97 |
a7[2] = MAKE_EDGE(IUTURN, 4, 10);
|
|
98 | 98 |
|
99 | 99 |
Edge* a8 = new Edge[ARRAY_SIZE]; |
100 | 100 |
a8[0] = MAKE_EDGE(ISTRAIGHT, 7, 0); |
Also available in: Unified diff