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"

#include "motor.h"

#include <avr/io.h>

#include <avr/interrupt.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 // 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

#define PROP(X,Y,Z)                (((X)*(Y))/(Z))

uint8_t internal_index = 0;

uint8_t internal_data[] = {

  TRACKING_ID,

  SERIAL_NUMBER,

  0,

  0,

  0,

  0,

  150

};

int error;

int i_term;

void init_int0(void)

{

  TCCR0B = (1<<CS02)|(1<<CS00); //Timer clock = system clock / 1024

  TIFR0 = 1<<TOV0; //Clear TOV0  clear pending interrupts

  TIMSK0 = 1<<TOIE0; //Enable Timer0 Overflow Interrupt

}

void slave_rx(uint8_t* data, int len)

{

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

  {

    internal_index = data[0];

    if (len > 1)

        {

          if (internal_index == FORKLIFT_HEIGHT_SETPOINT)

        internal_data[internal_index] = data[1];

      else if (internal_index == FORKLIFT_LINE_THRESH_HIGH)

          {

            internal_data[internal_index] = data[1];

                line_set_threshold_high(data[1]);

          }

      else if (internal_index == FORKLIFT_LINE_THRESH_LOW)

          {

            internal_data[internal_index] = data[1];

                line_set_threshold_low(data[1]);

          }

        }

  }

}

void slave_tx(void)

{

  twi_transmit(&internal_data[internal_index], 1);

  internal_index++;

  if (internal_index >= FORKLIFT_DATA_LEN)

    internal_index = 0;

}

void update_height(void)

//Call to update the forklift height point. Should be called each loop

{

        internal_data[FORKLIFT_HEIGHT] = analog_read(ADC_HEIGHT);

}

int clamp(int min, int max, int val)

{

        if(val>max) return max;

        if(val<min) return min;

        return val;

}

SIGNAL(TIMER0_OVF_vect)

{

        error = (int)internal_data[FORKLIFT_HEIGHT]

                                                -(int)internal_data[FORKLIFT_HEIGHT_SETPOINT];

        i_term = i_term +  error/10;

        set_motor(clamp(-127,127, i_term/10 + (error*2)/5));

}

int main()

{

  error = 0;

  i_term = 0;

  init_int0();

  sei();

  twi_attachSlaveRxEvent(slave_rx);

  twi_attachSlaveTxEvent(slave_tx);

  twi_setAddress(TRACKING_ID);

  twi_init();

  analog_init();

  motor_init();

  while (1)

  {

          /*error = PROP((int)internal_data[FORKLIFT_HEIGHT]

                                                -(int)internal_data[FORKLIFT_HEIGHT_SETPOINT],2,5);

                set_motor(clamp(-127,127,error));*/

    // do motor stuff

        internal_data[FORKLIFT_LINE_POS] = (analog_read(ADC_LINE)>>2);//line_read_pos();

    internal_data[FORKLIFT_HEIGHT] = (analog_read(ADC_HEIGHT)>>2);

  }

  return 0;

}