Colony » Colony Scout » Colony Scout EE

/*

 * ARM -> us

 * -height setpoint

 *

 * us -> ARM

 * -tracking ID (same for all forklifts)

 * -serial number (unique to each forklift)

 * -current height

 * -current height setpoint

 * -payload weight

 *

 */

#include "twi.h"

#include "analog.h"

#define TRACKING_ID 0x41

#define SERIAL_NUMBER 0x12

// indicies for forklift internal data

#define FORKLIFT_TRACKING_ID     0

#define FORKLIFT_SERIAL_NUMBER   1

#define FORKLIFT_HEIGHT          2

#define FORKLIFT_HEIGHT_SETPOINT 3 // only one that's r/w

#define FORKLIFT_LINEPOS         4

#define FORKLIFT_DATA_LEN        5

uint8_t internal_index = 0;

uint8_t internal_data[] = {

  TRACKING_ID,

  SERIAL_NUMBER,

  0,

  0,

  0

};

void slave_rx(uint8_t* data, int len)

{

  if (len > 0 && data[0] < FORKLIFT_DATA_LEN)

  {

    internal_index = data[0];

    if (len > 1 && internal_index == FORKLIFT_HEIGHT_SETPOINT)

      internal_data[internal_index] = data[1];

  }

}

void slave_tx(void)

{

  twi_transmit(&internal_data[internal_index], 1);

  internal_index++;

  if (internal_index >= FORKLIFT_DATA_LEN)

    internal_index = 0;

}

int main()

{

  twi_init();

  twi_setAddress(TRACKING_ID);

  twi_attachSlaveRxEvent(slave_rx);

  twi_attachSlaveTxEvent(slave_tx);

  analog_init();

  while (1)

  {

    // do motor stuff

    internal_data[FORKLIFT_LINEPOS] = line_read_pos();

    internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);

  }

  return 0;

}