root / scout / libscout / src / behaviors / line_follow.cpp @ 7b5ea072
History | View | Annotate | Download (3.75 KB)
1 | 9143e077 | Lalitha Ganesan | /**
|
---|---|---|---|
2 | 58371433 | Priya | * Copyright (c) 2011 Colony Project
|
3 | *
|
||
4 | * Permission is hereby granted, free of charge, to any person
|
||
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | *
|
||
13 | * The above copyright notice and this permission notice shall be
|
||
14 | * included in all copies or substantial portions of the Software.
|
||
15 | *
|
||
16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | 9143e077 | Lalitha Ganesan | */
|
25 | |||
26 | 60b98383 | Priya | #include "line_follow.h" |
27 | 9143e077 | Lalitha Ganesan | |
28 | 58371433 | Priya | using namespace std; |
29 | 9143e077 | Lalitha Ganesan | |
30 | 58371433 | Priya | static int motor_l; |
31 | static int motor_r; |
||
32 | 9143e077 | Lalitha Ganesan | |
33 | 414d2b48 | Alex | Duration init_turn_time(0.8); |
34 | afa9104d | Priya | |
35 | 58371433 | Priya | void line_follow::follow_line()
|
36 | { |
||
37 | af7e0f94 | Alex | vector<uint32_t> readings; |
38 | afa9104d | Priya | do
|
39 | 58371433 | Priya | { |
40 | double line_loc = linesensor->readline();
|
||
41 | 9143e077 | Lalitha Ganesan | |
42 | 6ee555a3 | Priya | if (line_loc == 0.0) |
43 | { |
||
44 | motors->set_sides(-60, -60, MOTOR_ABSOLUTE); |
||
45 | } |
||
46 | 414d2b48 | Alex | 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); |
||
50 | 6ee555a3 | Priya | |
51 | 414d2b48 | Alex | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
52 | } |
||
53 | af7e0f94 | Alex | readings = linesensor->query(); |
54 | 58371433 | Priya | } |
55 | af7e0f94 | Alex | while(!linesensor->fullline(readings) &&
|
56 | !linesensor->destination(readings) && |
||
57 | ok()); |
||
58 | 58371433 | Priya | halt(); |
59 | } |
||
60 | 9143e077 | Lalitha Ganesan | |
61 | afa9104d | Priya | void line_follow::turn_straight()
|
62 | { |
||
63 | af7e0f94 | Alex | vector<uint32_t> readings; |
64 | afa9104d | Priya | do
|
65 | { |
||
66 | 414d2b48 | Alex | motors->set_sides(-MOTOR_BASE, -MOTOR_BASE, MOTOR_ABSOLUTE); |
67 | af7e0f94 | Alex | readings = linesensor->query(); |
68 | afa9104d | Priya | } |
69 | af7e0f94 | Alex | while(!linesensor->fullline(readings) &&
|
70 | !linesensor->destination(readings) && |
||
71 | ok()); |
||
72 | afa9104d | Priya | } |
73 | |||
74 | 58371433 | Priya | void line_follow::turn_left()
|
75 | 60b98383 | Priya | { |
76 | 58371433 | Priya | bool first = true; |
77 | afa9104d | Priya | float line_loc;
|
78 | 58371433 | Priya | do
|
79 | { |
||
80 | motor_l = -MOTOR_BASE; |
||
81 | 414d2b48 | Alex | motor_r = 0;
|
82 | 60b98383 | Priya | |
83 | 58371433 | Priya | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
84 | 60b98383 | Priya | |
85 | 58371433 | Priya | if(first)
|
86 | 9143e077 | Lalitha Ganesan | { |
87 | afa9104d | Priya | init_turn_time.sleep(); |
88 | 58371433 | Priya | first = false;
|
89 | 9143e077 | Lalitha Ganesan | } |
90 | afa9104d | Priya | |
91 | line_loc = linesensor->readline(); |
||
92 | 60b98383 | Priya | } |
93 | af7e0f94 | Alex | while(!(-1 < line_loc && line_loc < 1) && ok()); |
94 | 9143e077 | Lalitha Ganesan | } |
95 | |||
96 | 58371433 | Priya | void line_follow::halt()
|
97 | 9143e077 | Lalitha Ganesan | { |
98 | 58371433 | Priya | motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
99 | 9143e077 | Lalitha Ganesan | } |
100 | |||
101 | afa9104d | Priya | void line_follow::spot_turn()
|
102 | { |
||
103 | bool first = true; |
||
104 | 1cb59616 | Hui Jun Tay | float initial_loc = linesensor->readline();
|
105 | float line_loc = initial_loc;
|
||
106 | |||
107 | afa9104d | Priya | do
|
108 | { |
||
109 | motor_l = MOTOR_BASE; |
||
110 | motor_r = -MOTOR_BASE; |
||
111 | |||
112 | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
||
113 | |||
114 | if(first)
|
||
115 | { |
||
116 | 1cb59616 | Hui Jun Tay | init_turn_time.sleep(); |
117 | do
|
||
118 | c840fbe6 | Priya | { |
119 | 1cb59616 | Hui Jun Tay | line_loc = linesensor->readline(); |
120 | c840fbe6 | Priya | } |
121 | 1cb59616 | Hui Jun Tay | while ((abs(line_loc - initial_loc) < 0.0001 || |
122 | (line_loc < -2 || line_loc > 2)) && ok()); |
||
123 | afa9104d | Priya | first = false;
|
124 | } |
||
125 | line_loc = linesensor->readline(); |
||
126 | 1cb59616 | Hui Jun Tay | } |
127 | af7e0f94 | Alex | while(!(-1 < line_loc && line_loc < 1) && ok()); |
128 | 1cb59616 | Hui Jun Tay | //keep repeating until line_loc gets a fresh reading
|
129 | afa9104d | Priya | } |
130 | |||
131 | 58371433 | Priya | void line_follow::turn_right()
|
132 | 9143e077 | Lalitha Ganesan | { |
133 | 58371433 | Priya | bool first = true; |
134 | afa9104d | Priya | float line_loc;
|
135 | 58371433 | Priya | do
|
136 | { |
||
137 | 414d2b48 | Alex | motor_l = 0;
|
138 | 58371433 | Priya | motor_r = -MOTOR_BASE; |
139 | 60b98383 | Priya | |
140 | 58371433 | Priya | motors->set_sides(motor_l, motor_r, MOTOR_ABSOLUTE); |
141 | 60b98383 | Priya | |
142 | 58371433 | Priya | if(first)
|
143 | 60b98383 | Priya | { |
144 | afa9104d | Priya | init_turn_time.sleep(); |
145 | 58371433 | Priya | first = false;
|
146 | 60b98383 | Priya | } |
147 | afa9104d | Priya | |
148 | line_loc = linesensor->readline(); |
||
149 | 60b98383 | Priya | } |
150 | af7e0f94 | Alex | while(!(-1 < line_loc && line_loc < 1) && ok()); |
151 | 9143e077 | Lalitha Ganesan | } |
152 | |||
153 | 1905324e | Priya | void line_follow::u_turn()
|
154 | 9143e077 | Lalitha Ganesan | { |
155 | 1905324e | Priya | turn_right(); |
156 | 58371433 | Priya | follow_line(); |
157 | turn_right(); |
||
158 | 1905324e | Priya | } |
159 | |||
160 | void line_follow::run()
|
||
161 | { |
||
162 | 25694a03 | Priya | while(ok())
|
163 | { |
||
164 | follow_line(); |
||
165 | } |
||
166 | 9143e077 | Lalitha Ganesan | } |