Revision 812788aa

View differences:

scout_avr/Makefile
11 11
#PROG=avrispMKII
12 12
PROG=stk600
13 13

  
14
F_CPU=8000000
14
F_CPU=16000000
15 15
SRC=src/*.cpp src/ros_lib/*.cpp
16 16
HDR=src/*.h
17 17
FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall
scout_avr/src/Atmega128rfa1.cpp
1 1
#include "Atmega128rfa1.h"
2
#include "bom.h"
2 3

  
3 4
extern "C"
4 5
{
......
7 8
  void __cxa_pure_virtual(void) {}
8 9
}
9 10

  
11
#define T0_KHZ 76
12
#define T0_OCR (F_CPU / 1000 / T0_KHZ) // 210 ticks
13
#define T0_ERROR (F_CPU / 1000 - T0_OCR*T0_KHZ) // 40 ticks/ms
14

  
15
unsigned char t0_count, t0_error;
10 16
unsigned long millis;
11 17

  
12 18
int rx_start = 0, rx_end = 0;
......
18 24

  
19 25
ISR(TIMER0_COMPA_vect)
20 26
{
21
  millis++;
27
  bom_isr();
28

  
29
  // F_CPU = T0_OCR * T0_KHZ + T0_ERROR
30
  // every millisecond, accumulate T0_ERROR, and when it reaches T0_OCR skip
31
  // one iteration
32
  t0_count++;
33
  if (t0_count >= T0_KHZ) {
34
    t0_error += T0_ERROR;
35
    if (t0_error < T0_OCR) {
36
      t0_count = 0;
37
    } else {
38
      t0_count = -1;
39
      t0_error -= T0_OCR;
40
    }
41
    millis++;
42
  }
22 43
}
23 44

  
24 45
ISR(USART0_RX_vect)
......
54 75
  // === init time ===
55 76
  // COM0x = 0, pin OC0x not used
56 77
  // WGM0 = 2, clear timer on compare match, TOP = OCRA
57
  // CS0 = 3, 64 prescaler
78
  // CS0 = 1, no prescaler
58 79
  TCCR0A = _BV(WGM01);
59
  TCCR0B = _BV(CS01) | _BV(CS00);
80
  TCCR0B = _BV(CS00);
60 81
  // enable interrupt on compare match A
61 82
  TIMSK0 = _BV(OCIE0A);
62
  // 1 ms with 1/64 prescaler
63
  OCR0A = F_CPU / 1000 / 64;
83
  OCR0A = T0_OCR;
64 84
  millis = 0;
65 85

  
66 86
  sei();
scout_avr/src/bom.cpp
12 12
 *   0: 1ms period
13 13
 *  Signal:
14 14
 *   38kHz pulse for 320us, then off for rest of time (680us or 1680us)
15
 * BOM uses timer 4 for period timing, and timer 3 for 38kHz signal
15
 * BOM uses timer 4 for period timing, and timer 0 for 38kHz signal
16 16
 */
17 17

  
18
#define TIME_KHZ(khz, prescale) (F_CPU / 1000 / (khz) / (prescale))
19 18
#define TIME_MICROS(us, prescale) (F_CPU / 1000000 * (us) / (prescale))
20 19

  
21 20
typedef uint16_t sharp_msg_t;
......
52 51
  return robot_id;
53 52
}
54 53

  
55
ISR(TIMER3_COMPA_vect) {
54
void bom_isr() {
56 55
  if (out_high) {
57 56
    BOM_EMIT ^= out_pin_mask;
58 57
  }
......
62 61
  out_high = 0;
63 62
  out_pin_mask = 0;
64 63
  
65
  // timer 3 mode CTC (clear timer on compare), TOP = OCRA
66
  TCCR3A = 0;
67
  TCCR3B = _BV(WGM32);
68
  TCCR3C = 0;
69
  
70
  // toggling at 38kHz * 2 gives 38kHz wave
71
  OCR3A = TIME_KHZ(38 * 2, 1);
72
  
73
  // enable interrupt
74
  TIMSK3 = _BV(OCIE3A);
75
  
76
  // start timer 3 at F_CPU, no prescaling
77
  TCCR3B |= _BV(CS30);
64
  // timer configuration now done in Atmega128rfa1.cpp
78 65
}
79 66

  
80 67
static void start_38kHz_signal() {
......
92 79
  // timer 4 mode CTC (clear timer on compare), TOP = OCRA
93 80
  TCCR4A = 0;
94 81
  TCCR4B = _BV(WGM42);
95
  TCCR3C = 0;
96 82
  
97 83
  // run interrupt immediately when timer started
98 84
  OCR4A = 0;
......
166 152
  if (is_rising) {
167 153
    // TODO check 320us? or have 320us timeout on rising edge?
168 154
  } else {
169
    // uses timer 1, assuming prescale 1/8
170
    // timer 1 is set up by range_init()
155
    // uses timer 5, assuming prescale 1/8
156
    // timer 5 is set up by range_init()
171 157
    // this should be in units of microseconds
172
    int now = TCNT1 * (F_CPU / 1000000 / 8);
158
    int now = TCNT5 / (F_CPU / 8 / 1000000);
173 159
    
174 160
    if (rx->count) {
175 161
      int diff = now - rx->last_time;
scout_avr/src/bom.h
53 53
void bom_send(char dir);
54 54
int bom_get(char dir);
55 55

  
56
// toggles output - should be called at around 76 kHz
57
void bom_isr();
58

  
56 59
#endif
scout_avr/src/main.cpp
5 5

  
6 6
ros::Publisher *ptest_out;
7 7

  
8
void debug(const char *str) {
8
void debug(const char *str)
9
{
9 10
}
10 11

  
11 12
void callback(const std_msgs::Int16& msg)
......
49 50

  
50 51
Atmega128rfa1 avr;
51 52

  
52
void debug(const char *str) {
53
void debug(const char *str)
54
{
53 55
  avr.puts(str);
54 56
}
55 57

  
......
68 70
    now = avr.time();
69 71
    if (now > next) {
70 72
      next = now + 100;
73
      ultoa(now, buf, 10);
74
      avr.puts(buf);
75
      avr.puts("\r\n");
71 76
      id++;
72 77
      if (id == 0x40) {
73 78
        id = 0;
......
79 84
      avr.puts(", ");
80 85
      utoa(range_get(1), buf, 10);
81 86
      avr.puts(buf);*/
82
      for (i = 0; i < 4; i++) {
87
      /*for (i = 0; i < 4; i++) {
83 88
        bom_send(i);
84 89
        int msg = bom_get(i);
85 90
        if (msg != BOM_NO_DATA) {
......
94 99
          avr.puts(buf);
95 100
          avr.puts(")\r\n");
96 101
        }
97
      }
102
      }*/
98 103
    }
99 104
  }
100 105
  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
36
  // TODO centralize timer 5
37 37
  // TODO ensure this is in microseconds even for 16MHz
38
  unsigned int time = TCNT1;
38
  unsigned int time = TCNT5;
39 39
  
40 40
  if (which)
41 41
  {
......
76 76
  
77 77
  // CS1 = 2, 1/8 prescaler
78 78
  // if this is changed, remember to change recv_edge in bom.cpp!
79
  TCCR1B = _BV(CS11);
79
  TCCR5B = _BV(CS51);
80 80
  
81 81
  range[0].value = RANGE_ERR;
82 82
  range[1].value = RANGE_ERR;

Also available in: Unified diff