Colony » Colony Scout » Colony Scout OS

  // 1 ms with 1/64 prescaler

  OCR0A = F_CPU / 1000 / 64;

  void puts(const char* str) {write((uint8_t*) str, strlen(str));}

extern "C" {

#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>

}

#include "bom.h"

/*

 * Sharp protocol:

 *  Period modulation

 *   1: 2ms period

 *   0: 1ms period

 *  Signal:

 *   38kHz pulse for 320us, then off for rest of time (680us or 1680us)

 * BOM uses timer 4 for period timing, and timer 3 for 38kHz signal

 */

#define TIME_KHZ(khz, prescale) (F_CPU / 1000 / (khz) / (prescale))

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

typedef uint16_t sharp_msg_t;

static sharp_msg_t sharp_msg_make(char address, bom_msg_t bom_msg) {

  return (((uint16_t)address & 0x3F) << 10) | ((uint16_t)bom_msg << 2) | 2;

}

static char robot_id;

// tx global vars

static sharp_msg_t out_msg;

static uint8_t out_pin_mask;

static char out_high, out_counter;

static volatile char out_done;

// rx global vars

static uint8_t prev_bom_sig;

static struct bom_rx_t {

    uint8_t bits_rx;

    uint16_t bits;

    int last_bit;

    uint8_t new_data;

    uint8_t address;

    uint8_t data;

} bom_rx[4];

void set_robot_id(char id) {

  robot_id = id;

}

char get_robot_id(void) {

  return robot_id;

}

ISR(TIMER3_COMPA_vect) {

  if (out_high) {

    BOM_EMIT ^= out_pin_mask;

  }

}

static void init_38kHz_signal() {

  out_high = 0;

  out_pin_mask = 0;

  // timer 3 mode CTC (clear timer on compare), TOP = OCRA

  TCCR3A = 0;

  TCCR3B = _BV(WGM32);

  TCCR3C = 0;

  // toggling at 38kHz * 2 gives 38kHz wave

  OCR3A = TIME_KHZ(38 * 2, 1);

  // enable interrupt

  TIMSK3 = _BV(OCIE3A);

  // start timer 3 at F_CPU, no prescaling

  TCCR3B |= _BV(CS30);

}

static void start_38kHz_signal() {

  TCNT3 = 0;

  BOM_EMIT |= out_pin_mask;

  out_high = 1;

}

static void stop_38kHz_signal() {

  BOM_EMIT &= ~ out_pin_mask;

  out_high = 0;

}

static void init_data_signal() {

  // timer 4 mode CTC (clear timer on compare), TOP = OCRA

  TCCR4A = 0;

  TCCR4B = _BV(WGM42);

  TCCR3C = 0;

  // run interrupt immediately when timer started

  OCR4A = 0;

  // enable interrupt

  TIMSK4 = _BV(OCIE4A);

}

static void start_data_signal() {

  TCNT4 = 0;

  // start timer 4 at F_CPU/64 prescaling

  TCCR4B |= _BV(CS41) | _BV(CS40);

}

static void stop_data_signal() {

  // stop timer 4

  TCCR4B &= ~ (_BV(CS42) | _BV(CS41) | _BV(CS40));

}

ISR(TIMER4_COMPA_vect) {

  if (out_counter) {

    if (out_high) {

      stop_38kHz_signal();

      if (out_msg & 1) {

        OCR4A = TIME_MICROS(1680, 64);

      } else {

        OCR4A = TIME_MICROS(680, 64);

      }

      out_msg >>= 1;

      out_counter--;

    } else {

      start_38kHz_signal();

      OCR4A = TIME_MICROS(320, 64);

    }

  } else {

    stop_data_signal();

    out_done = 1;

  }

}

void bom_init(void) {

  // BOM_SIG as input, interrupts enabled

  DDRB &= ~ (_BV(DDB0) | _BV(DDB1) | _BV(DDB2) | _BV(DDB3));

  PCMSK0 |= _BV(PCINT0) | _BV(PCINT1) | _BV(PCINT2) | _BV(PCINT3);

  PCICR |= _BV(PCIE0);

  // BOM_EMIT as output

  DDRH |= _BV(DDH4) | _BV(DDH5) | _BV(DDH6) | _BV(DDH7);

  init_38kHz_signal();

  init_data_signal();

}

void bom_send(char dir) {

  switch (dir) {

    case BOM_FRONT: out_pin_mask = _BV(BOM_EMIT0); break;

    case BOM_LEFT:  out_pin_mask = _BV(BOM_EMIT1); break;

    case BOM_RIGHT: out_pin_mask = _BV(BOM_EMIT2); break;

    case BOM_BACK:  out_pin_mask = _BV(BOM_EMIT3); break;

  }

  out_counter = 17;

  out_msg = sharp_msg_make(0x2A, bom_msg_make(robot_id, dir));

  out_done = 0;

  start_data_signal();

  while (!out_done) {

    _delay_ms(0.1);

  }

}

static void recv_edge(char is_rising, struct bom_rx_t *rx) {

  if (is_rising) {

    // uses timer 1, assuming prescale 1/8

    // timer 1 is set up by range_init()

    int now = TCNT1 * (F_CPU / 1000000) / 8;

    int diff = now - rx->last_bit;

    rx->last_bit = now;

    rx->bits_rx++;

    rx->bits <<= 1;

    if (MIN_LOW_PW < diff && diff < MAX_LOW_PW) {

      // 0 already in bits

    } else if (MIN_HIGH_PW < diff && diff < MAX_HIGH_PW) {

      // add 1 to bits

      rx->bits |= 1;

    } else {

      // error, start from beginning

      rx->bits_rx = 0;

      rx->bits = 0;

    }

    if (rx->bits_rx == 16) {

      // finished!

      if ((rx->bits & 3) == 2) { // expansion and check bits

        rx->data = (rx->bits >> 2) & 0xff;

        rx->address = (rx->bits >> 10) & 0x1f;

        rx->new_data = 1;

      }

      rx->bits_rx = 0;

      rx->bits = 0;

    }

  } else {

    // TODO check 320us? or have 320us timeout on rising edge?

  }

}

ISR(PCINT0_vect) {

  uint8_t bom_sig = BOM_SIG;

  uint8_t changed = bom_sig ^ prev_bom_sig;

  if (changed & _BV(BOM_SIG0)) recv_edge(bom_sig & _BV(BOM_SIG0), &bom_rx[0]);

  if (changed & _BV(BOM_SIG1)) recv_edge(bom_sig & _BV(BOM_SIG1), &bom_rx[1]);

  if (changed & _BV(BOM_SIG2)) recv_edge(bom_sig & _BV(BOM_SIG2), &bom_rx[2]);

  if (changed & _BV(BOM_SIG3)) recv_edge(bom_sig & _BV(BOM_SIG3), &bom_rx[3]);

  prev_bom_sig = bom_sig;

}

int bom_get(char dir) {

  bom_rx_t *rx = &bom_rx[(int)dir];

  int ret = BOM_NO_DATA;

  cli();

  if (rx->new_data) {

    rx->new_data = 0;

    ret = rx->data;

  }

  sei();

  return ret;

}

#ifndef _BOM_H

#define _BOM_H

extern "C" {

#include <stdint.h>

}

// constants for direction

#define BOM_FRONT 0

#define BOM_BACK  1

#define BOM_LEFT  2

#define BOM_RIGHT 3

// timing, in us, for read of valid bits

#define MIN_LOW_PW 800

#define MAX_LOW_PW 1200

#define MIN_HIGH_PW 1800

#define MAX_HIGH_PW 2200

// returned by bom_get if there is no new data since last call

#define BOM_NO_DATA -1

// i/o pins

// if these are changed, remember to change bom_init and/or ISRs

#define BOM_SIG PORTB

#define BOM_SIG0 PB0

#define BOM_SIG1 PB1

#define BOM_SIG2 PB2

#define BOM_SIG3 PB3

#define BOM_EMIT PORTH

#define BOM_EMIT0 PH4

#define BOM_EMIT1 PH5

#define BOM_EMIT2 PH6

#define BOM_EMIT3 PH7

typedef uint8_t bom_msg_t;

inline char bom_msg_get_robot_id(bom_msg_t msg) {

    return msg >> 2;

}

inline char bom_msg_get_dir(bom_msg_t msg) {

    return msg & 3;

}

inline bom_msg_t bom_msg_make(char id, char dir) {

    return (id << 2) | (dir & 3);

}

void set_robot_id(char id);

char get_robot_id(void);

// NOTE: call range_init before bom_init to ensure timer 1 is set up!

void bom_init(void);

void bom_send(char dir);

int bom_get(char dir);

#endif

#include "bom.h"

  int i;

  bom_init();

  set_robot_id(0x2a);

      /*utoa(range_get(0), buf, 10);

      avr.puts("Range: ");

      avr.puts(buf);

      avr.puts(", ");

      avr.puts(buf);*/

      for (i = 0; i < 4; i++) {

        bom_send(i);

        int msg = bom_get(i);

        if (msg != BOM_NO_DATA) {

          avr.puts("BOM ");

          itoa(i, buf, 10);

          avr.puts(buf);

          avr.puts(": ");

          itoa(msg, buf, 10);

          avr.puts(buf);

        }

      avr.puts("\r\n");

  // if this is changed, remember to change recv_edge in bom.cpp!