Statistics
| Branch: | Revision:

root / scout_avr / src / bom.cpp @ 31f4a032

History | View | Annotate | Download (5.04 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 count;
39
    uint16_t bits;
40
    int last_time;
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_high) {
118
    stop_38kHz_signal();
119
    if (out_counter) {
120
      out_counter--;
121
      if ((out_msg >> out_counter) & 1) {
122
        OCR4A = TIME_MICROS(1680, 64);
123
      } else {
124
        OCR4A = TIME_MICROS(680, 64);
125
      }
126
    } else {
127
      stop_data_signal();
128
      out_done = 1;
129
    }
130
  } else {
131
    start_38kHz_signal();
132
    OCR4A = TIME_MICROS(320, 64);
133
  }
134
}
135

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

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

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

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

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

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