Revision fd73d758
| scout_avr/src/main.cpp | ||
|---|---|---|
| 1 |
#if 1 ///////////////////////////////////////////////
|
|
| 1 |
#if 0 ///////////////////////////////////////////////
|
|
| 2 | 2 |
|
| 3 | 3 |
#include "ros.h" |
| 4 | 4 |
#include <std_msgs/Int16.h> |
| ... | ... | |
| 42 | 42 |
{
|
| 43 | 43 |
#include <stdlib.h> |
| 44 | 44 |
#include <string.h> |
| 45 |
#include <avr/io.h> |
|
| 46 |
#include <util/delay.h> |
|
| 45 | 47 |
} |
| 46 | 48 |
|
| 47 | 49 |
#include "Atmega128rfa1.h" |
| ... | ... | |
| 70 | 72 |
now = avr.time(); |
| 71 | 73 |
if (now > next) {
|
| 72 | 74 |
next = now + 100; |
| 73 |
ultoa(now, buf, 10); |
|
| 75 |
/*ultoa(now, buf, 10);
|
|
| 74 | 76 |
avr.puts(buf); |
| 75 |
avr.puts("\r\n");
|
|
| 77 |
avr.puts("\r\n");*/
|
|
| 76 | 78 |
id++; |
| 77 | 79 |
if (id == 0x40) {
|
| 78 | 80 |
id = 0; |
| ... | ... | |
| 84 | 86 |
avr.puts(", ");
|
| 85 | 87 |
utoa(range_get(1), buf, 10); |
| 86 | 88 |
avr.puts(buf);*/ |
| 87 |
/*for (i = 0; i < 4; i++) {
|
|
| 89 |
for (i = 0; i < 4; i++) {
|
|
| 88 | 90 |
bom_send(i); |
| 89 | 91 |
int msg = bom_get(i); |
| 90 | 92 |
if (msg != BOM_NO_DATA) {
|
| ... | ... | |
| 99 | 101 |
avr.puts(buf); |
| 100 | 102 |
avr.puts(")\r\n");
|
| 101 | 103 |
} |
| 102 |
}*/
|
|
| 104 |
} |
|
| 103 | 105 |
} |
| 104 | 106 |
} |
| 105 | 107 |
return 0; |
| scout_avr/src/range.cpp | ||
|---|---|---|
| 17 | 17 |
struct range_t {
|
| 18 | 18 |
unsigned int start; // timer value on rising edge |
| 19 | 19 |
unsigned int value; // last measured range |
| 20 |
} range[2]; |
|
| 20 |
char done; |
|
| 21 |
} volatile range[2]; |
|
| 21 | 22 |
|
| 22 | 23 |
static void on_edge(int which) |
| 23 | 24 |
{
|
| ... | ... | |
| 39 | 40 |
} |
| 40 | 41 |
else |
| 41 | 42 |
{
|
| 43 |
PORT_SONAR_TX &= ~ _BV(P_SONAR_TX); |
|
| 42 | 44 |
// if timer overflowed since start, this arithmetic should still work out |
| 43 | 45 |
range[which].value = time - range[which].start; |
| 46 |
range[which].done = 1; |
|
| 44 | 47 |
} |
| 45 | 48 |
} |
| 46 | 49 |
|
| ... | ... | |
| 64 | 67 |
// CS1 = 2, 1/8 prescaler |
| 65 | 68 |
// if this is changed, remember to change recv_edge in bom.cpp! |
| 66 | 69 |
TCCR5B = _BV(CS51); |
| 67 |
|
|
| 68 |
range[0].value = RANGE_ERR; |
|
| 69 |
range[1].value = RANGE_ERR; |
|
| 70 |
|
|
| 71 |
// set tx as output |
|
| 72 |
DDRG |= _BV(DDG1); |
|
| 73 |
PORT_SONAR_TX &= ~ _BV(P_SONAR_TX); |
|
| 70 | 74 |
} |
| 71 | 75 |
|
| 72 |
unsigned int range_get(int which)
|
|
| 76 |
void range_measure(unsigned int *values)
|
|
| 73 | 77 |
{
|
| 74 |
unsigned int ret; |
|
| 75 |
if (0 <= which && which <= 1) |
|
| 78 |
int i; |
|
| 79 |
|
|
| 80 |
for (i = 0; i < 2; i++) |
|
| 81 |
{
|
|
| 82 |
range[i].value = RANGE_ERR; |
|
| 83 |
range[i].done = 0; |
|
| 84 |
} |
|
| 85 |
|
|
| 86 |
// TODO ensure that one interrupt won't be delayed because of the other |
|
| 87 |
PORT_SONAR_TX |= _BV(P_SONAR_TX); |
|
| 88 |
|
|
| 89 |
for (i = 0; i < 2; i++) |
|
| 76 | 90 |
{
|
| 77 |
cli(); |
|
| 78 |
ret = range[which].value; |
|
| 79 |
sei(); |
|
| 80 |
return ret; |
|
| 91 |
while (!range[i].done) {}
|
|
| 92 |
values[i] = range[i].value; |
|
| 81 | 93 |
} |
| 82 |
else return RANGE_ERR; |
|
| 83 | 94 |
} |
| scout_avr/src/range.h | ||
|---|---|---|
| 10 | 10 |
#define PORT_SONAR_TX PORTG |
| 11 | 11 |
#define P_SONAR_TX PG1 |
| 12 | 12 |
|
| 13 |
// initializes timer 5, also used by bom |
|
| 13 | 14 |
void range_init(); |
| 14 |
unsigned int range_get(int which); |
|
| 15 |
|
|
| 16 |
// blocks during measurement (up to 37.5ms for each) |
|
| 17 |
// writes values into array of 2 unsigned ints |
|
| 18 |
void range_measure(unsigned int *values); |
|
| 15 | 19 |
|
| 16 | 20 |
#endif |
Also available in: Unified diff