Colony

#define USE_ALT_TWI // an alternative to libdragonfly's i2c code, in twi.cpp

#include "Forklift.h"

extern "C"

{

#include <util/delay.h>

}

#ifdef USE_ALT_TWI

#  include "twi.h"

#else

extern "C"

{

#  include "i2c.h"

}

#endif

#define SCOUT_AVR_ADDR 0x01

#define FORKLIFT_ADDR 0x41

// indicies for forklift internal data

#define FORKLIFT_TRACKING_ID       0

#define FORKLIFT_SERIAL_NUMBER     1

#define FORKLIFT_HEIGHT            2

#define FORKLIFT_HEIGHT_SETPOINT   3 // r/w

#define FORKLIFT_LINE_POS          4

#define FORKLIFT_LINE_THRESH_HIGH  5 // r/w

#define FORKLIFT_LINE_THRESH_LOW   6 // r/w

#define FORKLIFT_DATA_LEN          7

char volatile response;

char volatile received;

static int mrecv(char data)

{

  response = data;

  received = 1;

  return 0;

}

static void srecv(char) {}

static char ssend() {return 0;}

static int read_addr(char addr)

{

#ifdef USE_ALT_TWI

  return FORKLIFT_ERROR;

#else

  if (i2c_send(FORKLIFT_ADDR, &addr, 1))

    return FORKLIFT_ERROR;

  received = 0;

  if (i2c_request(FORKLIFT_ADDR))

    return FORKLIFT_ERROR;

  for (int i = 0; i < 500; i++) // waits up to ~50 ms

  {

    _delay_ms(.1);

    if (received)

      return response;

  }

  return FORKLIFT_ERROR;

#endif

}

static int write_addr(char addr, char data)

{

  char buf[2];

  buf[0] = addr;

  buf[1] = data;

#ifdef USE_ALT_TWI

  if (twi_writeTo(FORKLIFT_ADDR, (uint8_t*) buf, 2, 1))

    return FORKLIFT_ERROR;

#else

  if (i2c_send(FORKLIFT_ADDR, buf, 2))

    return FORKLIFT_ERROR;

#endif

  return 0;

}

void Forklift::forklift_init()

{

#ifdef USE_ALT_TWI

  twi_init();

  twi_setAddress(SCOUT_AVR_ADDR);

#else

  i2c_init(SCOUT_AVR_ADDR, mrecv, srecv, ssend);

#endif

}

int Forklift::get_tracking_id()

{

  return read_addr(FORKLIFT_TRACKING_ID);

}

int Forklift::get_serial_number()

{

  return read_addr(FORKLIFT_SERIAL_NUMBER);

}

int Forklift::get_height()

{

  return read_addr(FORKLIFT_HEIGHT);

}

int Forklift::get_height_setpoint()

{

  return read_addr(FORKLIFT_HEIGHT_SETPOINT);

}

int Forklift::set_height_setpoint(char setpoint)

{

  return write_addr(FORKLIFT_HEIGHT_SETPOINT, setpoint);

}