Colony

#include "hardware_turntable.h"

#include <encoders.h>

#define TURNTABLE_ENCODER_ZERO 737

#define MOTOR_TURNTABLE_SET motor1_set

#define TURNTABLE_ENCODER RIGHT

#define TURNTABLE_MAX_PWM 255

#define TURNTABLE_TOLERANCE 5

#define TURNTABLE_STABLE_READINGS 100

//180 seems to be just below

#define TURNTABLE_PWM_OFFSET 160

#define TURNTABLE_P 2

//this seems like it should be between 50:200

//100 is a little slow, but probably good

#define TURNTABLE_I_DIV 100

int16_t turntable_encoder_offset = 0;

void turntable_init ()

{

  int e;

  encoders_init();

  encoder_wait(1);

  // get a reading to check alignment

  e = encoder_read(TURNTABLE_ENCODER);

  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);

  //get the offset

  turntable_encoder_offset = e - TURNTABLE_ENCODER_ZERO;

  //make it be less than 512

  while(turntable_encoder_offset > 512)

    turntable_encoder_offset -= 1024;

  while(turntable_encoder_offset < -512)

    turntable_encoder_offset += 1024;

  motors_init();

}

void turntable_rotate_to_position (int16_t position)

{

  int16_t turntable=0;

  uint8_t direction=0, last_direction=0;

  int16_t error = 0;

  //must be 16 bit to check for overflow

  uint16_t power=0;

  uint8_t cycle_count=0;

  usb_puts ("# Turntable rotating to ");

  usb_puti (position);

  usb_puts (NL);

  /*  MOTOR_TURNTABLE_SET(1, TURNTABLE_PWM_OFFSET);

  delay_ms(1000);

  return;

  */

  while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE)

  {

    while(abs((turntable = turntable_get_position()) - position) > TURNTABLE_TOLERANCE)

    {

      //figure out which direction to set the motor

      last_direction = direction;

      direction = turntable < position;

      //clear the accumuliated error when switching directions

      if(last_direction != direction)

        error = 0;

//only add to the error every TURNTABLE_I_DIV cycles

        if(++cycle_count > TURNTABLE_I_DIV)

        {

          error += turntable - position;

          cycle_count = 0;

        }

        power = abs(TURNTABLE_P*(turntable - position) + error) + TURNTABLE_PWM_OFFSET;

        if(power > TURNTABLE_MAX_PWM)

          power = TURNTABLE_MAX_PWM;

        MOTOR_TURNTABLE_SET(direction, power);

        /*     usb_puti(turntable); */

        /*     usb_puts(NL); */

        if(turntable >= 1024)

        {

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

          break;

        }

        usb_puti(error);

        usb_puts(" ");

        usb_puti(direction);

        usb_puts(" ");

        usb_puti(power);

        usb_puts(NL);

        delay_ms(1);

        encoder_wait(1);

    }

    encoder_wait(TURNTABLE_STABLE_READINGS);

  }

  MOTOR_TURNTABLE_SET(direction, 0);

  //just to prevent resetting

  delay_ms(50);

  usb_puts ("# Turntable position reached" NL);

}

uint16_t turntable_get_position (void)

{

  return encoder_get_dx(TURNTABLE_ENCODER) + turntable_encoder_offset;

  //  return encoder_read(TURNTABLE_ENCODER);

}