Statistics
| Branch: | Revision:

root / scout_avr / src / bom.cpp @ 399f7d1b

History | View | Annotate | Download (4.85 KB)

1 f115416e Tom Mullins
extern "C" {
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
4
#include <util/delay.h>
5
}
6
#include "bom.h"
7
8
/*
9
 * Sharp protocol:
10
 *  Period modulation
11
 *   1: 2ms period
12
 *   0: 1ms period
13
 *  Signal:
14
 *   38kHz pulse for 320us, then off for rest of time (680us or 1680us)
15 812788aa Tom Mullins
 * BOM uses timer 4 for period timing, and timer 0 for 38kHz signal
16 f115416e Tom Mullins
 */
17
18
#define TIME_MICROS(us, prescale) (F_CPU / 1000000 * (us) / (prescale))
19
20
typedef uint16_t sharp_msg_t;
21
22
static sharp_msg_t sharp_msg_make(char address, bom_msg_t bom_msg) {
23
  return (((uint16_t)address & 0x3F) << 10) | ((uint16_t)bom_msg << 2) | 2;
24
}
25
26
static char robot_id;
27
28
// tx global vars
29
static sharp_msg_t out_msg;
30
static uint8_t out_pin_mask;
31
static char out_high, out_counter;
32
static volatile char out_done;
33
34
// rx global vars
35
static uint8_t prev_bom_sig;
36
static struct bom_rx_t {
37 31f4a032 Tom Mullins
    uint8_t count;
38 f115416e Tom Mullins
    uint16_t bits;
39 31f4a032 Tom Mullins
    int last_time;
40 f115416e Tom Mullins
41
    uint8_t new_data;
42
    uint8_t address;
43
    uint8_t data;
44
} bom_rx[4];
45
46
void set_robot_id(char id) {
47
  robot_id = id;
48
}
49
50
char get_robot_id(void) {
51
  return robot_id;
52
}
53
54 812788aa Tom Mullins
void bom_isr() {
55 f115416e Tom Mullins
  if (out_high) {
56 cc9ca04e Tom Mullins
    PORT_BOM_EMIT ^= out_pin_mask;
57 f115416e Tom Mullins
  }
58
}
59
60
static void init_38kHz_signal() {
61
  out_high = 0;
62
  out_pin_mask = 0;
63
  
64 812788aa Tom Mullins
  // timer configuration now done in Atmega128rfa1.cpp
65 f115416e Tom Mullins
}
66
67
static void start_38kHz_signal() {
68
  TCNT3 = 0;
69 cc9ca04e Tom Mullins
  PORT_BOM_EMIT |= out_pin_mask;
70 f115416e Tom Mullins
  out_high = 1;
71
}
72
73
static void stop_38kHz_signal() {
74 cc9ca04e Tom Mullins
  PORT_BOM_EMIT &= ~ out_pin_mask;
75 f115416e Tom Mullins
  out_high = 0;
76
}
77
78
static void init_data_signal() {
79
  // timer 4 mode CTC (clear timer on compare), TOP = OCRA
80
  TCCR4A = 0;
81
  TCCR4B = _BV(WGM42);
82
  
83
  // run interrupt immediately when timer started
84
  OCR4A = 0;
85
  
86
  // enable interrupt
87
  TIMSK4 = _BV(OCIE4A);
88
}
89
90
static void start_data_signal() {
91
  TCNT4 = 0;
92
  
93
  // start timer 4 at F_CPU/64 prescaling
94
  TCCR4B |= _BV(CS41) | _BV(CS40);
95
}
96
97
static void stop_data_signal() {
98
  // stop timer 4
99
  TCCR4B &= ~ (_BV(CS42) | _BV(CS41) | _BV(CS40));
100
}
101
102
ISR(TIMER4_COMPA_vect) {
103 31f4a032 Tom Mullins
  if (out_high) {
104
    stop_38kHz_signal();
105
    if (out_counter) {
106
      out_counter--;
107
      if ((out_msg >> out_counter) & 1) {
108 f115416e Tom Mullins
        OCR4A = TIME_MICROS(1680, 64);
109
      } else {
110
        OCR4A = TIME_MICROS(680, 64);
111
      }
112
    } else {
113 31f4a032 Tom Mullins
      stop_data_signal();
114
      out_done = 1;
115 f115416e Tom Mullins
    }
116
  } else {
117 31f4a032 Tom Mullins
    start_38kHz_signal();
118
    OCR4A = TIME_MICROS(320, 64);
119 f115416e Tom Mullins
  }
120
}
121
122
void bom_init(void) {
123
  // BOM_SIG as input, interrupts enabled
124
  DDRB &= ~ (_BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB3));
125
  PCMSK0 |= _BV(PCINT0) | _BV(PCINT1) | _BV(PCINT2) | _BV(PCINT3);
126
  PCICR |= _BV(PCIE0);
127
  
128
  // BOM_EMIT as output
129 cc9ca04e Tom Mullins
  DDRF |= _BV(DDF4) | _BV(DDF5) | _BV(DDF6) | _BV(DDF7);
130 f115416e Tom Mullins
131
  init_38kHz_signal();
132
  init_data_signal();
133
}
134
135
void bom_send(char dir) {
136
  switch (dir) {
137 cc9ca04e Tom Mullins
    case BOM_FRONT: out_pin_mask = _BV(P_BOM_EMIT0); break;
138
    case BOM_LEFT:  out_pin_mask = _BV(P_BOM_EMIT1); break;
139
    case BOM_RIGHT: out_pin_mask = _BV(P_BOM_EMIT2); break;
140
    case BOM_BACK:  out_pin_mask = _BV(P_BOM_EMIT3); break;
141 f115416e Tom Mullins
  }
142 31f4a032 Tom Mullins
  out_counter = 16;
143 f115416e Tom Mullins
  out_msg = sharp_msg_make(0x2A, bom_msg_make(robot_id, dir));
144
  out_done = 0;
145
  start_data_signal();
146
  while (!out_done) {
147
    _delay_ms(0.1);
148
  }
149
}
150
151
static void recv_edge(char is_rising, struct bom_rx_t *rx) {
152
  if (is_rising) {
153 31f4a032 Tom Mullins
    // TODO check 320us? or have 320us timeout on rising edge?
154
  } else {
155 399f7d1b Tom Mullins
    // uses timer 5, assuming prescale 1/64
156 812788aa Tom Mullins
    // timer 5 is set up by range_init()
157 399f7d1b Tom Mullins
    int now = TCNT5;
158
    int min_low = TIME_MICROS(MIN_LOW_PW, 64);
159
    int max_low = TIME_MICROS(MAX_LOW_PW, 64);
160
    int min_high = TIME_MICROS(MIN_HIGH_PW, 64);
161
    int max_high = TIME_MICROS(MAX_HIGH_PW, 64);
162 31f4a032 Tom Mullins
    
163
    if (rx->count) {
164 399f7d1b Tom Mullins
      int diff = (now - rx->last_time);
165 31f4a032 Tom Mullins
      rx->bits <<= 1;
166 399f7d1b Tom Mullins
      if (min_low < diff && diff < max_low) {
167 31f4a032 Tom Mullins
        // 0 already in bits
168 399f7d1b Tom Mullins
      } else if (min_high < diff && diff < max_high) {
169 31f4a032 Tom Mullins
        // add 1 to bits
170
        rx->bits |= 1;
171
      } else {
172
        // error, start from beginning
173
        rx->count = 0;
174
        rx->bits = 0;
175
      }
176
      if (rx->count == 16) {
177
        // finished!
178 69c2203a Tom Mullins
        if ((rx->bits & 3) == 2) { // expansion and check bits
179 31f4a032 Tom Mullins
          rx->data = (rx->bits >> 2) & 0xff;
180
          rx->address = (rx->bits >> 10) & 0x1f;
181
          rx->new_data = 1;
182 69c2203a Tom Mullins
        }
183 31f4a032 Tom Mullins
        rx->count = 0;
184
        rx->bits = 0;
185 f115416e Tom Mullins
      }
186
    }
187 31f4a032 Tom Mullins
    
188
    rx->count++;
189
    rx->last_time = now;
190 f115416e Tom Mullins
  }
191
}
192
193
ISR(PCINT0_vect) {
194 cc9ca04e Tom Mullins
  uint8_t bom_sig = PIN_BOM_SIG;
195 f115416e Tom Mullins
  uint8_t changed = bom_sig ^ prev_bom_sig;
196 cc9ca04e Tom Mullins
  if (changed & _BV(P_BOM_SIG0)) recv_edge(bom_sig & _BV(P_BOM_SIG0), &bom_rx[0]);
197
  if (changed & _BV(P_BOM_SIG1)) recv_edge(bom_sig & _BV(P_BOM_SIG1), &bom_rx[1]);
198
  if (changed & _BV(P_BOM_SIG2)) recv_edge(bom_sig & _BV(P_BOM_SIG2), &bom_rx[2]);
199
  if (changed & _BV(P_BOM_SIG3)) recv_edge(bom_sig & _BV(P_BOM_SIG3), &bom_rx[3]);
200 f115416e Tom Mullins
  prev_bom_sig = bom_sig;
201
}
202
203
int bom_get(char dir) {
204
  bom_rx_t *rx = &bom_rx[(int)dir];
205
  int ret = BOM_NO_DATA;
206
  cli();
207
  if (rx->new_data) {
208
    rx->new_data = 0;
209
    ret = rx->data;
210
  }
211
  sei();
212
  return ret;
213
}