Colony

#include "Forklift.h"

#include "libdragonfly/i2c.h"

#include <util/delay.h>

#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

using namespace Forklift;

char volatile response;

char volatile received;

int mrecv(char data)

{

  response = data;

  received = 1;

}

void srecv(char) {}

char ssend() {}

int read_addr(char addr)

{

  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;

}

int write_addr(char addr, char data)

{

  char buf[2];

  buf[0] = addr;

  buf[1] = data;

  if (i2c_send(FORKLIFT_ADDR, buf, 2))

    return FORKLIFT_ERROR;

  else

    return 0;

}

void forklift_init()

{

  i2c_init(SCOUT_AVR_ADDR, mrecv, srecv, ssend);

}

int get_tracking_id()

{

  return read_addr(FORKLIFT_TRACKING_ID);

}

int get_serial_number()

{

  return read_addr(FORKLIFT_SERIAL_NUMBER);

}

int get_height()

{

  return read_addr(FORKLIFT_HEIGHT);

}

int get_height_setpoint()

{

  return read_addr(FORKLIFT_HEIGHT_SETPOINT);

}

int set_height_setpoint(char setpoint)

{

  return write_addr(FORKLIFT_HEIGHT_SETPOINT, setpoint);

}

#ifndef _FORKLIFT_H_

#define _FORKLIFT_H_

#define FORKLIFT_ERROR -1

namespace Forklift

{

  void forklift_init();

  int get_tracking_id(); // 0x41 for all forklifts

  int get_serial_number(); // unique to each forklift

  int get_height();

  int get_height_setpoint();

  int set_height_setpoint(int setpoint);

};

#endif