Revision 2853d46b
Changed stepper to always step in step_sweep
Also added whole/half step as a separate setting, with a single
step_do_step function. Also added stepper to main.cpp.
scout_avr/src/main.cpp | ||
---|---|---|
12 | 12 |
#include <util/delay.h> |
13 | 13 |
|
14 | 14 |
/* Period of main loop in ms */ |
15 |
#define MAINLOOP_PERIOD 1000//50
|
|
15 |
#define MAINLOOP_PERIOD 100//50 |
|
16 | 16 |
|
17 | 17 |
char range_enabled = 0; |
18 | 18 |
|
... | ... | |
52 | 52 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback); |
53 | 53 |
nh.subscribe(test_in); |
54 | 54 |
|
55 |
/* Stepper */ |
|
56 |
// TODO ROS messages to set bounds |
|
57 |
step_init(); |
|
58 |
step_dir(1); |
|
59 |
step_set_size(STEP_WHOLE); |
|
60 |
step_sweep_bounds(-26, 26); |
|
61 |
|
|
55 | 62 |
/* Range */ |
56 | 63 |
range_init(); |
57 | 64 |
sonar::sonar_distance range_msg; |
... | ... | |
105 | 112 |
} |
106 | 113 |
|
107 | 114 |
/* Stepper / range sensor */ |
108 |
// TODO sweep stepper |
|
109 |
range_measure(ranges); |
|
110 |
range_msg.header.stamp = nh.now(); |
|
111 |
range_msg.header.seq++; |
|
112 |
range_msg.pos = 0; // TODO fill this |
|
113 |
range_msg.distance0 = ranges[0]; |
|
114 |
range_msg.distance1 = ranges[1]; |
|
115 |
range_pub.publish(&range_msg); |
|
115 |
if (range_enabled) { |
|
116 |
step_sweep(); |
|
117 |
range_measure(ranges); |
|
118 |
range_msg.header.stamp = nh.now(); |
|
119 |
range_msg.header.seq++; |
|
120 |
range_msg.pos = 0; // TODO fill this |
|
121 |
range_msg.distance0 = ranges[0]; |
|
122 |
range_msg.distance1 = ranges[1]; |
|
123 |
range_pub.publish(&range_msg); |
|
124 |
} |
|
116 | 125 |
|
117 | 126 |
} |
118 | 127 |
|
... | ... | |
146 | 155 |
char buf[20]; |
147 | 156 |
//int i; |
148 | 157 |
//char id = 0; |
149 |
unsigned long now, next = 0; |
|
150 |
unsigned int ranges[2]; |
|
158 |
/*unsigned long now, next = 0;
|
|
159 |
unsigned int ranges[2];*/
|
|
151 | 160 |
avr.init(); |
152 | 161 |
range_init(); |
153 | 162 |
bom_init(); |
154 |
/*func step_sweep_cb = step_init(10);
|
|
163 |
func step_sweep_cb = step_init(10); |
|
155 | 164 |
step_sweep_bounds(-26, 26); |
156 | 165 |
step_dir(1); |
157 |
step_sweep_speed(50);*/ |
|
158 |
avr.puts("Hello!\r\n"); |
|
166 |
step_sweep_speed(50); |
|
167 |
PORTF |= _BV(PF4); |
|
168 |
_delay_ms(500); |
|
169 |
PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5); |
|
170 |
_delay_ms(500); |
|
171 |
PORTF &= ~_BV(PF5); |
|
159 | 172 |
while (1) |
160 | 173 |
{ |
161 |
/*step_sweep_cb();
|
|
162 |
_delay_ms(10);*/
|
|
163 |
now = avr.time(); |
|
174 |
step_sweep_cb(); |
|
175 |
_delay_ms(10); |
|
176 |
/*now = avr.time();
|
|
164 | 177 |
if (now > next) { |
165 |
next = now + 500; |
|
166 |
//step_sweep_cb(); |
|
178 |
next = now + 500;*/ |
|
167 | 179 |
/*ultoa(now, buf, 10); |
168 | 180 |
avr.puts(buf); |
169 | 181 |
avr.puts("\r\n");*/ |
... | ... | |
172 | 184 |
id = 0; |
173 | 185 |
} |
174 | 186 |
set_robot_id(id);*/ |
175 |
range_measure(ranges); |
|
187 |
/*range_measure(ranges);
|
|
176 | 188 |
utoa(ranges[0], buf, 10); |
177 | 189 |
avr.puts("Range: "); |
178 | 190 |
avr.puts(buf); |
179 | 191 |
avr.puts(", "); |
180 | 192 |
utoa(ranges[1], buf, 10); |
181 | 193 |
avr.puts(buf); |
182 |
avr.puts("\r\n"); |
|
194 |
avr.puts("\r\n");*/
|
|
183 | 195 |
/*for (i = 0; i < 4; i++) { |
184 | 196 |
bom_send(i); |
185 | 197 |
int msg = bom_get(i); |
... | ... | |
196 | 208 |
avr.puts(")\r\n"); |
197 | 209 |
} |
198 | 210 |
}*/ |
199 |
} |
|
211 |
//}
|
|
200 | 212 |
} |
201 | 213 |
return 0; |
202 | 214 |
} |
Also available in: Unified diff