Colony

#include "hardware_turntable.h"

#include <encoders.h>

void turntable_init ()

{

  int e;

  encoders_init();

  // get a reading to check alignment

  e = encoder_read(LEFT);

  if(e==ENCODER_MAGNET_FAILURE)

    usb_puts("# ERROR: turntable encoder magnet failure!!" NL);

  else if(e==ENCODER_MISALIGNED)

    usb_puts("# ERROR: turntable encoder misaligned!!" NL);

  else if(e==ENCODER_DATA_NOT_READY)

    usb_puts("# ERROR: turntable encoder data not ready!!" NL);

  else

    usb_puts("# Turntable encoder initialized" NL);

  motors_init();

}

void turntable_rotate_to_position (int16_t position)

{

  uint16_t turntable;

  return;

  usb_puts ("# Turntable rotating to ");

  usb_puti (position);

  usb_puts (NL);

  //TODO: p control

  //TODO: proper direction

  motor1_set(1, 180);

  while((turntable = turntable_get_position()) <= position)

  {

    delay_ms(1);

    usb_puti(turntable);

    usb_puts(NL);

    if(turntable >= 1024)

    {

      usb_puts("# ERROR: magnetic encoder error!" NL);

      break;

    }

  }

  motor1_set(1, 0);

  usb_puts ("# Turntable position reached" NL);

}

uint16_t turntable_get_position (void)

{

  //TODO: convert units

  return encoder_read(RIGHT);

}