Revision 1c3c96ce
ID | 1c3c96ce8316e2bdeeb5aabaf8be4d184121a91e |
Added and tested range sensor to scout_avr
Max output should be 37500 (max observed value actually ~38600), and it
may be scaled down later. Also, the second sonar will need to be added
later... forgot about that...
Also, added an option to avrdude in the Makefile which makes it upload
amazingly faster :D
scout_avr/Makefile | ||
---|---|---|
14 | 14 |
avr-g++ $(FLAGS) $(SRC) -o scout_avr.elf |
15 | 15 |
|
16 | 16 |
download: scout_avr.hex |
17 |
avrdude -p m328p -c avrispMKII -P usb -F -U flash:w:scout_avr.hex |
|
17 |
avrdude -p m328p -c avrispMKII -P usb -B 1 -F -U flash:w:scout_avr.hex
|
|
18 | 18 |
|
19 | 19 |
clean: |
20 | 20 |
rm -f scout_avr.elf scout_avr.hex |
scout_avr/src/Atmega128rfa1.cpp | ||
---|---|---|
41 | 41 |
{ |
42 | 42 |
// === init serial === |
43 | 43 |
// baud = F_CPU / (16 (UBRR + 1)) |
44 |
uint16_t ubrr = F_CPU / 16 / 38400 - 1;
|
|
44 |
uint16_t ubrr = F_CPU / 16 / BAUD_RATE - 1;
|
|
45 | 45 |
UBRR0H = ubrr >> 8; |
46 | 46 |
UBRR0L = ubrr; |
47 | 47 |
// UMSEL0 = 0, asynchronous usart |
scout_avr/src/Atmega128rfa1.h | ||
---|---|---|
3 | 3 |
|
4 | 4 |
#include "ros/node_handle.h" |
5 | 5 |
|
6 |
#define BAUD_RATE 38400 |
|
7 |
|
|
8 |
#define RX_BUFFER_SIZE 256 |
|
9 |
|
|
6 | 10 |
#define MAX_SUBSCRIBERS 2 |
7 | 11 |
#define MAX_PUBLISHERS 2 |
8 | 12 |
#define INPUT_SIZE 128 |
9 | 13 |
#define OUTPUT_SIZE 128 |
10 | 14 |
|
11 |
#define RX_BUFFER_SIZE 256 |
|
12 |
|
|
13 | 15 |
class Atmega128rfa1 |
14 | 16 |
{ |
15 | 17 |
public: |
scout_avr/src/main.cpp | ||
---|---|---|
37 | 37 |
} |
38 | 38 |
|
39 | 39 |
#include "Atmega128rfa1.h" |
40 |
#include "range.h" |
|
40 | 41 |
|
41 | 42 |
int main() |
42 | 43 |
{ |
43 |
char time[20];
|
|
44 |
char buf[20];
|
|
44 | 45 |
int data; |
45 | 46 |
unsigned long now, next = 0; |
46 | 47 |
Atmega128rfa1 avr; |
47 | 48 |
avr.init(); |
49 |
range_init(); |
|
48 | 50 |
while (1) |
49 | 51 |
{ |
50 | 52 |
now = avr.time(); |
51 | 53 |
if (now > next) { |
52 |
next = now + 500;
|
|
53 |
//ultoa(avr.time(), time, 10);
|
|
54 |
//avr.write((uint8_t*) time, strlen(time));
|
|
55 |
data = avr.read(); |
|
54 |
next = now + 100;
|
|
55 |
utoa(range_get(), buf, 10);
|
|
56 |
avr.write((uint8_t*) buf, strlen(buf));
|
|
57 |
/*data = avr.read();
|
|
56 | 58 |
if (data >= 0) |
57 | 59 |
{ |
58 | 60 |
uint8_t data8 = data; |
... | ... | |
60 | 62 |
avr.write(&data8, 1); |
61 | 63 |
} |
62 | 64 |
else |
63 |
avr.write((uint8_t*) ".", 1); |
|
64 |
avr.write((uint8_t*) "\n\r", 1);
|
|
65 |
avr.write((uint8_t*) ".", 1);*//*
|
|
66 |
avr.write((uint8_t*) "\r\n", 2);
|
|
65 | 67 |
} |
66 | 68 |
} |
67 | 69 |
return 0; |
scout_avr/src/range.cpp | ||
---|---|---|
1 |
#include "range.h" |
|
2 |
|
|
3 |
extern "C" |
|
4 |
{ |
|
5 |
#include <avr/io.h> |
|
6 |
#include <avr/interrupt.h> |
|
7 |
} |
|
8 |
|
|
9 |
/* Ultrasonic Sensor: |
|
10 |
* -if RX pin is left open, it will continuously take readings |
|
11 |
* -PW output is 147us/in. |
|
12 |
* -PW will be high for a maximum of 37.5ms if no target is detected |
|
13 |
* |
|
14 |
* 37.5ms * 8 MHz / 8 prescaler = 37500 max wait |
|
15 |
*/ |
|
16 |
|
|
17 |
// so that we can test on a 328 |
|
18 |
#if defined(__AVR_ATmega128RFA1__) |
|
19 |
# define read_INT0 (PIND & _BV(PD0)) |
|
20 |
#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) |
|
21 |
# define read_INT0 (PIND & _BV(PD2)) |
|
22 |
#else |
|
23 |
# error "Please define read_INT0 for this device" |
|
24 |
#endif |
|
25 |
|
|
26 |
unsigned int range; |
|
27 |
|
|
28 |
ISR(INT0_vect) |
|
29 |
{ |
|
30 |
if (read_INT0) |
|
31 |
{ |
|
32 |
// reset timer |
|
33 |
TCNT1 = 0; |
|
34 |
} |
|
35 |
else |
|
36 |
{ |
|
37 |
range = TCNT1; |
|
38 |
} |
|
39 |
} |
|
40 |
|
|
41 |
void range_init() |
|
42 |
{ |
|
43 |
// ISC0 = 1, edge triggered |
|
44 |
EICRA |= _BV(ISC00); |
|
45 |
// enable INT0 |
|
46 |
EIMSK |= _BV(INT0); |
|
47 |
|
|
48 |
// CS1 = 2, 1/8 prescaler |
|
49 |
TCCR1B = _BV(CS11); |
|
50 |
|
|
51 |
range = -1; |
|
52 |
} |
|
53 |
|
|
54 |
int range_get() |
|
55 |
{ |
|
56 |
int ret; |
|
57 |
cli(); |
|
58 |
ret = range; |
|
59 |
sei(); |
|
60 |
return ret; |
|
61 |
} |
scout_avr/src/range.h | ||
---|---|---|
1 |
#ifndef _RANGE_H_ |
|
2 |
#define _RANGE_H_ |
|
3 |
|
|
4 |
void range_init(); |
|
5 |
int range_get(); |
|
6 |
|
|
7 |
#endif |
Also available in: Unified diff