//=========================================================
// ESBE ARA600 series servo controller
//=========================================================

//----------------------------------------------------------------------------------------------------------------------------------
// To control the ESBE ARA600 series servo instructions are
// send via the serial port.
//
// Instructions are build up with single characters
//
// To allow for multiple servos the first part of an instruction is
// Sx where x ranges from 1 to 4 (This version only takes id 1)
//
// Then each servo can turn clock wise (open) or counter clock wise (close) for a certain time in seconds
// Oxxx for clock wise motion
// Cxxx for counter clock wise motion
// H for stopping the motor (halt) Is an emergency stop since the other instructions are self stopping
//
// xxx ranges from 001 to 120
//
// Feedback is given by repeating the instruction back to the host after it has been executed
//
// When the servo stops on reaching its limit the halt instruction is send to the host
//
//----------------------------------------------------------------------------------------------------------------------------------

#define F_CPU 16000000UL
#ifndef __AVR_ATmega328P__
  #define __AVR_ATmega328P__
#endif

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

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

uint8_t usart_tx[32];                                 //Buffer for USART transmit data
uint8_t volatile usart_tx_in_idx = 0;                 //Index for putting data into the USART transmit buffer
uint8_t volatile usart_tx_out_idx = 0;                //Index for taking data from the USART transmit buffer. Set volatile since it changes in interrupt routine.

uint8_t  volatile usart_rx_state = 0;                 //State indicator for the receiver state machine

uint8_t  volatile servo_id = 0;                       //Current servo id
uint8_t  volatile servo_execute = 0;                  //Flag to signal command needs to be executed
uint8_t  volatile servo_move_command = 0;             //Current servo move command
uint8_t  volatile servo_low_detect_timer = 0;         //Timer for detecting already on servo limit detection when motor started
uint8_t  volatile servo_high_detect_timer = 0;        //Timer for detecting servo really stopped
uint8_t  volatile servo_motor_running = 0;            //Flag to signal motor is running
uint16_t volatile servo_move_time = 0;                //Current servo move time

uint8_t  volatile led_timer = 0;                      //Timer counter for blinking the led

#define USART_RX_STATE_WAIT_SERVO_SELECT      0
#define USART_RX_STATE_WAIT_SERVO_ID          1
#define USART_RX_STATE_WAIT_MOVE_COMMAND      2
#define USART_RX_STATE_WAIT_MOVE_TIME_0       3
#define USART_RX_STATE_WAIT_MOVE_TIME_1       4
#define USART_RX_STATE_WAIT_MOVE_TIME_2       5

#define SERVO_MOVE_NO_ACTION                  0
#define SERVO_MOVE_CLOCK_WISE                 'O'
#define SERVO_MOVE_COUNTER_CLOCK_WISE         'C'
#define SERVO_MOVE_HALT                       'H'
#define SERVO_MOVE_ON_LIMIT                   'L'

//Put a string in the send buffer
void Usart_send(uint8_t *str)
{
  while(*str)
  {
    //Put the character in the transmit buffer and move to next free location
    usart_tx[usart_tx_in_idx++] = *str++;

    //Keep index in range of buffer size
    usart_tx_in_idx %= sizeof(usart_tx);
  }
  
  //Check if interrupt is disabled
  if((UCSR0B & 0x20) == 0)
  {
    //If so enable the interrupt
    UCSR0B |= 0x20;
  }
}

//Control a servo
void Servo_execute_command()
{
  uint8_t str[8];
  
  //Turn the servo off before setting new state
  PORTD = 0x0C;
  
  //Signal motor is no longer running
  servo_motor_running = 0;
  
  //Wait 5ms to allow the triacs to settle
  _delay_ms(5);
  
  //Set the state of the outputs based on the given command
  switch(servo_move_command)
  {
    case SERVO_MOVE_CLOCK_WISE:
      //Keep counter clock wise output high. A low turns the channel on.
      PORTD = 0x04;
      
      //Motor running input should go low within 50ms (5 timer ticks)
      servo_low_detect_timer = 5;
      break;
      
    case SERVO_MOVE_COUNTER_CLOCK_WISE:
      //Keep clock wise output high. A low turns the channel on.
      PORTD = 0x08;
      
      //Motor running input should go low within 50ms (5 timer ticks)
      servo_low_detect_timer = 5;
      break;
  }

  //Echo the servo move command after execution
  snprintf(str, sizeof(str), "S%c%c", servo_id, servo_move_command);
  
  Usart_send(str);
}

int main(void)
{
  //Reset the receiver state machine
  usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
  
  //Reset the servo id and  move command
  servo_id = 0;
  servo_move_command = SERVO_MOVE_NO_ACTION;
  servo_move_time = 0;
  servo_execute = 0;
  servo_low_detect_timer = 0;
  servo_high_detect_timer = 0;
  servo_motor_running = 0;  
  
  //Reset the led timer
  led_timer = 0;
  
  //Clear the transmit buffer
  usart_tx_in_idx = 0;
  usart_tx_out_idx = 0;
  memset(usart_tx, 0, sizeof(usart_tx));

  //9600 baud uses double speed and clear any pending interrupts and errors
  UBRR0 = 207;
  UCSR0A = 0xC2;

  //Enable needed interrupts and bidirectional communication
  UCSR0B = 0x98;

  //Set usart to async, no parity, 8 data bits and 1 stop bit
  UCSR0C = 0x06;
  
  //Led is off
  PORTB = 0x00;
  
  //Led port pin 5 set to output
  DDRB = 0x20;

  //Servo pins high for off condition
  PORTD = 0x0C;
  
  //Servo port pin 2 and 3 set to output
  DDRD = 0x0C;

  //Setup timer 1 for timing. For 10ms the compare needs to be set to 2500 with the prescaler on 64
  TCCR1A = 0x00;  //No outputs enabled
  TCCR1B = 0x0B;  //CTC mode with prescaler on 64
  OCR1A  = 2500;  //Compare value set to 2500
  TIMSK1 = 0x02;  //Enable timer compare interrupt
  TIFR1  = 0x27;  //Clear all pending interrupts
  
  //enable global interrupts when initialization is done
  SREG |= 0x80;
  
  //Keep on going for ever
  while(1)
  {
    //Check if a servo command needs to be executed
    if(servo_execute)
    {
      //Execute the given servo command
      Servo_execute_command();
      
      //Signal command has been executed
      servo_execute = 0;
    }
    
    //Toggle the led pin when needed
    if(led_timer > 50)
    {
      led_timer = 0;
      PINB = 0x20;
    }
  }
}

//Timer 1 interrupt handler
ISR(TIMER1_COMPA_vect)
{
  //Sample the servo motor running input
  uint8_t motor_running_pin = PIND & 0x10;
  
  //Add one to the led timer to keep it blinking
  led_timer++;
  
  //Handle servo halt timing
  if(servo_move_time)
  {
    //Take one tick of
    servo_move_time--;
    
    //Check if it reached zero
    if(servo_move_time == 0)
    {
      //Issue a halt command if so
      servo_move_command = SERVO_MOVE_HALT;
      servo_execute = 1;
    }
  }
  
  //Handle motor running low detect
  if(servo_low_detect_timer)
  {
    //Take one tick of
    servo_low_detect_timer--;
    
    //Check if timer reached zero and the pin is still high
    if(servo_low_detect_timer == 0)
    {
      //Check if pin still high
      if(motor_running_pin)
      {
        //Issue an on limit command if so
        servo_move_command = SERVO_MOVE_ON_LIMIT;
        servo_move_time = 0;
        servo_execute = 1;
      }
      else
      {
        //Mark motor as running
        servo_motor_running = 1;
      }
    }
  }

  //Handle motor running low detect
  if(servo_high_detect_timer)
  {
    //Take one tick of
    servo_high_detect_timer--;
    
    //Check if pin went low again
    if(motor_running_pin == 0)
    {
      //Stop high detection if so
      servo_high_detect_timer = 0;
    }
    else
    {
      //Check if pin was high all the time
      if(servo_high_detect_timer == 0)
      {
        //Check if motor was still running
        if((servo_move_command == SERVO_MOVE_CLOCK_WISE) || (servo_move_command == SERVO_MOVE_COUNTER_CLOCK_WISE))
        {
          //Issue an on limit command if so
          servo_move_command = SERVO_MOVE_ON_LIMIT;
          servo_move_time = 0;
          servo_execute = 1;
        }
      }
    }
  }
  else
  {
    //Handle motor stopped detection when not already in high detection mode
    if(servo_motor_running && motor_running_pin)
    {
      //When motor is running and the input is high start detection if it is actually stopped
      servo_high_detect_timer = 3;
    }
  }
  
  //clear the interrupt flag    
  TIFR1 = 0x02;
}

//Data received interrupt routine
ISR(USART_RX_vect)
{
  //Get the data from the receive register
  uint8_t c = UDR0;
  
  //Handle the received data through a state machine
  switch(usart_rx_state)
  {
    case USART_RX_STATE_WAIT_SERVO_SELECT:
      //Check if a servo select is received
      if(c == 'S')
      {
        //If so wait for the servo id to come in
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_ID;
      }
      else
      {
        //Signal base command error
        Usart_send("E0");
      }
      break;
    
    case USART_RX_STATE_WAIT_SERVO_ID:
      //Check if a valid servo id is received
      if(c == '1')
      {
        //Set the servo id
        servo_id = c;
        
        //If so wait for a servo move command to come in
        usart_rx_state = USART_RX_STATE_WAIT_MOVE_COMMAND;
      }
      else
      {
        //Signal servo id error
        Usart_send("E1");
        
        //Reset state machine to wait for servo select
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      }
      break;
    
    case USART_RX_STATE_WAIT_MOVE_COMMAND:
      //Check if a valid move command is received
      if(c == 'O')
      {
        //Set the command to execute and reset the move time
        servo_move_command = SERVO_MOVE_CLOCK_WISE;
        servo_move_time = 0;
       
        //If open command wait for the first digit of the servo move time to come in
        usart_rx_state = USART_RX_STATE_WAIT_MOVE_TIME_0;
      }
      else if(c == 'C')
      {
        //Set the command to execute and reset the move time
        servo_move_command = SERVO_MOVE_COUNTER_CLOCK_WISE;
        servo_move_time = 0;
        
        //If close command wait for the first digit of the servo move time to come in
        usart_rx_state = USART_RX_STATE_WAIT_MOVE_TIME_0;
      }
      else if(c == 'H')
      {
        //Set the command to execute with zero run time and flagged to execute
        servo_move_command = SERVO_MOVE_HALT;
        servo_move_time = 0;
        servo_execute = 1;
        
        //Reset state machine to wait for servo select
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      }
      else
      {
        //Signal servo move command error
        Usart_send("E2");
        
        //Reset state machine to wait for servo select
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      }
      break;
    
    case USART_RX_STATE_WAIT_MOVE_TIME_0:
      //Check if a valid first servo move time digit is received
      if((c == '0') || (c == '1'))
      {
        //If so wait for the second digit of the servo move time to come in
        usart_rx_state = USART_RX_STATE_WAIT_MOVE_TIME_1;
        
        //Add the received digit tot the servo move time
        servo_move_time += (c - '0') * 100;
      }
      else
      {
        //Signal servo first move time digit error
        Usart_send("E3");
        
        //Reset state machine to wait for servo select
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      }
      break;
    
    case USART_RX_STATE_WAIT_MOVE_TIME_1:
      //Check if a valid second servo move time digit is received
      if((c >= '0') && (c <= '9'))
      {
        //If so wait for the third digit of the servo move time to come in
        usart_rx_state = USART_RX_STATE_WAIT_MOVE_TIME_2;
        
        //Add the received digit tot the servo move time
        servo_move_time += (c - '0') * 10;
      }
      else
      {
        //Signal servo second move time digit error
        Usart_send("E4");
        
        //Reset state machine to wait for servo select
        usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      }
      break;
    
    case USART_RX_STATE_WAIT_MOVE_TIME_2:
      //Check if a valid third servo move time digit is received
      if((c >= '0') && (c <= '9'))
      {
        //Add the received digit tot the servo move time
        servo_move_time += (c - '0');
        
        //Check if valid time is received
        if((servo_move_time > 0) && (servo_move_time <= 130))
        {
          //Adjust the time to 10th of milliseconds and signal execution is needed
          servo_move_time *= 100;
          servo_execute = 1;
        }
        else
        {
          //Signal servo move time zero or to long error
          Usart_send("E6");
        }
      }
      else
      {
        //Signal servo third move time digit error
        Usart_send("E5");
      }
        
      //Reset state machine to wait for servo select
      usart_rx_state = USART_RX_STATE_WAIT_SERVO_SELECT;
      break;
  }
  
}

//Data register empty interrupt routine
ISR(USART_UDRE_vect)
{
  //Send the byte and point to the next one
  UDR0 = usart_tx[usart_tx_out_idx++];

  //Keep pointer in range of the buffer
  usart_tx_out_idx %= sizeof(usart_tx);    
  
  //Check if more data needs to be send
  if(usart_tx_out_idx == usart_tx_in_idx)
  {
    //Disable the data register empty interrupt
    UCSR0B &= 0xDF;
  }
}