Statistics
| Branch: | Revision:

root / scout_avr / src / bom.cpp @ f115416e

History | View | Annotate | Download (4.92 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 3 for 38kHz signal
16
 */
17

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

    
21
typedef uint16_t sharp_msg_t;
22

    
23
static sharp_msg_t sharp_msg_make(char address, bom_msg_t bom_msg) {
24
  return (((uint16_t)address & 0x3F) << 10) | ((uint16_t)bom_msg << 2) | 2;
25
}
26

    
27
static char robot_id;
28

    
29
// tx global vars
30
static sharp_msg_t out_msg;
31
static uint8_t out_pin_mask;
32
static char out_high, out_counter;
33
static volatile char out_done;
34

    
35
// rx global vars
36
static uint8_t prev_bom_sig;
37
static struct bom_rx_t {
38
    uint8_t bits_rx;
39
    uint16_t bits;
40
    int last_bit;
41

    
42
    uint8_t new_data;
43
    uint8_t address;
44
    uint8_t data;
45
} bom_rx[4];
46

    
47
void set_robot_id(char id) {
48
  robot_id = id;
49
}
50

    
51
char get_robot_id(void) {
52
  return robot_id;
53
}
54

    
55
ISR(TIMER3_COMPA_vect) {
56
  if (out_high) {
57
    BOM_EMIT ^= out_pin_mask;
58
  }
59
}
60

    
61
static void init_38kHz_signal() {
62
  out_high = 0;
63
  out_pin_mask = 0;
64
  
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);
78
}
79

    
80
static void start_38kHz_signal() {
81
  TCNT3 = 0;
82
  BOM_EMIT |= out_pin_mask;
83
  out_high = 1;
84
}
85

    
86
static void stop_38kHz_signal() {
87
  BOM_EMIT &= ~ out_pin_mask;
88
  out_high = 0;
89
}
90

    
91
static void init_data_signal() {
92
  // timer 4 mode CTC (clear timer on compare), TOP = OCRA
93
  TCCR4A = 0;
94
  TCCR4B = _BV(WGM42);
95
  TCCR3C = 0;
96
  
97
  // run interrupt immediately when timer started
98
  OCR4A = 0;
99
  
100
  // enable interrupt
101
  TIMSK4 = _BV(OCIE4A);
102
}
103

    
104
static void start_data_signal() {
105
  TCNT4 = 0;
106
  
107
  // start timer 4 at F_CPU/64 prescaling
108
  TCCR4B |= _BV(CS41) | _BV(CS40);
109
}
110

    
111
static void stop_data_signal() {
112
  // stop timer 4
113
  TCCR4B &= ~ (_BV(CS42) | _BV(CS41) | _BV(CS40));
114
}
115

    
116
ISR(TIMER4_COMPA_vect) {
117
  if (out_counter) {
118
    if (out_high) {
119
      stop_38kHz_signal();
120
      if (out_msg & 1) {
121
        OCR4A = TIME_MICROS(1680, 64);
122
      } else {
123
        OCR4A = TIME_MICROS(680, 64);
124
      }
125
      out_msg >>= 1;
126
      out_counter--;
127
    } else {
128
      start_38kHz_signal();
129
      OCR4A = TIME_MICROS(320, 64);
130
    }
131
  } else {
132
    stop_data_signal();
133
    out_done = 1;
134
  }
135
}
136

    
137
void bom_init(void) {
138
  // BOM_SIG as input, interrupts enabled
139
  DDRB &= ~ (_BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB3));
140
  PCMSK0 |= _BV(PCINT0) | _BV(PCINT1) | _BV(PCINT2) | _BV(PCINT3);
141
  PCICR |= _BV(PCIE0);
142
  
143
  // BOM_EMIT as output
144
  DDRH |= _BV(DDH4) | _BV(DDH5) | _BV(DDH6) | _BV(DDH7);
145

    
146
  init_38kHz_signal();
147
  init_data_signal();
148
}
149

    
150
void bom_send(char dir) {
151
  switch (dir) {
152
    case BOM_FRONT: out_pin_mask = _BV(BOM_EMIT0); break;
153
    case BOM_LEFT:  out_pin_mask = _BV(BOM_EMIT1); break;
154
    case BOM_RIGHT: out_pin_mask = _BV(BOM_EMIT2); break;
155
    case BOM_BACK:  out_pin_mask = _BV(BOM_EMIT3); break;
156
  }
157
  out_counter = 17;
158
  out_msg = sharp_msg_make(0x2A, bom_msg_make(robot_id, dir));
159
  out_done = 0;
160
  start_data_signal();
161
  while (!out_done) {
162
    _delay_ms(0.1);
163
  }
164
}
165

    
166
static void recv_edge(char is_rising, struct bom_rx_t *rx) {
167
  if (is_rising) {
168
    // uses timer 1, assuming prescale 1/8
169
    // timer 1 is set up by range_init()
170
    int now = TCNT1 * (F_CPU / 1000000) / 8;
171
    int diff = now - rx->last_bit;
172
    rx->last_bit = now;
173
    rx->bits_rx++;
174
    rx->bits <<= 1;
175
    if (MIN_LOW_PW < diff && diff < MAX_LOW_PW) {
176
      // 0 already in bits
177
    } else if (MIN_HIGH_PW < diff && diff < MAX_HIGH_PW) {
178
      // add 1 to bits
179
      rx->bits |= 1;
180
    } else {
181
      // error, start from beginning
182
      rx->bits_rx = 0;
183
      rx->bits = 0;
184
    }
185
    if (rx->bits_rx == 16) {
186
      // finished!
187
      if ((rx->bits & 3) == 2) { // expansion and check bits
188
        rx->data = (rx->bits >> 2) & 0xff;
189
        rx->address = (rx->bits >> 10) & 0x1f;
190
        rx->new_data = 1;
191
      }
192
      rx->bits_rx = 0;
193
      rx->bits = 0;
194
    }
195
  } else {
196
    // TODO check 320us? or have 320us timeout on rising edge?
197
  }
198
}
199

    
200
ISR(PCINT0_vect) {
201
  uint8_t bom_sig = BOM_SIG;
202
  uint8_t changed = bom_sig ^ prev_bom_sig;
203
  if (changed & _BV(BOM_SIG0)) recv_edge(bom_sig & _BV(BOM_SIG0), &bom_rx[0]);
204
  if (changed & _BV(BOM_SIG1)) recv_edge(bom_sig & _BV(BOM_SIG1), &bom_rx[1]);
205
  if (changed & _BV(BOM_SIG2)) recv_edge(bom_sig & _BV(BOM_SIG2), &bom_rx[2]);
206
  if (changed & _BV(BOM_SIG3)) recv_edge(bom_sig & _BV(BOM_SIG3), &bom_rx[3]);
207
  prev_bom_sig = bom_sig;
208
}
209

    
210
int bom_get(char dir) {
211
  bom_rx_t *rx = &bom_rx[(int)dir];
212
  int ret = BOM_NO_DATA;
213
  cli();
214
  if (rx->new_data) {
215
    rx->new_data = 0;
216
    ret = rx->data;
217
  }
218
  sei();
219
  return ret;
220
}