Revision 53349043
Disabled stepper movement and cleaned up code a bit
scout_avr/src/main.cpp | ||
---|---|---|
1 |
#if 1 /////////////////////////////////////////////// |
|
2 |
|
|
3 | 1 |
#include "ros.h" |
4 | 2 |
#include "bom.h" |
5 | 3 |
#include "range.h" |
... | ... | |
19 | 17 |
|
20 | 18 |
char range_enabled = 0; |
21 | 19 |
|
22 |
void debug(const char *str) |
|
23 |
{ |
|
24 |
} |
|
25 |
|
|
26 | 20 |
void orb_callback(const messages::set_headlights& msg) |
27 | 21 |
{ |
28 | 22 |
orb_set0(msg.left_red, msg.left_green, msg.left_blue); |
... | ... | |
61 | 55 |
// TODO ROS messages to set bounds |
62 | 56 |
step_init(); |
63 | 57 |
step_dir(1); |
64 |
step_set_size(STEP_WHOLE);
|
|
58 |
step_set_size(STEP_HALF);
|
|
65 | 59 |
step_sweep_bounds(-26, 26); |
66 | 60 |
|
67 | 61 |
/* Range */ |
... | ... | |
85 | 79 |
nh.advertise(bom_pub); |
86 | 80 |
|
87 | 81 |
/* Headlights (aka Orbs) */ |
88 |
//orb_init(); |
|
82 |
//orb_init(); TODO debug orbs
|
|
89 | 83 |
ros::Subscriber<messages::set_headlights> orb_sub("set_headlights", |
90 | 84 |
orb_callback); |
91 | 85 |
nh.subscribe(orb_sub); |
... | ... | |
158 | 152 |
|
159 | 153 |
return 0; |
160 | 154 |
} |
161 |
|
|
162 |
#else /////////////////////////////////////////////// |
|
163 |
|
|
164 |
extern "C" |
|
165 |
{ |
|
166 |
#include <stdlib.h> |
|
167 |
#include <string.h> |
|
168 |
#include <avr/io.h> |
|
169 |
#include <util/delay.h> |
|
170 |
} |
|
171 |
|
|
172 |
#include "Atmega128rfa1.h" |
|
173 |
#include "range.h" |
|
174 |
#include "bom.h" |
|
175 |
#include "stepper.h" |
|
176 |
|
|
177 |
Atmega128rfa1 avr; |
|
178 |
|
|
179 |
void debug(const char *str) |
|
180 |
{ |
|
181 |
avr.puts(str); |
|
182 |
} |
|
183 |
|
|
184 |
int main() |
|
185 |
{ |
|
186 |
char buf[20]; |
|
187 |
//int i; |
|
188 |
//char id = 0; |
|
189 |
/*unsigned long now, next = 0; |
|
190 |
unsigned int ranges[2];*/ |
|
191 |
avr.init(); |
|
192 |
range_init(); |
|
193 |
bom_init(); |
|
194 |
func step_sweep_cb = step_init(10); |
|
195 |
step_sweep_bounds(-26, 26); |
|
196 |
step_dir(1); |
|
197 |
step_sweep_speed(50); |
|
198 |
PORTF |= _BV(PF4); |
|
199 |
_delay_ms(500); |
|
200 |
PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5); |
|
201 |
_delay_ms(500); |
|
202 |
PORTF &= ~_BV(PF5); |
|
203 |
while (1) |
|
204 |
{ |
|
205 |
step_sweep_cb(); |
|
206 |
_delay_ms(10); |
|
207 |
/*now = avr.time(); |
|
208 |
if (now > next) { |
|
209 |
next = now + 500;*/ |
|
210 |
/*ultoa(now, buf, 10); |
|
211 |
avr.puts(buf); |
|
212 |
avr.puts("\r\n");*/ |
|
213 |
/*id++; |
|
214 |
if (id == 0x40) { |
|
215 |
id = 0; |
|
216 |
} |
|
217 |
set_robot_id(id);*/ |
|
218 |
/*range_measure(ranges); |
|
219 |
utoa(ranges[0], buf, 10); |
|
220 |
avr.puts("Range: "); |
|
221 |
avr.puts(buf); |
|
222 |
avr.puts(", "); |
|
223 |
utoa(ranges[1], buf, 10); |
|
224 |
avr.puts(buf); |
|
225 |
avr.puts("\r\n");*/ |
|
226 |
/*for (i = 0; i < 4; i++) { |
|
227 |
bom_send(i); |
|
228 |
int msg = bom_get(i); |
|
229 |
if (msg != BOM_NO_DATA) { |
|
230 |
avr.puts("BOM "); |
|
231 |
itoa(i, buf, 10); |
|
232 |
avr.puts(buf); |
|
233 |
avr.puts(": "); |
|
234 |
itoa(bom_msg_get_robot_id(msg), buf, 10); |
|
235 |
avr.puts(buf); |
|
236 |
avr.puts(" ("); |
|
237 |
itoa(bom_msg_get_dir(msg), buf, 10); |
|
238 |
avr.puts(buf); |
|
239 |
avr.puts(")\r\n"); |
|
240 |
} |
|
241 |
}*/ |
|
242 |
//} |
|
243 |
} |
|
244 |
return 0; |
|
245 |
} |
|
246 |
|
|
247 |
#endif ////////////////////////////////////////////// |
Also available in: Unified diff