Revision 31f4a032
Some fixes to BOM, which is now tested and it works
scout_avr/src/bom.cpp | ||
---|---|---|
35 | 35 |
// rx global vars |
36 | 36 |
static uint8_t prev_bom_sig; |
37 | 37 |
static struct bom_rx_t { |
38 |
uint8_t bits_rx;
|
|
38 |
uint8_t count;
|
|
39 | 39 |
uint16_t bits; |
40 |
int last_bit;
|
|
40 |
int last_time;
|
|
41 | 41 |
|
42 | 42 |
uint8_t new_data; |
43 | 43 |
uint8_t address; |
... | ... | |
114 | 114 |
} |
115 | 115 |
|
116 | 116 |
ISR(TIMER4_COMPA_vect) { |
117 |
if (out_counter) { |
|
118 |
if (out_high) { |
|
119 |
stop_38kHz_signal(); |
|
120 |
if (out_msg & 1) { |
|
117 |
if (out_high) { |
|
118 |
stop_38kHz_signal(); |
|
119 |
if (out_counter) { |
|
120 |
out_counter--; |
|
121 |
if ((out_msg >> out_counter) & 1) { |
|
121 | 122 |
OCR4A = TIME_MICROS(1680, 64); |
122 | 123 |
} else { |
123 | 124 |
OCR4A = TIME_MICROS(680, 64); |
124 | 125 |
} |
125 |
out_msg >>= 1; |
|
126 |
out_counter--; |
|
127 | 126 |
} else { |
128 |
start_38kHz_signal();
|
|
129 |
OCR4A = TIME_MICROS(320, 64);
|
|
127 |
stop_data_signal();
|
|
128 |
out_done = 1;
|
|
130 | 129 |
} |
131 | 130 |
} else { |
132 |
stop_data_signal();
|
|
133 |
out_done = 1;
|
|
131 |
start_38kHz_signal();
|
|
132 |
OCR4A = TIME_MICROS(320, 64);
|
|
134 | 133 |
} |
135 | 134 |
} |
136 | 135 |
|
... | ... | |
154 | 153 |
case BOM_RIGHT: out_pin_mask = _BV(BOM_EMIT2); break; |
155 | 154 |
case BOM_BACK: out_pin_mask = _BV(BOM_EMIT3); break; |
156 | 155 |
} |
157 |
out_counter = 17;
|
|
156 |
out_counter = 16;
|
|
158 | 157 |
out_msg = sharp_msg_make(0x2A, bom_msg_make(robot_id, dir)); |
159 | 158 |
out_done = 0; |
160 | 159 |
start_data_signal(); |
... | ... | |
165 | 164 |
|
166 | 165 |
static void recv_edge(char is_rising, struct bom_rx_t *rx) { |
167 | 166 |
if (is_rising) { |
167 |
// TODO check 320us? or have 320us timeout on rising edge? |
|
168 |
} else { |
|
168 | 169 |
// uses timer 1, assuming prescale 1/8 |
169 | 170 |
// timer 1 is set up by range_init() |
170 |
int now = TCNT1 * (F_CPU / 1000000) / 8; |
|
171 |
int diff = now - rx->last_bit; |
|
172 |
rx->last_bit = now; |
|
173 |
rx->bits_rx++; |
|
174 |
rx->bits <<= 1; |
|
175 |
if (MIN_LOW_PW < diff && diff < MAX_LOW_PW) { |
|
176 |
// 0 already in bits |
|
177 |
} else if (MIN_HIGH_PW < diff && diff < MAX_HIGH_PW) { |
|
178 |
// add 1 to bits |
|
179 |
rx->bits |= 1; |
|
180 |
} else { |
|
181 |
// error, start from beginning |
|
182 |
rx->bits_rx = 0; |
|
183 |
rx->bits = 0; |
|
184 |
} |
|
185 |
if (rx->bits_rx == 16) { |
|
186 |
// finished! |
|
187 |
if ((rx->bits & 3) == 2) { // expansion and check bits |
|
188 |
rx->data = (rx->bits >> 2) & 0xff; |
|
189 |
rx->address = (rx->bits >> 10) & 0x1f; |
|
190 |
rx->new_data = 1; |
|
171 |
// this should be in units of microseconds |
|
172 |
int now = TCNT1 * (F_CPU / 1000000 / 8); |
|
173 |
|
|
174 |
if (rx->count) { |
|
175 |
int diff = now - rx->last_time; |
|
176 |
rx->bits <<= 1; |
|
177 |
if (MIN_LOW_PW < diff && diff < MAX_LOW_PW) { |
|
178 |
// 0 already in bits |
|
179 |
} else if (MIN_HIGH_PW < diff && diff < MAX_HIGH_PW) { |
|
180 |
// add 1 to bits |
|
181 |
rx->bits |= 1; |
|
182 |
} else { |
|
183 |
// error, start from beginning |
|
184 |
rx->count = 0; |
|
185 |
rx->bits = 0; |
|
186 |
} |
|
187 |
if (rx->count == 16) { |
|
188 |
// finished! |
|
189 |
//if ((rx->bits & 3) == 2) { // expansion and check bits |
|
190 |
rx->data = (rx->bits >> 2) & 0xff; |
|
191 |
rx->address = (rx->bits >> 10) & 0x1f; |
|
192 |
rx->new_data = 1; |
|
193 |
//} |
|
194 |
rx->count = 0; |
|
195 |
rx->bits = 0; |
|
191 | 196 |
} |
192 |
rx->bits_rx = 0; |
|
193 |
rx->bits = 0; |
|
194 | 197 |
} |
195 |
} else { |
|
196 |
// TODO check 320us? or have 320us timeout on rising edge? |
|
198 |
|
|
199 |
rx->count++; |
|
200 |
rx->last_time = now; |
|
197 | 201 |
} |
198 | 202 |
} |
199 | 203 |
|
scout_avr/src/bom.h | ||
---|---|---|
12 | 12 |
#define BOM_RIGHT 3 |
13 | 13 |
|
14 | 14 |
// timing, in us, for read of valid bits |
15 |
#define MIN_LOW_PW 800
|
|
16 |
#define MAX_LOW_PW 1200
|
|
17 |
#define MIN_HIGH_PW 1800
|
|
18 |
#define MAX_HIGH_PW 2200
|
|
15 |
#define MIN_LOW_PW 600
|
|
16 |
#define MAX_LOW_PW 1400
|
|
17 |
#define MIN_HIGH_PW 1600
|
|
18 |
#define MAX_HIGH_PW 2400
|
|
19 | 19 |
|
20 | 20 |
// returned by bom_get if there is no new data since last call |
21 | 21 |
#define BOM_NO_DATA -1 |
22 | 22 |
|
23 | 23 |
// i/o pins |
24 | 24 |
// if these are changed, remember to change bom_init and/or ISRs |
25 |
#define BOM_SIG PORTB
|
|
25 |
#define BOM_SIG PINB
|
|
26 | 26 |
#define BOM_SIG0 PB0 |
27 | 27 |
#define BOM_SIG1 PB1 |
28 | 28 |
#define BOM_SIG2 PB2 |
scout_avr/src/debug.h | ||
---|---|---|
1 |
#ifndef _DEBUG_H |
|
2 |
#define _DEBUG_H |
|
3 |
|
|
4 |
void debug(const char *str); |
|
5 |
|
|
6 |
#endif |
scout_avr/src/main.cpp | ||
---|---|---|
5 | 5 |
|
6 | 6 |
ros::Publisher *ptest_out; |
7 | 7 |
|
8 |
void debug(const char *str) { |
|
9 |
} |
|
10 |
|
|
8 | 11 |
void callback(const std_msgs::Int16& msg) |
9 | 12 |
{ |
10 | 13 |
ptest_out->publish(&msg); |
... | ... | |
44 | 47 |
#include "range.h" |
45 | 48 |
#include "bom.h" |
46 | 49 |
|
50 |
Atmega128rfa1 avr; |
|
51 |
|
|
52 |
void debug(const char *str) { |
|
53 |
avr.puts(str); |
|
54 |
} |
|
55 |
|
|
47 | 56 |
int main() |
48 | 57 |
{ |
49 | 58 |
char buf[20]; |
50 | 59 |
int i; |
60 |
char id = 0; |
|
51 | 61 |
unsigned long now, next = 0; |
52 |
Atmega128rfa1 avr; |
|
53 | 62 |
avr.init(); |
54 | 63 |
range_init(); |
55 | 64 |
bom_init(); |
56 |
set_robot_id(0x2a);
|
|
65 |
avr.puts("Hello!\r\n");
|
|
57 | 66 |
while (1) |
58 | 67 |
{ |
59 | 68 |
now = avr.time(); |
60 | 69 |
if (now > next) { |
61 | 70 |
next = now + 100; |
71 |
id++; |
|
72 |
if (id == 0x40) { |
|
73 |
id = 0; |
|
74 |
} |
|
75 |
set_robot_id(id); |
|
62 | 76 |
/*utoa(range_get(0), buf, 10); |
63 | 77 |
avr.puts("Range: "); |
64 | 78 |
avr.puts(buf); |
... | ... | |
73 | 87 |
itoa(i, buf, 10); |
74 | 88 |
avr.puts(buf); |
75 | 89 |
avr.puts(": "); |
76 |
itoa(msg, buf, 10); |
|
90 |
itoa(bom_msg_get_robot_id(msg), buf, 10); |
|
91 |
avr.puts(buf); |
|
92 |
avr.puts(" ("); |
|
93 |
itoa(bom_msg_get_dir(msg), buf, 10); |
|
77 | 94 |
avr.puts(buf); |
95 |
avr.puts(")\r\n"); |
|
78 | 96 |
} |
79 | 97 |
} |
80 |
avr.puts("\r\n"); |
|
81 | 98 |
} |
82 | 99 |
} |
83 | 100 |
return 0; |
scout_avr/src/range.cpp | ||
---|---|---|
33 | 33 |
static void on_edge(int which) |
34 | 34 |
{ |
35 | 35 |
unsigned char int_high; |
36 |
// TODO centralize timer 1 |
|
37 |
// TODO ensure this is in microseconds even for 16MHz |
|
36 | 38 |
unsigned int time = TCNT1; |
37 | 39 |
|
38 | 40 |
if (which) |
Also available in: Unified diff