Statistics
| Branch: | Revision:

root / scout_avr / src / bom.cpp @ 63e19141

History | View | Annotate | Download (5.83 KB)

1
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
 * BOM uses timer 4 for period timing, and timer 0 for 38kHz signal
16
 */
17

    
18
 /*
19
  * Recently modified so that when BOM is sending, it stops listening. When
20
  * not sending, it keeps listening.
21
  */
22

    
23
#define TIME_MICROS(us, prescale) (F_CPU / 1000000 * (us) / (prescale))
24

    
25
typedef uint16_t sharp_msg_t;
26

    
27
static sharp_msg_t sharp_msg_make(char address, bom_msg_t bom_msg) {
28
  return (((uint16_t)address & 0x3F) << 10) | ((uint16_t)bom_msg << 2) | 2;
29
}
30

    
31
static char robot_id;
32

    
33
// tx global vars
34
static sharp_msg_t out_msg;
35
static uint8_t out_pin_mask;
36
static char out_high, out_counter;
37
static volatile char out_done;
38

    
39
// rx global vars
40
static uint8_t prev_bom_sig;
41
static struct bom_rx_t {
42
    uint8_t count;
43
    uint16_t bits;
44
    int last_time;
45

    
46
    uint8_t new_data;
47
    uint8_t address;
48
    uint8_t data;
49
} bom_rx[4];
50

    
51
void set_robot_id(char id) {
52
  robot_id = id;
53
}
54

    
55
char get_robot_id(void) {
56
  return robot_id;
57
}
58

    
59
void bom_isr() {
60
  if (out_high) {
61
    PORT_BOM_EMIT ^= out_pin_mask;
62
  }
63
}
64

    
65
static void init_38kHz_signal() {
66
  out_high = 0;
67
  out_pin_mask = 0;
68

    
69
  // timer configuration now done in Atmega128rfa1.cpp
70
}
71

    
72
static void start_38kHz_signal() {
73
  TCNT3 = 0;
74
  PORT_BOM_EMIT |= out_pin_mask;
75
  out_high = 1;
76
}
77

    
78
static void stop_38kHz_signal() {
79
  PORT_BOM_EMIT &= ~ out_pin_mask; // why not just do PORT_BOM_EMIT &= 0; ?
80
  out_high = 0;
81
}
82

    
83
static void init_data_signal() {
84
  // timer 4 mode CTC (clear timer on compare), TOP = OCRA
85
  TCCR4A = 0;
86
  TCCR4B = _BV(WGM42);
87

    
88
  // run interrupt immediately when timer started
89
  OCR4A = 0;
90

    
91
  // enable interrupt
92
  TIMSK4 = _BV(OCIE4A);
93
}
94

    
95
static void start_data_signal() {
96
  TCNT4 = 0;
97

    
98
  // start timer 4 at F_CPU/64 prescaling
99
  TCCR4B |= _BV(CS41) | _BV(CS40);
100
}
101

    
102
static void stop_data_signal() {
103
  // stop timer 4
104
  TCCR4B &= ~ (_BV(CS42) | _BV(CS41) | _BV(CS40));
105
}
106

    
107
ISR(TIMER4_COMPA_vect) {
108
  if (out_high) {
109
    stop_38kHz_signal();
110
    if (out_counter) {
111
      out_counter--;
112
      if ((out_msg >> out_counter) & 1) {
113
        OCR4A = TIME_MICROS(1680, 64);
114
      } else {
115
        OCR4A = TIME_MICROS(680, 64);
116
      }
117
    } else {
118
      stop_data_signal();
119
      out_done = 1;
120
    }
121
  } else {
122
    start_38kHz_signal();
123
    OCR4A = TIME_MICROS(320, 64);
124
  }
125
}
126

    
127
void bom_init(void) {
128
  // BOM_SIG as input, interrupts enabled
129
  DDRB &= ~ (_BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB3));
130
  PCMSK0 |= _BV(PCINT0) | _BV(PCINT1) | _BV(PCINT2) | _BV(PCINT3);
131
  PCICR |= _BV(PCIE0);
132

    
133
  // BOM_EMIT as output
134
  DDRF |= _BV(DDF4) | _BV(DDF5) | _BV(DDF6) | _BV(DDF7);
135

    
136
  init_38kHz_signal();
137
  init_data_signal();
138
}
139

    
140
static void stop_receiving(char dir) {
141
  switch (dir) {
142
    case BOM_FRONT: PCMSK0 &= ~_BV(PCINT0); break;
143
    case BOM_LEFT : PCMSK0 &= ~_BV(PCINT1); break;
144
    case BOM_RIGHT: PCMSK0 &= ~_BV(PCINT2); break;
145
    case BOM_BACK : PCMSK0 &= ~_BV(PCINT3); break;
146
  }
147
}
148

    
149
static void start_receiving(char dir) {
150
  // flush rx buffer before turning on the receiver
151
  bom_rx[(int)dir].bits = 0;
152
  bom_rx[(int)dir].count = 0;
153

    
154
  switch (dir) {
155
    case BOM_FRONT: PCMSK0 |= _BV(PCINT0); break;
156
    case BOM_LEFT : PCMSK0 |= _BV(PCINT1); break;
157
    case BOM_RIGHT: PCMSK0 |= _BV(PCINT2); break;
158
    case BOM_BACK : PCMSK0 |= _BV(PCINT3); break;
159
  }
160
}
161

    
162
void bom_send(char dir) {
163
  switch (dir) {
164
    case BOM_FRONT: out_pin_mask = _BV(P_BOM_EMIT0); break;
165
    case BOM_LEFT:  out_pin_mask = _BV(P_BOM_EMIT1); break;
166
    case BOM_RIGHT: out_pin_mask = _BV(P_BOM_EMIT2); break;
167
    case BOM_BACK:  out_pin_mask = _BV(P_BOM_EMIT3); break;
168
  }
169
  out_counter = 16;
170
  out_msg = sharp_msg_make(0x2A, bom_msg_make(robot_id, dir));
171
  out_done = 0;
172

    
173
  stop_receiving(dir); // disable receiver
174
  start_38kHz_signal(); // enable the transmitter
175
  start_data_signal();
176
  while (!out_done) {
177
    _delay_ms(0.1);
178
  }
179
  stop_data_signal(); // disable the transmitter
180
  stop_38kHz_signal();
181
  start_receiving(dir);  // enable the receiver
182
}
183

    
184
static void recv_edge(char is_rising, struct bom_rx_t *rx) {
185
  if (is_rising) {
186
    // TODO check 320us? or have 320us timeout on rising edge?
187
  } else {
188
    // uses timer 5, assuming prescale 1/64
189
    // timer 5 is set up by range_init()
190
    int now = TCNT5;
191
    int min_low = TIME_MICROS(MIN_LOW_PW, 64);
192
    int max_low = TIME_MICROS(MAX_LOW_PW, 64);
193
    int min_high = TIME_MICROS(MIN_HIGH_PW, 64);
194
    int max_high = TIME_MICROS(MAX_HIGH_PW, 64);
195

    
196
    if (rx->count) {
197
      int diff = (now - rx->last_time);
198
      rx->bits <<= 1;
199
      if (min_low < diff && diff < max_low) {
200
        // 0 already in bits
201
      } else if (min_high < diff && diff < max_high) {
202
        // add 1 to bits
203
        rx->bits |= 1;
204
      } else {
205
        // error, start from beginning
206
        rx->count = 0;
207
        rx->bits = 0;
208
      }
209
      if (rx->count == 16) {
210
        // finished!
211
        if ((rx->bits & 3) == 2) { // expansion and check bits
212
          rx->data = (rx->bits >> 2) & 0xff;
213
          rx->address = (rx->bits >> 10) & 0x1f;
214
          rx->new_data = 1;
215
        }
216
        rx->count = 0;
217
        rx->bits = 0;
218
      }
219
    }
220

    
221
    rx->count++;
222
    rx->last_time = now;
223
  }
224
}
225

    
226
ISR(PCINT0_vect) {
227
  uint8_t bom_sig = PIN_BOM_SIG;
228
  uint8_t changed = bom_sig ^ prev_bom_sig;
229
  if (changed & _BV(P_BOM_SIG0)) recv_edge(bom_sig & _BV(P_BOM_SIG0), &bom_rx[0]);
230
  if (changed & _BV(P_BOM_SIG1)) recv_edge(bom_sig & _BV(P_BOM_SIG1), &bom_rx[1]);
231
  if (changed & _BV(P_BOM_SIG2)) recv_edge(bom_sig & _BV(P_BOM_SIG2), &bom_rx[2]);
232
  if (changed & _BV(P_BOM_SIG3)) recv_edge(bom_sig & _BV(P_BOM_SIG3), &bom_rx[3]);
233
  prev_bom_sig = bom_sig;
234
}
235

    
236
int bom_get(char dir) {
237
  bom_rx_t *rx = &bom_rx[(int)dir];
238
  int ret = BOM_NO_DATA;
239
  cli();
240
  if (rx->new_data) {
241
    rx->new_data = 0;
242
    ret = rx->data;
243
  }
244
  sei();
245
  return ret;
246
}