Colony

#include "hardware_encoders.h"

volatile int16_t count_l=0, count_r=0;

void encoders_init ()

{

  //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 encoders

  DDRB |= _BV(PB4);

  PORTB &= ~_BV(PB4);

}

void encoders_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_l--;

  else

    count_l++;

}

ISR(INT5_vect)

{

  uint8_t temp = PINE;

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

    count_r++;

  else

    count_r--;

}

void encoders_read (int16_t *values)

{

        usb_puts ("# Reading encoder values");

  SYNC

  {

        *values = count_l;

  *(values+1) = count_r;

  }

}