Revision f115416e

View differences:

scout_avr/src/Atmega128rfa1.cpp
59 59
  TCCR0B = _BV(CS01) | _BV(CS00);
60 60
  // enable interrupt on compare match A
61 61
  TIMSK0 = _BV(OCIE0A);
62
  // (1 ms) * 16 MHz / 64 prescaler = 250
63
  //OCR0A = 250;
64
  // (1 ms) * 8 MHz / 64 prescaler = 125
65
  OCR0A = 125;
62
  // 1 ms with 1/64 prescaler
63
  OCR0A = F_CPU / 1000 / 64;
66 64
  millis = 0;
67 65

  
68 66
  sei();
scout_avr/src/Atmega128rfa1.h
19 19
  void init();
20 20
  int read();
21 21
  void write(uint8_t* data, int length);
22
  void puts(const char* str) {write((uint8_t*) str, strlen(str));}
22 23
  unsigned long time();
23 24
};
24 25

  
scout_avr/src/bom.cpp
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
}
scout_avr/src/bom.h
1
#ifndef _BOM_H
2
#define _BOM_H
3

  
4
extern "C" {
5
#include <stdint.h>
6
}
7

  
8
// constants for direction
9
#define BOM_FRONT 0
10
#define BOM_BACK  1
11
#define BOM_LEFT  2
12
#define BOM_RIGHT 3
13

  
14
// timing, in us, for read of valid bits
15
#define MIN_LOW_PW 800
16
#define MAX_LOW_PW 1200
17
#define MIN_HIGH_PW 1800
18
#define MAX_HIGH_PW 2200
19

  
20
// returned by bom_get if there is no new data since last call
21
#define BOM_NO_DATA -1
22

  
23
// i/o pins
24
// if these are changed, remember to change bom_init and/or ISRs
25
#define BOM_SIG PORTB
26
#define BOM_SIG0 PB0
27
#define BOM_SIG1 PB1
28
#define BOM_SIG2 PB2
29
#define BOM_SIG3 PB3
30
#define BOM_EMIT PORTH
31
#define BOM_EMIT0 PH4
32
#define BOM_EMIT1 PH5
33
#define BOM_EMIT2 PH6
34
#define BOM_EMIT3 PH7
35

  
36
typedef uint8_t bom_msg_t;
37

  
38
inline char bom_msg_get_robot_id(bom_msg_t msg) {
39
    return msg >> 2;
40
}
41
inline char bom_msg_get_dir(bom_msg_t msg) {
42
    return msg & 3;
43
}
44
inline bom_msg_t bom_msg_make(char id, char dir) {
45
    return (id << 2) | (dir & 3);
46
}
47

  
48
void set_robot_id(char id);
49
char get_robot_id(void);
50

  
51
// NOTE: call range_init before bom_init to ensure timer 1 is set up!
52
void bom_init(void);
53
void bom_send(char dir);
54
int bom_get(char dir);
55

  
56
#endif
scout_avr/src/main.cpp
42 42

  
43 43
#include "Atmega128rfa1.h"
44 44
#include "range.h"
45
#include "bom.h"
45 46

  
46 47
int main()
47 48
{
48 49
  char buf[20];
50
  int i;
49 51
  unsigned long now, next = 0;
50 52
  Atmega128rfa1 avr;
51 53
  avr.init();
52 54
  range_init();
55
  bom_init();
56
  set_robot_id(0x2a);
53 57
  while (1)
54 58
  {
55 59
    now = avr.time();
56 60
    if (now > next) {
57 61
      next = now + 100;
58
      utoa(range_get(0), buf, 10);
59
      avr.write((uint8_t*) buf, strlen(buf));
60
      avr.write((uint8_t*) ", ", 2);
62
      /*utoa(range_get(0), buf, 10);
63
      avr.puts("Range: ");
64
      avr.puts(buf);
65
      avr.puts(", ");
61 66
      utoa(range_get(1), buf, 10);
62
      avr.write((uint8_t*) buf, strlen(buf));
63
      /*int data = avr.read();
64
      if (data >= 0)
65
      {
66
        uint8_t data8 = data;
67
        avr.write((uint8_t*) "Received ", 9);
68
        avr.write(&data8, 1);
67
      avr.puts(buf);*/
68
      for (i = 0; i < 4; i++) {
69
        bom_send(i);
70
        int msg = bom_get(i);
71
        if (msg != BOM_NO_DATA) {
72
          avr.puts("BOM ");
73
          itoa(i, buf, 10);
74
          avr.puts(buf);
75
          avr.puts(": ");
76
          itoa(msg, buf, 10);
77
          avr.puts(buf);
78
        }
69 79
      }
70
      else
71
        avr.write((uint8_t*) ".", 1);*/
72
      avr.write((uint8_t*) "\r\n", 2);
80
      avr.puts("\r\n");
73 81
    }
74 82
  }
75 83
  return 0;
scout_avr/src/range.cpp
73 73
  EIMSK |= _BV(INT1) | _BV(INT0);
74 74
  
75 75
  // CS1 = 2, 1/8 prescaler
76
  // if this is changed, remember to change recv_edge in bom.cpp!
76 77
  TCCR1B = _BV(CS11);
77 78
  
78 79
  range[0].value = RANGE_ERR;

Also available in: Unified diff