// target is atmega32
//
// Note JTAG enabled by default. This conflicts with full use of PORTC. To
// disable: (in avrdude terminal) write hfuse 0 0xd9

//
// Pin           Function
//
// PB3 / OC0     steering motor PWM positive
// PD7 / OC2     steering motor PWM negative
//
// PD5 / OC1A    drive motor PWM A
// PD4 / OC1B    drive motor PWM B
//
// PC4           relay coil DPDT reverse A
// PC5           relay coil DPDT reverse B
//
// PD0 / RXD     serial port receive
// PD1 / TXD     serial port transmit
//
// PC2           pulse to piezo rate gyro
// PC3           pulse from piezo rate gyro
//
// AD0           steering potentiometer
// AD1           Sharp IR distance sensor 1
// AD2           Sharp IR distance sensor 2
//
// PC6           general purpose 1
// PC7           general purpose 2
//

// actual clock speed is 1.229 MHz (on chip RC oscillator)
#define F_CPU 1229000

#include <avr/io.h>
#include <avr/delay.h>

#define UART_TXEN_BUF_GYRO     0
#define UART_TXEN_BUF_STEERING 1
#define UART_TXEN_BUF_RANGE    2

#define RECV_FRAME_BYTE 0xff
#define TXEN_FRAME_BYTE 0x00

// 0x55 = 01010101 appears as ...11110101010101111...
#define TEST_FRAME_BYTE 0x55

//
// *** Serial protocol to microcontroller ***
//
// byte 0 - framing byte
// 0xff             - value only occurs as framing byte
//
// byte 1 - command
// 0x03             - set duty cycle for OC0   (steering motor PWM positive)
// 0x0f             - set duty cycle for OC1A  (drive motor PWM A)
// 0xf0             - set duty cycle for OC1B  (drive motor PWM B)
// 0x30             - set duty cycle for OC2   (steering motor PWM negative)
// 0x5e             - set PC4 on/off           (relay coil DPDT reverse A)
// 0x6e             - set PC5 on/off           (relay coil DPDT reverse B)
// 0x5d             - set PC6 on/off
// 0x6d             - set PC7 on/off
// 0x75             - set steering potentiometer target window low value
// 0x76             - set steering potentiometer target window high value
// 0x11             - set pulse width to gyro
// 0x7e             - stop pulse generation
// 0x66             - calibration mode
//
// byte 2 - value or specifier
// 0x00 to 0xfe     - duty cycle for PWM
//                  - delay count for pulse width
//                  - steering potentiometer target window limit value
// 0x0e             - off                                 (relay coil)
// 0xee             - on                                  (relay coil)
// 0xe7             - drive and steering motor plus gyro  (stop pulse)
// 0xe2             - drive motor                         (stop pulse)
// 0xe4             - steering motor                      (stop pulse)
// 0xe6             - gyro                                (stop pulse)
// 0x60             - off                           (calibration mode)
// 0x62             - transmit test frame           (calibration mode)
//
// byte 3 - checksum
// 0x00 to 0xfe     - byte 1 + byte 2 if sum is not 0xff
//                  - 0x00 if sum is 0xff
//
// Note that the checksum can have 255 different values only. At best, there
// is a 1/255 probability that noise causes an undetected error.
//
// *** Serial protocol from microcontroller ***
//
// Voltages are left adjusted ADCH A/D voltage conversion for 8 bit resolution
//
// Each frame looks like:
//
// (calibration mode off)
// byte 0 - pulse width from gyro
// byte 1 - voltage of steering potentiometer divider     (AD0 pin)
// byte 2 - voltage of sharp IR sensors 1 and 2 averaged  (AD1 and AD2 pins)
//
// Jitter accumulates so the earlier bytes are transmitted with fewer errors.
// The order is deliberate and reflects importance.
//
// (calibration mode on)
// byte 0, 1, 2 - TEST_FRAME_BYTE
//

uint8_t uart_recv_buf[3],  // serial receive buffer data (no frame byte)
        uart_txen_buf[3],  // serial transmit buffer data
        pulse_to_gyro,     // delay count for pulse to gyro
	relay_coil_a,      // relay coil controlled by PC4 pin
	relay_coil_b,      // relay coil controlled by PC5 pin
	general_1,         // general purpose PC6 pin
	general_2,         // general purpose PC7 pin
	steering_low,      // window low limit for steering pot to stop motor
	steering_high,     // window high limit for steering pot to stop motor
        calibration_mode,  // calibration mode
        runaway_counter;   // when counter reaches overflows then all stop

//
// Configure I/O ports
//
void init_ports()
{
  // serial port
  UBRRL  = 0x19;  // 2400 baud at 1.229 MHz clock, pulse width is 335-340 us
  UCSRB  = _BV(RXEN) | _BV(TXEN);  // enable receiver and transmitter
  UCSRC  = _BV(URSEL) | _BV(UCSZ1) | _BV(UCSZ0);  // 8-N-1

  // OC0 timer/counter PWM output
  DDRB   = _BV(DDB3);

  // PC2 gyro pulse, PC4/5 relay coil and PC6/7 general purpose
  DDRC   = _BV(DDC2) | _BV(DDC4) | _BV(DDC5) | _BV(DDC6) | _BV(DDC7);

  // OC1B, OC1A, OC2 timer/counter PWM output
  DDRD   = _BV(DDD4) | _BV(DDD5) | _BV(DDD7);

  // 8 bit phase correct PWM, actual frequency is 1.975 kHz
  TCCR0  = _BV(COM01)  | _BV(WGM00)  | _BV(CS00);
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
  TCCR1B = _BV(CS10);
  TCCR2  = _BV(COM21)  | _BV(WGM20)  | _BV(CS20);
}

//
// Read byte from serial port and store in the frame buffer.
// Return true if there is a complete frame, otherwise false.
//
uint8_t complete_recv_frame()
{
  static uint8_t uart_recv_idx = sizeof(uart_recv_buf);

  uint8_t inbyte = UDR;  // read in byte from serial port

  if (inbyte == RECV_FRAME_BYTE)  // new frame detected so reset buffer index
  {
    uart_recv_idx = 0;
  }
  else if (uart_recv_idx < sizeof(uart_recv_buf))
  {
    // store byte in frame buffer
    uart_recv_buf[uart_recv_idx++] = inbyte;

    // return true if there is a complete frame
    return uart_recv_idx == sizeof(uart_recv_buf);
  }

  // the frame is not complete
  return 0;
}

//
// Take appropriate action based on the command and value/specifier in the
// received frame from the serial port. This should only be called when there
// is a complete frame available.
//
void process_recv_frame()
{
  const uint8_t cmd      = uart_recv_buf[0];
  const uint8_t valspec  = uart_recv_buf[1];
  const uint8_t checksum = uart_recv_buf[2];  // should equal cmd + valspec
                                              // except when sum is 0xff

  // only proceed if no checksum error detected
  const uint8_t msgchk = cmd + valspec;
  if ( (msgchk == checksum) || (msgchk == 0xff && checksum == 0x00) )
  {
    uint8_t good_command = 1;  // presume the command is good

    switch (cmd)  // command byte
    {
      // 3
      case (0x03):        // set duty cycle for OC0     (steering motor PWM)
        OCR0  = valspec;
        break;

      // 15
      case (0x0f):        // set duty cycle for OC1A    (drive motor PWM A)
        OCR1A = valspec;
        break;

      // 240
      case (0xf0):        // set duty cycle for OC1B    (drive motor PWM B)
        OCR1B = valspec;
        break;

      // 48
      case (0x30):        // set duty cycle for OC2     (steering motor PWM)
        OCR2  = valspec;
        break;

      // 94
      case (0x5e):        // set PC4 on/off             (relay coil reverse A)
        if (valspec == 0x0e)       // 14 - relay coil off
        {
          relay_coil_a = 0;
        }
        else if (valspec == 0xee)  // 238 - relay coil on
        {
          relay_coil_a = 1;
        }
        break;

      // 110
      case (0x6e):        // set PC5 on/off             (relay coil reverse B)
        if (valspec == 0x0e)       // 14 - relay coil off
        {
          relay_coil_b = 0;
        }
        else if (valspec == 0xee)  // 238 - relay coil on
        {
          relay_coil_b = 1;
        }
        break;

      // 93
      case (0x5d):        // set PC6 on/off
        if (valspec == 0x0e)       // 14 - off
        {
          general_1 = 0;
        }
        else if (valspec == 0xee)  // 238 - on
        {
          general_1 = 1;
        }
        break;

      // 109
      case (0x6d):        // set PC7 on/off
        if (valspec == 0x0e)       // 14 - off
        {
          general_2 = 0;
        }
        else if (valspec == 0xee)  // 238 - on
        {
          general_2 = 1;
        }
        break;

      // 117
      case (0x75):        // set steering potentiometer target window low
	steering_low = valspec;
        break;

      // 118
      case (0x76):        // set steering potentiometer target window high
	steering_high = valspec;
        break;

      // 17
      case (0x11):        // set pulse width to gyro
        pulse_to_gyro = valspec;
        break;

      // 126
      case (0x7e):        // stop pulse generation
        if (valspec == 0xe7)       // 231 - stop all pulses
        {
          OCR0  = 0x00;
          OCR1A = 0x00;
          OCR1B = 0x00;
          OCR2  = 0x00;
          pulse_to_gyro = 0x00;
        }
        else if (valspec == 0xe2)  // 226 - stop drive motors only
        {
          OCR1A = 0x00;
          OCR1B = 0x00;
        }
        else if (valspec == 0xe4)  // 228 - stop steering motor only
        {
          OCR0  = 0x00;
          OCR2  = 0x00;
        }
        else if (valspec == 0xe6)  // 230 - stop pulses to gyro only
        {
          pulse_to_gyro = 0x00;
        }
        break;

      // 102
      case (0x66):        // calibration mode
	if (valspec == 0x60)       // 96 - off
	{
          calibration_mode = 0;
	}
	else if (valspec == 0x62)  // 98 - transmit test frame
	{
          calibration_mode = 1;
	}
	break;

      // unrecognized command
      default:
	good_command = 0;
    }

    // only reset the runaway counter if the command was good
    if (good_command)
    {
      runaway_counter = 0;
    }
  }
}

//
// Process serial port receive and handle commands in input frames
//
void uart_recv_frame()
{
  if (complete_recv_frame())
  {
    process_recv_frame();
  }

  // check if runaway detected - no good command frames after 256 bytes read
  // from serial port
  if (runaway_counter++ == 0xff)
  {
    // stop all pulse generation
    OCR0  = 0x00;
    OCR1A = 0x00;
    OCR1B = 0x00;
    OCR2  = 0x00;
    pulse_to_gyro = 0x00;
  }
}

//
// Transmit one byte from a complete frame over the serial port
//
void uart_txen_frame()
{
  static uint8_t uart_txen_idx = sizeof(uart_txen_buf);

  if (uart_txen_idx < sizeof(uart_txen_buf))
  {
    if (calibration_mode)
    {
      UDR = TEST_FRAME_BYTE;               // transmit test byte
    }
    else
    {
      UDR = uart_txen_buf[uart_txen_idx];  // transmit byte from buffer
    }

    uart_txen_idx++;      // next byte
  } 
  else
  {
    // gyro pulse width reading is unpredictable while the AD conversions
    // occur on every pass so the frame is complete when the gyro pulse is
    // available (as thee AD conversions are always available)
    if (uart_txen_buf[UART_TXEN_BUF_GYRO])
    {
      uart_txen_idx = 0;                   // transmit the frame
    }
  }
}

//
// Generate software delay loop pulses to the steering motor and gyro.
// Output pulses from the gyro are measured and stored.
//
void gen_pulses()
{
  // pin outputs go high
  uint8_t portc = 0x00;
  if (pulse_to_gyro) { portc |= _BV(DDC2); }
  if (relay_coil_a) { portc |= _BV(DDC4); }
  if (relay_coil_b) { portc |= _BV(DDC5); }
  if (general_1) { portc |= _BV(DDC6); }
  if (general_2) { portc |= _BV(DDC7); }
  PORTC = portc;

  uint8_t before_gyro_pulse = 0,
          after_gyro_pulse  = 0,
          gyro_count        = 0,
          i                 = 0,
          doLoop            = 1;

  // main loop to turn pins off and count gyro output pulse width
  while (doLoop)
  {
    // pin outputs go low
    if (i == pulse_to_gyro) { portc &= (uint8_t) ~ _BV(DDC2); PORTC = portc; }

    // is there a pulse from the gyro?
    if (PINC & _BV(PINC3))
    {
      // increment counter of the gyro's output pulse width
      gyro_count++;
    }
    // there is no pulse
    else
    {
      // the gyro pulse will arrive later, the starting edge is not sliced off
      if (gyro_count == 0)
      {
        before_gyro_pulse = 1;
      }
      // the gyro pulse has passed
      else
      {
        after_gyro_pulse = 1;
      }
    }

    // increment index
    if (++i == 0)
    {
      doLoop = 0;
    }
  }

  // partial, sliced gyro pulses are not counted
  if (before_gyro_pulse && after_gyro_pulse)
  {
    // store the reading for later transmission
    uart_txen_buf[UART_TXEN_BUF_GYRO] = gyro_count;
  }
}

//
// Read voltages on pins AD0, AD1, AD2 and store in transmit buffer
//
void ad_conversion()
{
  // steering potentiometer
  ADMUX  = _BV(REFS0) | _BV(ADLAR);              // AD0 pin
  ADCSRA = _BV(ADEN)  | _BV(ADSC);               // start conversion
  while (ADCSRA & _BV(ADSC)) ;                   // wait for result
  uint8_t steering_pot = ADCH;                   // 8 bit resolution
  uart_txen_buf[UART_TXEN_BUF_STEERING] = steering_pot;

  // sharp IR rangefinder 1
  ADMUX  = _BV(REFS0) | _BV(ADLAR) | _BV(MUX0);  // AD1 pin
  ADCSRA = _BV(ADEN)  | _BV(ADSC);               // start conversion
  while (ADCSRA & _BV(ADSC)) ;                   // wait for result
  uint8_t sensor1 = ADCH;                        // 8 bit resolution

  // sharp IR rangefinder 2
  ADMUX  = _BV(REFS0) | _BV(ADLAR) | _BV(MUX1);  // AD2 pin
  ADCSRA = _BV(ADEN)  | _BV(ADSC);               // start conversion
  while (ADCSRA & _BV(ADSC)) ;                   // wait for result
  uint8_t sensor2 = ADCH;                        // 8 bit resolution

  // average range reading
  int sensor_sum = sensor1 + sensor2;
  uart_txen_buf[UART_TXEN_BUF_RANGE] = sensor_sum / 2;  // average value

  // check for steering potentiometer inside of target window
  if ( (steering_pot >= steering_low) && (steering_pot <= steering_high) )
  {
    // stop the steering motor
    OCR0  = 0x00;
    OCR2  = 0x00;
  }
}

//
// Main entry point
//
int main()
{
  // configure I/O ports
  init_ports();

  // loop forever
  while (1)
  {
    // serial port receive and process command frames
    uart_recv_frame();

    // serial port transmit status frames
    uart_txen_frame();

    // generate PC4/5 and gyro pulses
    gen_pulses();

    // analog-digital conversions
    ad_conversion();
  }
}