Revision 86b48573
| ID | 86b48573fdb86a6acb04eb3a1e9de80b22f0791e |
Range sensors work now! :D
| 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 50 |
|
| 15 |
#define MAINLOOP_PERIOD 1000//50
|
|
| 16 | 16 |
|
| 17 | 17 |
char range_enabled = 0; |
| 18 | 18 |
|
| ... | ... | |
| 55 | 55 |
/* Range */ |
| 56 | 56 |
range_init(); |
| 57 | 57 |
sonar::sonar_distance range_msg; |
| 58 |
ros::Publisher range_distance("sonar_distance", &range_msg);
|
|
| 58 |
ros::Publisher range_pub("sonar_distance", &range_msg);
|
|
| 59 | 59 |
ros::ServiceServer |
| 60 | 60 |
<sonar::sonar_toggleRequest, sonar::sonar_toggleResponse> |
| 61 | 61 |
range_toggle("sonar_toggle", range_toggle_cb);
|
| 62 | 62 |
ros::ServiceServer |
| 63 | 63 |
<sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse> |
| 64 | 64 |
range_set_scan("sonar_set_scan", range_set_scan_cb);
|
| 65 |
nh.advertise(range_pub); |
|
| 65 | 66 |
|
| 66 | 67 |
/* BOM */ |
| 67 | 68 |
bom_init(); |
| ... | ... | |
| 111 | 112 |
range_msg.pos = 0; // TODO fill this |
| 112 | 113 |
range_msg.distance0 = ranges[0]; |
| 113 | 114 |
range_msg.distance1 = ranges[1]; |
| 115 |
range_pub.publish(&range_msg); |
|
| 114 | 116 |
|
| 115 | 117 |
} |
| 116 | 118 |
|
| ... | ... | |
| 142 | 144 |
int main() |
| 143 | 145 |
{
|
| 144 | 146 |
char buf[20]; |
| 145 |
int i; |
|
| 146 |
char id = 0; |
|
| 147 |
//int i;
|
|
| 148 |
//char id = 0;
|
|
| 147 | 149 |
unsigned long now, next = 0; |
| 150 |
unsigned int ranges[2]; |
|
| 148 | 151 |
avr.init(); |
| 149 | 152 |
range_init(); |
| 150 | 153 |
bom_init(); |
| 151 |
func step_sweep_cb = step_init(10); |
|
| 154 |
/*func step_sweep_cb = step_init(10);
|
|
| 152 | 155 |
step_sweep_bounds(-26, 26); |
| 153 | 156 |
step_dir(1); |
| 154 |
step_sweep_speed(50); |
|
| 157 |
step_sweep_speed(50);*/
|
|
| 155 | 158 |
avr.puts("Hello!\r\n");
|
| 156 | 159 |
while (1) |
| 157 | 160 |
{
|
| 158 |
step_sweep_cb(); |
|
| 159 |
_delay_ms(10); |
|
| 160 |
/*now = avr.time();
|
|
| 161 |
/*step_sweep_cb();
|
|
| 162 |
_delay_ms(10);*/
|
|
| 163 |
now = avr.time(); |
|
| 161 | 164 |
if (now > next) {
|
| 162 |
next = now + 50;*/
|
|
| 165 |
next = now + 500;
|
|
| 163 | 166 |
//step_sweep_cb(); |
| 164 | 167 |
/*ultoa(now, buf, 10); |
| 165 | 168 |
avr.puts(buf); |
| ... | ... | |
| 169 | 172 |
id = 0; |
| 170 | 173 |
} |
| 171 | 174 |
set_robot_id(id);*/ |
| 172 |
/*utoa(range_get(0), buf, 10); |
|
| 175 |
range_measure(ranges); |
|
| 176 |
utoa(ranges[0], buf, 10); |
|
| 173 | 177 |
avr.puts("Range: ");
|
| 174 | 178 |
avr.puts(buf); |
| 175 | 179 |
avr.puts(", ");
|
| 176 |
utoa(range_get(1), buf, 10); |
|
| 177 |
avr.puts(buf);*/ |
|
| 180 |
utoa(ranges[1], buf, 10); |
|
| 181 |
avr.puts(buf); |
|
| 182 |
avr.puts("\r\n");
|
|
| 178 | 183 |
/*for (i = 0; i < 4; i++) {
|
| 179 | 184 |
bom_send(i); |
| 180 | 185 |
int msg = bom_get(i); |
| ... | ... | |
| 191 | 196 |
avr.puts(")\r\n");
|
| 192 | 197 |
} |
| 193 | 198 |
}*/ |
| 194 |
//}
|
|
| 199 |
} |
|
| 195 | 200 |
} |
| 196 | 201 |
return 0; |
| 197 | 202 |
} |
| scout_avr/src/range.cpp | ||
|---|---|---|
| 14 | 14 |
* 37.5ms * 8 MHz / 8 prescaler = 37500 max wait |
| 15 | 15 |
* 37.5ms * 16 MHz / 8 prescaler = problem |
| 16 | 16 |
* 37.5ms * 16 MHz / 64 prescaler = 9375 max wait |
| 17 |
* 147us/in * 16 MHz / 64 prescaler = 1.44685039 ticks / mm |
|
| 17 | 18 |
*/ |
| 18 | 19 |
|
| 19 | 20 |
struct range_t {
|
| 20 | 21 |
unsigned int start; // timer value on rising edge |
| 21 | 22 |
unsigned int value; // last measured range |
| 22 |
char started; |
|
| 23 |
char done; |
|
| 23 |
char busy; |
|
| 24 | 24 |
} volatile range[2]; |
| 25 | 25 |
|
| 26 | 26 |
static void on_edge(int which) |
| ... | ... | |
| 40 | 40 |
if (int_high) |
| 41 | 41 |
{
|
| 42 | 42 |
range[which].start = time; |
| 43 |
range[which].started = 1;
|
|
| 43 |
range[which].busy = 1;
|
|
| 44 | 44 |
} |
| 45 | 45 |
else |
| 46 | 46 |
{
|
| 47 | 47 |
// if timer overflowed since start, this arithmetic should still work out |
| 48 | 48 |
range[which].value = time - range[which].start; |
| 49 |
range[which].done = 1;
|
|
| 49 |
range[which].busy = 0;
|
|
| 50 | 50 |
} |
| 51 | 51 |
} |
| 52 | 52 |
|
| ... | ... | |
| 83 | 83 |
for (i = 0; i < 2; i++) |
| 84 | 84 |
{
|
| 85 | 85 |
range[i].value = RANGE_ERR; |
| 86 |
range[i].started = 0; |
|
| 87 |
range[i].done = 0; |
|
| 86 |
range[i].busy = 0; |
|
| 88 | 87 |
} |
| 89 | 88 |
|
| 90 | 89 |
// TODO ensure that one interrupt won't be delayed because of the other |
| 91 | 90 |
PORT_SONAR_TX |= _BV(P_SONAR_TX); |
| 92 |
_delay_ms(1);
|
|
| 91 |
_delay_ms(40);
|
|
| 93 | 92 |
PORT_SONAR_TX &= ~ _BV(P_SONAR_TX); |
| 94 | 93 |
|
| 95 | 94 |
for (i = 0; i < 2; i++) |
| 96 | 95 |
{
|
| 97 |
if (range[i].started) {
|
|
| 98 |
while (!range[i].done) {}
|
|
| 99 |
values[i] = range[i].value; |
|
| 100 |
} |
|
| 96 |
while (range[i].busy) {}
|
|
| 97 |
values[i] = range[i].value; |
|
| 101 | 98 |
} |
| 102 | 99 |
} |
Also available in: Unified diff