Statistics
| Branch: | Revision:

scoutos / scout_avr / src / bom.cpp @ cc9ca04e

History | View | Annotate | Download (4.74 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
#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
    uint8_t count;
38
    uint16_t bits;
39
    int last_time;
40

    
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
void bom_isr() {
55
  if (out_high) {
56
    PORT_BOM_EMIT ^= out_pin_mask;
57
  }
58
}
59

    
60
static void init_38kHz_signal() {
61
  out_high = 0;
62
  out_pin_mask = 0;
63
  
64
  // timer configuration now done in Atmega128rfa1.cpp
65
}
66

    
67
static void start_38kHz_signal() {
68
  TCNT3 = 0;
69
  PORT_BOM_EMIT |= out_pin_mask;
70
  out_high = 1;
71
}
72

    
73
static void stop_38kHz_signal() {
74
  PORT_BOM_EMIT &= ~ out_pin_mask;
75
  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
  if (out_high) {
104
    stop_38kHz_signal();
105
    if (out_counter) {
106
      out_counter--;
107
      if ((out_msg >> out_counter) & 1) {
108
        OCR4A = TIME_MICROS(1680, 64);
109
      } else {
110
        OCR4A = TIME_MICROS(680, 64);
111
      }
112
    } else {
113
      stop_data_signal();
114
      out_done = 1;
115
    }
116
  } else {
117
    start_38kHz_signal();
118
    OCR4A = TIME_MICROS(320, 64);
119
  }
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
  DDRF |= _BV(DDF4) | _BV(DDF5) | _BV(DDF6) | _BV(DDF7);
130

    
131
  init_38kHz_signal();
132
  init_data_signal();
133
}
134

    
135
void bom_send(char dir) {
136
  switch (dir) {
137
    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
  }
142
  out_counter = 16;
143
  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
    // TODO check 320us? or have 320us timeout on rising edge?
154
  } else {
155
    // uses timer 5, assuming prescale 1/8
156
    // timer 5 is set up by range_init()
157
    // this should be in units of microseconds
158
    int now = TCNT5 / (F_CPU / 8 / 1000000);
159
    
160
    if (rx->count) {
161
      int diff = now - rx->last_time;
162
      rx->bits <<= 1;
163
      if (MIN_LOW_PW < diff && diff < MAX_LOW_PW) {
164
        // 0 already in bits
165
      } else if (MIN_HIGH_PW < diff && diff < MAX_HIGH_PW) {
166
        // add 1 to bits
167
        rx->bits |= 1;
168
      } else {
169
        // error, start from beginning
170
        rx->count = 0;
171
        rx->bits = 0;
172
      }
173
      if (rx->count == 16) {
174
        // finished!
175
        if ((rx->bits & 3) == 2) { // expansion and check bits
176
          rx->data = (rx->bits >> 2) & 0xff;
177
          rx->address = (rx->bits >> 10) & 0x1f;
178
          rx->new_data = 1;
179
        }
180
        rx->count = 0;
181
        rx->bits = 0;
182
      }
183
    }
184
    
185
    rx->count++;
186
    rx->last_time = now;
187
  }
188
}
189

    
190
ISR(PCINT0_vect) {
191
  uint8_t bom_sig = PIN_BOM_SIG;
192
  uint8_t changed = bom_sig ^ prev_bom_sig;
193
  if (changed & _BV(P_BOM_SIG0)) recv_edge(bom_sig & _BV(P_BOM_SIG0), &bom_rx[0]);
194
  if (changed & _BV(P_BOM_SIG1)) recv_edge(bom_sig & _BV(P_BOM_SIG1), &bom_rx[1]);
195
  if (changed & _BV(P_BOM_SIG2)) recv_edge(bom_sig & _BV(P_BOM_SIG2), &bom_rx[2]);
196
  if (changed & _BV(P_BOM_SIG3)) recv_edge(bom_sig & _BV(P_BOM_SIG3), &bom_rx[3]);
197
  prev_bom_sig = bom_sig;
198
}
199

    
200
int bom_get(char dir) {
201
  bom_rx_t *rx = &bom_rx[(int)dir];
202
  int ret = BOM_NO_DATA;
203
  cli();
204
  if (rx->new_data) {
205
    rx->new_data = 0;
206
    ret = rx->data;
207
  }
208
  sei();
209
  return ret;
210
}