Revision 807483bf
ID | 807483bfcb4c1490e8ef4794cf6c19b93211894a |
Fixed range sensor code to support 2
Also, tested (with only one sensor) and it seemed to work adequately.
scout_avr/Makefile | ||
---|---|---|
1 |
|
|
2 |
#PART=m128rfa1 |
|
1 | 3 |
#MCU=atmega128rfa1 |
2 |
MCU=atmega328 |
|
4 |
|
|
5 |
#PART=m328p |
|
6 |
#MCU=atmega328 |
|
7 |
|
|
8 |
PART=m2560 |
|
9 |
MCU=atmega2560 |
|
10 |
|
|
11 |
#PROG=avrispMKII |
|
12 |
PROG=stk600 |
|
13 |
|
|
3 | 14 |
F_CPU=8000000 |
4 | 15 |
SRC=src/*.cpp src/ros_lib/*.cpp |
5 | 16 |
HDR=src/*.h |
... | ... | |
14 | 25 |
avr-g++ $(FLAGS) $(SRC) -o scout_avr.elf |
15 | 26 |
|
16 | 27 |
download: scout_avr.hex |
17 |
avrdude -p m328p -c avrispMKII -P usb -B 1 -F -U flash:w:scout_avr.hex
|
|
28 |
avrdude -p $(PART) -c $(PROG) -P usb -B 1 -F -U flash:w:scout_avr.hex
|
|
18 | 29 |
|
19 | 30 |
clean: |
20 | 31 |
rm -f scout_avr.elf scout_avr.hex |
scout_avr/src/Atmega128rfa1.cpp | ||
---|---|---|
21 | 21 |
millis++; |
22 | 22 |
} |
23 | 23 |
|
24 |
ISR(USART_RX_vect) |
|
24 |
ISR(USART0_RX_vect)
|
|
25 | 25 |
{ |
26 | 26 |
char data = UDR0; |
27 | 27 |
if (rx_end == rx_start-1 || (rx_start == 0 && rx_end == RX_BUFFER_SIZE-1)) |
scout_avr/src/main.cpp | ||
---|---|---|
1 |
#if 0 /////////////////////////////////////////////// |
|
2 |
|
|
1 | 3 |
#include "ros.h" |
2 | 4 |
#include <std_msgs/Int16.h> |
3 | 5 |
|
... | ... | |
30 | 32 |
return 0; |
31 | 33 |
} |
32 | 34 |
|
33 |
/*extern "C" |
|
35 |
#else /////////////////////////////////////////////// |
|
36 |
|
|
37 |
extern "C" |
|
34 | 38 |
{ |
35 | 39 |
#include <stdlib.h> |
36 | 40 |
#include <string.h> |
... | ... | |
42 | 46 |
int main() |
43 | 47 |
{ |
44 | 48 |
char buf[20]; |
45 |
int data; |
|
46 | 49 |
unsigned long now, next = 0; |
47 | 50 |
Atmega128rfa1 avr; |
48 | 51 |
avr.init(); |
... | ... | |
54 | 57 |
next = now + 100; |
55 | 58 |
utoa(range_get(0), buf, 10); |
56 | 59 |
avr.write((uint8_t*) buf, strlen(buf)); |
57 |
/*data = avr.read(); |
|
60 |
avr.write((uint8_t*) ", ", 2); |
|
61 |
utoa(range_get(1), buf, 10); |
|
62 |
avr.write((uint8_t*) buf, strlen(buf)); |
|
63 |
/*int data = avr.read(); |
|
58 | 64 |
if (data >= 0) |
59 | 65 |
{ |
60 | 66 |
uint8_t data8 = data; |
... | ... | |
62 | 68 |
avr.write(&data8, 1); |
63 | 69 |
} |
64 | 70 |
else |
65 |
avr.write((uint8_t*) ".", 1);*//*
|
|
71 |
avr.write((uint8_t*) ".", 1);*/ |
|
66 | 72 |
avr.write((uint8_t*) "\r\n", 2); |
67 | 73 |
} |
68 | 74 |
} |
69 | 75 |
return 0; |
70 | 76 |
} |
71 |
*/ |
|
77 |
|
|
78 |
#endif ////////////////////////////////////////////// |
scout_avr/src/range.cpp | ||
---|---|---|
14 | 14 |
* 37.5ms * 8 MHz / 8 prescaler = 37500 max wait |
15 | 15 |
*/ |
16 | 16 |
|
17 |
// so that we can test on a 328 |
|
18 |
#if defined(__AVR_ATmega128RFA1__) |
|
17 |
// so that we can test on a 328 or the stk600
|
|
18 |
#if defined(__AVR_ATmega128RFA1__) || defined(__AVR_ATmega2560__)
|
|
19 | 19 |
# define read_INT0 (PIND & _BV(PD0)) |
20 | 20 |
# define read_INT1 (PIND & _BV(PD1)) |
21 | 21 |
#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) |
... | ... | |
32 | 32 |
|
33 | 33 |
static void on_edge(int which) |
34 | 34 |
{ |
35 |
unsigned char int_high; |
|
35 | 36 |
unsigned int time = TCNT1; |
36 |
if (read_INT0) |
|
37 |
|
|
38 |
if (which) |
|
39 |
{ |
|
40 |
int_high = read_INT1; |
|
41 |
} |
|
42 |
else |
|
43 |
{ |
|
44 |
int_high = read_INT0; |
|
45 |
} |
|
46 |
|
|
47 |
if (int_high) |
|
37 | 48 |
{ |
38 | 49 |
range[which].start = time; |
39 | 50 |
} |
Also available in: Unified diff