Colony

#include <encoders.h>

  encoders_init();

void turntable_rotate_to_position (int16_t position)

  usb_puts ("# Turntable rotating to ");

  usb_puti (position);

  usb_puts (NL);

  // FIXME implement

  usb_puts ("# Turntable position reached" NL);

  //TODO: convert units

  return encoder_read(RIGHT);

/*

 * position: 10 bit angle, 0 is facing directly in to the station

 *     positive numbers are counterclockwise looking down on the robot

 */

void turntable_rotate_to_position (int16_t position);

#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_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 dynamos_read (int16_t *left, int16_t *right)

{

  SYNC

  {

	*left= count_l;

    *right = count_r;

  }

}

#ifndef _hardware_dynamos_h

#define _hardware_dynamos_h

#include <dragonfly_lib.h>

#include "global.h"

void dynamos_init (void);

void dynamos_read (int16_t *left, int16_t *right);

void dynamos_reset (void);

#endif