Colony

#include "hardware_dynamos.h"

volatile int16_t count_l=0, count_r=0;

void dynamos_init (void)

{

  //enable pin change interrupts 4 and 5 (correspond to DIO2 and DIO3)

  //to trigger on both edges

  cli();

  EICRB |= _BV(ISC40) | _BV(ISC50);

  EIMSK |= _BV(INT4) | _BV(INT5);

  sei();

  //enable the 5V switching regulator to power the dynamos

  DDRB |= _BV(PB4);

  PORTB &= ~_BV(PB4);

}

void dynamos_reset (void)

{

  SYNC

  {

    count_l = 0;

    count_r = 0;

  }

}

ISR(INT4_vect)

{

  uint8_t temp = PINE;

  if(((temp&_BV(PE4))>>2)^(temp&_BV(PE2)))

    count_r--;

  else

    count_r++;

}

ISR(INT5_vect)

{

  uint8_t temp = PINE;

  if(((temp&_BV(PE5))>>2)^(temp&_BV(PE3)))

    count_l++;

  else

    count_l--;

}

void dynamos_read (int16_t *left, int16_t *right)

{

  SYNC

  {

        *left= count_l;

    *right = count_r;

  }

}