/* $Id: main.c,v 1.1 2007/08/02 00:46:27 bsd Exp $ */

/*
 * Dual RX50 test program w/encoder feedback for MAVRIC-IB or
 * MAVRIC-IIB
 *
 * Demonstration program only - this program commands the motors to
 * spin up to full speed and back down, reversing, and repeat.  This
 * program can be used as the basis for advanced motor control using
 * PID and other algorithms.
 *
 *
 * NOTES:
 *
 *   Uses locked anti-phase h-bridge drive
 *
 *   Full quadrature decoding supported, clocks at 4 times native
 *   encoder resolution
 *
 *
 * CONNECTIONS:
 *
 *   Motor 0 PWM : PORTB6    -> RX50 IN1   PWM
 *   Motor 0 ENA : PORTD6    -> RX50 EN1   Enable 
 *
 *   Motor 1 PWM : PORTB7    -> RX50 IN1   PWM
 *   Motor 1 ENA : PORTD7    -> RX50 EN1   Enable
 *
 *
 *   Encoder 0 Phase A : PORTE4
 *   Encoder 0 Phase B : PORTE5
 *
 *   Encoder 1 Phase A : PORTE6
 *   Encoder 1 Phase B : PORTE7
 *
 *
 * RX50 JUMPERS:
 *
 *   EN1-2  : installed
 *   IN1-/2 : installed
 *
 *
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#include <stdio.h>
#include <string.h>
#include <math.h>


#define CPU_FREQ 16000000L



/*
 * MOTOR STUFF --------------------------------------------------
 */

#define M0_PWM  OCR1B    /* motor 0 pwm register */
#define M1_PWM  OCR1C    /* motor 1 pwm register */

#define M0_PWM_DDR   DDRB
#define M0_PWM_PORT PORTB
#define M0_PWM_BIT      6

#define M0_ENA_DDR   DDRD
#define M0_ENA_PORT PORTD
#define M0_ENA_BIT      6

#define M1_PWM_DDR   DDRB
#define M1_PWM_PORT PORTB
#define M1_PWM_BIT      7

#define M1_ENA_DDR   DDRD
#define M1_ENA_PORT PORTD
#define M1_ENA_BIT      7


#define M0_ENA() M0_ENA_PORT |=  _BV(M0_ENA_BIT)
#define M0_DIS() M0_ENA_PORT &= ~_BV(M0_ENA_BIT)

#define M1_ENA() M1_ENA_PORT |=  _BV(M1_ENA_BIT)
#define M1_DIS() M1_ENA_PORT &= ~_BV(M1_ENA_BIT)


void motor_speed(int motor, int16_t speed)
{
  int32_t p;

  p = 512 + (int32_t)speed;
  if (p < 0)
    p = 0;
  else if (p > 1023)
    p = 1023;

  cli();
  if (motor == 0)
    M0_PWM = p;
  else 
    M1_PWM = p;
  sei();
}


void motor_enable(int n, int enable)
{
  if (enable) {
    if (n == 0) {
      M0_ENA();
    }
    else {
      M1_ENA();
    }
  }
  else {
    if (n == 0) {
      M0_DIS();
    }
    else {
      M1_DIS();
    }
  }
}


void motors_init(void)
{
  /* h-bridge enable pins are outputs */
  M0_ENA_DDR |= _BV(M0_ENA_BIT);
  M1_ENA_DDR |= _BV(M1_ENA_BIT);

  /* disable h-bridges to start */
  motor_enable(0, 0);
  motor_enable(1, 0);

  /* set up timer 1 and PORTB6 and PORTB7 for h-bridge PWM control */

  /* user timer mode 7 = fast PWM mode (see datasheet) WGM13=0,
   * WGM12=1, WGM11=1, WGM10=1, prescale=1 (no prescale) = ~16 kHz
   * frequency */
  TCCR1B &= ~(_BV(WGM13)|_BV(CS12)|_BV(CS11));
  TCCR1B |=  _BV(WGM12)|_BV(CS10);
  TCCR1A &= ~(_BV(COM1B0)|_BV(COM1C0));
  TCCR1A |=  _BV(WGM11)|_BV(WGM10) | _BV(COM1B1)|_BV(COM1C1);

  /* h-bridge PWM pins are outputs */
  M0_PWM_DDR |= _BV(M0_PWM_BIT);
  M1_PWM_DDR |= _BV(M1_PWM_BIT);

  /* set motor speed to zero (50% duty for locked antiphase) */
  motor_speed(0, 0);
  motor_speed(1, 0);
}



/*
 * ENCODER STUFF --------------------------------------------------
 */

#define E1PHA_INT        4
#define E1PHA_VECTOR SIG_INTERRUPT4
#define E1PHA_DDR     DDRE
#define E1PHA_PORT   PORTE
#define E1PHA_PIN     PINE
#define E1PHA_BIT        4
#define E1PHA_INTEN  { EICRB |= _BV(ISC##E1PHA_INT##1); \
                       EICRB &= ~_BV(ISC##E1PHA_INT##0); }

#define E1PHB_INT        5
#define E1PHB_VECTOR SIG_INTERRUPT5
#define E1PHB_DDR     DDRE
#define E1PHB_PORT   PORTE
#define E1PHB_PIN     PINE
#define E1PHB_BIT        5
#define E1PHB_INTEN  { EICRB |= _BV(ISC##E1PHB_INT##1); \
                       EICRB &= ~_BV(ISC##E1PHB_INT##0); }

#define E2PHA_INT        6
#define E2PHA_VECTOR SIG_INTERRUPT6
#define E2PHA_DDR     DDRE
#define E2PHA_PORT   PORTE
#define E2PHA_PIN     PINE
#define E2PHA_BIT        6
#define E2PHA_INTEN  { EICRB |= _BV(ISC##E2PHA_INT##1); \
                       EICRB &= ~_BV(ISC##E2PHA_INT##0); }

#define E2PHB_INT        7
#define E2PHB_VECTOR SIG_INTERRUPT7
#define E2PHB_DDR     DDRE
#define E2PHB_PORT   PORTE
#define E2PHB_PIN     PINE
#define E2PHB_BIT        7
#define E2PHB_INTEN  { EICRB |= _BV(ISC##E2PHB_INT##1); \
                       EICRB &= ~_BV(ISC##E2PHB_INT##0); }

volatile int32_t m0_pos;
volatile int32_t m1_pos;

void encoders_init(void)
{
  /* encoder pins are inputs */
  E1PHA_DDR &= ~(1 << E1PHA_BIT);
  E1PHB_DDR &= ~(1 << E1PHB_BIT);
  E2PHA_DDR &= ~(1 << E2PHA_BIT);
  E2PHB_DDR &= ~(1 << E2PHB_BIT);

  /* enable pull-ups */
  E1PHA_PORT |= 1 << E1PHA_BIT;
  E1PHB_PORT |= 1 << E1PHB_BIT;
  E2PHA_PORT |= 1 << E2PHA_BIT;
  E2PHB_PORT |= 1 << E2PHB_BIT;

  /* enable encoder interrupts */
  EIMSK |= (1<<INT5) | (1<<INT7);
  EICRB &= ~((1<<ISC50)|(1<<ISC70));
  EICRB |=  ((1<<ISC51)|(1<<ISC71));
}

SIGNAL(E1PHB_VECTOR) /* phase b interrupt */
{
  if (E1PHA_PIN & (1 << E1PHA_BIT)) {
    m0_pos--;
  }
  else {
    m0_pos++;
  }
}

SIGNAL(E2PHB_VECTOR) /* phase b interrupt */
{
  if (E2PHA_PIN & (1 << E2PHA_BIT)) {
    m1_pos--;
  }
  else {
    m1_pos++;
  }
}



/*
 * MS TIMER STUFF --------------------------------------------------
 */

volatile uint16_t ms_count;

/*
 * ms_sleep() - delay for specified number of milliseconds
 */
void ms_sleep(uint16_t ms)
{
  TCNT0  = 0;
  ms_count = 0;
  while (ms_count != ms)
    ;
}

/* 
 * millisecond counter interrupt vector 
 */
SIGNAL(SIG_OUTPUT_COMPARE0)
{
  ms_count++;
}

void mstimer_init(void)
{
  /* set up a millisecond timer on timer 0 for time keeping purposes */
  TIFR  |= _BV(OCIE0);
  TCCR0  = _BV(WGM01)|_BV(CS02)|_BV(CS00); /* CTC, prescale = 128 */
  TCNT0  = 0;
  TIMSK |= _BV(OCIE0);    /* enable output compare interrupt */
  OCR0   = 125;           /* match in 1 ms */
}



/*
 * A/D CONVERTER STUFF --------------------------------------------------
 */

#define N_ADC_SAMPLES 10   /* # of samples to average */
#define N_CHANNELS     8   /* # of channels to read, starting at 0 */

volatile uint16_t adc_result[N_CHANNELS];  /* conversion results */


static void adc_chsel(const uint8_t channel)
{
  /* select channel */
  ADMUX = (ADMUX & 0xe0) | (channel & 0x07);
}


/*
 * A/D converter interrupt vector - when called, the last conversion
 * has completed, either add the current result to the average, or
 * switch to the next channel if it is time to do that.  Always throw
 * away the first conversion after a channel switch as per datasheet
 * recommendations.
 */
SIGNAL(SIG_ADC)
{
  static uint32_t result = 0;
  static uint8_t i = 0;
  static uint8_t channel = 0;
  static uint8_t channel_switch = 1;

  if (channel_switch) {
    channel_switch = 0;
    return;
  }
  
  result += ADC;

  i++;
  if (i >= N_ADC_SAMPLES) {
    adc_result[channel] = ((uint32_t)result / (uint32_t)N_ADC_SAMPLES);
    result = 0;
    i = 0;
    channel++;
    if (channel >= N_CHANNELS)
      channel = 0;
    adc_chsel(channel);
    channel_switch = 1;
  }
}


/*
 * initialize A/D converter to free-run, generating an interrupt upon
 * conversion complete.
 */
void adc_init(void)
{
  /* configure ADC port as input */
  DDRF  = 0x00;
  PORTF = 0x00;

  ADMUX = _BV(REFS0);
  adc_chsel(0);

  ADCSR = _BV(ADEN)|_BV(ADSC)|_BV(ADFR)|_BV(ADIE)|_BV(ADIF) | 
    _BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0);
}




/* 
 * UART STUFF --------------------------------------------------
 */
#define BAUDRR(f,b) ((f % (16L*b) >= (16L*b)/2) ? \
                     (f / (16L*b)) : (f / (16L*b)) - 1)

void uart_setbaud(long baud)
{
  uint16_t brr;

  brr = BAUDRR(CPU_FREQ, baud);
  UBRR1H = (brr >> 8);
  UBRR1L = (brr & 0xff);
}

int uart_putc(char c)
{
  /* wait until shift buffer empty */
  while ((UCSR1A & _BV(UDRE)) == 0)
    ;

  UCSR1A |= _BV(TXC); /* reset transmit complete flag */
  UDR1 = c;           /* load char for transmit */

  return 0;
}


int uputc(char c, FILE * f)
{
  return uart_putc(c);
}


void uart_init(void)
{
  uart_setbaud(9600L);
  UCSR1B = _BV(TXEN); /* enable just the transmitter */
}




#define SLP 10

int main(void)
{
  int speed;
  int i;

  mstimer_init();
  motors_init();
  encoders_init();
  uart_init();
  adc_init();

  sei();

  ms_sleep(100);

  fdevopen(uputc, NULL);

  printf_P(PSTR("Motor and Encoder test program\n"));

  /* enable motors */
  motor_enable(0, 1);
  motor_enable(1, 1);

  while (1) {
    for (speed = 0; speed < 1024; speed += 10) {
      ms_sleep(SLP);
      motor_speed(0, speed);
      motor_speed(1, -speed);
      printf("M0 = %7ld   M1 = %7ld  ", m0_pos, m1_pos);
      for (i=0; i<N_CHANNELS; i++) {
        printf("CH%d=%4d  ", i, adc_result[i]);
      }
      printf("\n");
    }

    for (speed = 1023; speed > 0; speed -= 10) {
      ms_sleep(SLP);
      motor_speed(0, speed);
      motor_speed(1, -speed);
      printf("M0 = %7ld   M1 = %7ld  ", m0_pos, m1_pos);
      for (i=0; i<N_CHANNELS; i++) {
        printf("CH%d=%4d  ", i, adc_result[i]);
      }
      printf("\n");
    }

    for (speed = 0; speed < 1024; speed += 10) {
      ms_sleep(SLP);
      motor_speed(0, -speed);
      motor_speed(1, speed);
      printf("M0=%7ld   M1=%7ld  ", m0_pos, m1_pos);
      for (i=0; i<N_CHANNELS; i++) {
        printf("CH%d=%4d  ", i, adc_result[i]);
      }
      printf("\n");
    }
  
    for (speed = 1023; speed > 0; speed -= 10) {
      ms_sleep(SLP);
      motor_speed(0, -speed);
      motor_speed(1, speed);
      printf("M0 = %7ld   M1 = %7ld  ", m0_pos, m1_pos);
      for (i=0; i<N_CHANNELS; i++) {
        printf("CH%d=%4d  ", i, adc_result[i]);
      }
      printf("\n");
    }

    ms_sleep(SLP);
    motor_speed(0, 0);
    motor_speed(1, 0);

    printf("M0 = %7ld   M1 = %7ld  ", m0_pos, m1_pos);
    for (i=0; i<N_CHANNELS; i++) {
      printf("CH%d=%4d  ", i, adc_result[i]);
    }
    printf("\n");

    ms_sleep(1000);
  }
}