/*****************************
* AK Serial Servo Controller *
******************************
* Author: Aki Korhonen       *
* First version: 18-03-2008  *
* Current:       14-05-2008  *
******************************/

// ------------------------------------------------ //

// ------------------------------------------
// Configuration and Security bits for ATmega
// ------------------------------------------
// WDTON		= 1
// SPIEN		= 0 [X]
// CKOPT		= 0 [X]
// EESAVE		= 1
// BOOTSZ1	= 0 [X]
// BOOTSZ0	= 0 [X]
// BOOTRST	= 1
// BODLEVEL	= 1
// BODEN		= 1
// SUT1		= 1
// SUT0		= 1
// CKSEL3		= 1
// CKSEL2		= 0 [X]
// CKSEL1		= 1
// CKSEL0		= 1
// ------------------------------------------

// ------------------------------------------------ //

#define XTAL 14000000UL	// 14 MHz crystal
#define F_CPU XTAL // F_CPU is needed by delay.h etc
#define BAUD 9600 // UART speed

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

// ------------------------------------------------ //

// Define servo locations
#define SERVO_PORT	PORTC
#define SERVO0_PIN	0
#define SERVO1_PIN	1
#define SERVO2_PIN	2

// Number of servos
#define SERVO_N 3

// Servo data, x = servo number
// servo[x][0] -> 1 = enable, 0 = disable
// servo[x][1] -> servo position, 0-SERVO_MAXPOS
// servo[x][2] -> servo ramp, 0-63 [0 = no ramp, 1 = 3/4 sec -> 63 = 60 sec]
static int servo[SERVO_N][3];

// 0 = undefined, 1 = AK_SERVO, 2 = MINI SSC II/Seattlerobotics, 3 = Polulu, 4 = Parallax, 5 = ?
uint8_t controller_mode = 1;

#define BUFFER_SIZE		12	// UART buffer size
#define SERVO_MINPOS	0	// Servo min. position
#define SERVO_MAXPOS	180	// Servo max. position

// UART buffer, fill with 0x00
uint8_t cmd_buffer[BUFFER_SIZE] = {0};

// Misc
unsigned int i = 0;
unsigned int o = 0;
unsigned int second_counter = 0;
int counter = 0;

// ------------------------------------------------ //

ISR(TIMER0_OVF_vect)
{
	// Timer interrupt function for the servo pulses
	// Called 16 times to get a nice and precise 20 ms (50 Hz) between pulses
	// counter == 16 -> 20 ms ( 50 Hz)
	// counter == 12 -> 15 ms ( 66 Hz)
	// counter ==  8 -> 10 ms (100 Hz)
	
	if(counter == 16)
	{
		cli();
		
		// If servo is enabled, start a pulse
		if(servo[0][0] == 1) SERVO_PORT |= _BV(SERVO0_PIN);
		if(servo[1][0] == 1) SERVO_PORT |= _BV(SERVO1_PIN);
		if(servo[2][0] == 1) SERVO_PORT |= _BV(SERVO2_PIN);
		
		// Wait for about 0.8 ms to get the minimum pulse lenght for the servo pulses
		_delay_ms(0.8);
		
		// Loop all the positions from 0 to SERVO_MAXPOS and turn off the pulse if there is a match
		for(i=0 ; i<=SERVO_MAXPOS ; i++)
		{
			if(servo[0][1] <= i) SERVO_PORT &= ~(_BV(SERVO0_PIN));
			if(servo[1][1] <= i) SERVO_PORT &= ~(_BV(SERVO1_PIN));
			if(servo[2][1] <= i) SERVO_PORT &= ~(_BV(SERVO2_PIN));
			
			// Small delay to keep the servo pulses at the right lenght
			_delay_loop_1(23);
		}
		
		counter = 0;
		
		sei();
	}
	else
	{
		counter++;
	}
	
	// When second_counter reaches 800, about one second has passed
	second_counter++;
	
	if(second_counter == 800)
	{
		// One second passed, do what ever needed
		second_counter = 0;
	}
}

int Uart_putchar(uint8_t character)
{
	// Send characters to the serial line
	
	loop_until_bit_is_set(UCSRA, UDRE); // Wait until the last character is sent
	UDR = character;
	return 0;
}

SIGNAL(SIG_UART_RECV)
{
	// UART interrupt function to receive characters from the serial line
	
	uint8_t character;
	character = UDR;
	
	if(character == 'C' && controller_mode != 0 && o == 0)
	{
		// First character C is used for controller mode, CMx, where x is the mode number
		
		controller_mode = 0;
		cmd_buffer[o] = character;
		Uart_putchar(character);
		
		o++;
		if(o == BUFFER_SIZE) o = 0;
	}
	else if(character != '\r' && controller_mode < 2)
	{
		// Receive all characters until newline and carriage return
		
		if(character != '\n')
		{
			cmd_buffer[o] = character;
			Uart_putchar(character);
			
			o++;
			if(o == BUFFER_SIZE) o = 0;
		}
	}
	else if(character == '\r' && controller_mode < 2)
	{
		cli();
		
		Uart_putchar('\n');
		Uart_putchar('\r');
		
		if(controller_mode == 0)
		{
			if(cmd_buffer[0] == 'C' && cmd_buffer[1] == 'M' && cmd_buffer[3] == 0)
			{
				// Change the controller mode to x
				
				if((cmd_buffer[2]-48) == 1)
				{
					controller_mode = 1;
					printf("Setting controller mode to 1 [AK]");
				}
				else if((cmd_buffer[2]-48) == 2)
				{
					controller_mode = 2;
					printf("Setting controller mode to 2 [Mini SSC II/Seattlerobotics]");
				}
				else if((cmd_buffer[2]-48) == 3)
				{
					//controller_mode = 3;
					//printf("Setting controller mode to 3 [Pololu]");
					controller_mode = 1;
					printf("Controller mode 3 [Pololu] is not available, setting mode to 1 [AK]");
				}
				else if((cmd_buffer[2]-48) == 4)
				{
					controller_mode = 4;
					printf("Setting controller mode to 4 [Parallax]");
				}
				else if((cmd_buffer[2]-48) == 5)
				{
					//controller_mode = 5;
					//printf("Setting controller mode to 5 [Motion Tracking]");
					controller_mode = 1;
					printf("Controller mode 5 [Motion Tracking] is not available, setting mode to 1 [AK]");
				}
				else
				{
					controller_mode = 1;
					printf("Controller mode %d is not available, setting mode to 1 [AK]", (cmd_buffer[2]-48));
				}
			}
		}
		else if(controller_mode == 1)
		{
			// AK Controller mode
			
			// ## and ### = changing part of the command
			// S##P### -> S00P000 - S99P180 -> Move servo XX to position YYY, speed ramp 0 [0 = disable/full speed]
			// SxxP### -> SxxP000 - SxxP180 -> Move all servos to position YYY, speed ramp 0 [0 = disable/full speed]
			// S##P###R## -> S00P000#00 - S99P180#63 -> Move servo XX to position YYY, speed ramp ZZ
			// SxxP###R## -> SxxP000#00 - SxxP180#63 -> Move all servos to position YYY, speed ramp ZZ
			// S##E -> S00E - S99E -> Enable servo XX
			// S##D -> S00D - S99D -> Disable servo XX
			// SxxE -> Enable all servos
			// SxxD -> Disable all servos
			
			
			if(cmd_buffer[0] == 'S' && cmd_buffer[3] == 'P' && (cmd_buffer[7] == 0 || cmd_buffer[7] == 'R') && cmd_buffer[10] == 0)
			{
				int servo_ramp = 0;
				
				if(cmd_buffer[7] == 'R' && cmd_buffer[10] == 0)
				{
					servo_ramp = 10*(cmd_buffer[8]-48) + (cmd_buffer[9]-48);
					
					if(servo_ramp < 0) servo_ramp = 0;
					else if(63 < servo_ramp) servo_ramp = 63;
				}
				
				if(cmd_buffer[1] == 'x' && cmd_buffer[2] == 'x')
				{
					// Move all servos to the position YYY at the same time
					
					int servo_position = 100*(cmd_buffer[4]-48) + 10*(cmd_buffer[5]-48) + (cmd_buffer[6]-48);
					
					if(0 <= servo_position && servo_position <= SERVO_MAXPOS)
					{
						for(o=0 ; o<SERVO_N ; o++)
						{
							servo[o][1] = servo_position;
							servo[0][2] = servo_ramp;
						}
						
						printf("All servos moved to position %d [ramp = %d]", servo_position, servo_ramp);
					}
					else
					{
						printf("Unknown position %d", servo_position);
					}
				}
				else
				{
					// Normal mode, move servo XX to position YYY
					
					// Eg. S12 -> 10*1 + 2 = 12
					// cmd_buffer[x]-48 -> take away the decimal number of ASCII char '0'
					// Eg. 3 = 51 ASCII -> 51-48 = 3
					int servo_number = 10*(cmd_buffer[1]-48) + (cmd_buffer[2]-48);
					
					if(0 <= servo_number && servo_number < SERVO_N)
					{
						int servo_position = 100*(cmd_buffer[4]-48) + 10*(cmd_buffer[5]-48) + (cmd_buffer[6]-48);
						
						if(0 <= servo_position && servo_position <= SERVO_MAXPOS)
						{
							servo[servo_number][1] = servo_position;
							servo[servo_number][2] = servo_ramp;
							
							printf("Servo %d position set to %d [ramp = %d]", servo_number, servo_position, servo_ramp);
						}
						else
						{
							printf("Unknown position %d", servo_position);
						}
					}
					else
					{
						printf("Unknown servo id %d", servo_number);
					}
				}
			}
			else if(cmd_buffer[0] == 'S' && (cmd_buffer[3] == 'E' || cmd_buffer[3] == 'D') && cmd_buffer[4] == 0)
			{
				if(cmd_buffer[1] == 'x' && cmd_buffer[2] == 'x')
				{
					// Enable or disable all servos at the same time
					
					if(cmd_buffer[3] == 'D')
					{
						for(o=0 ; o<SERVO_N ; o++)
						{
							servo[o][0] = 0;
						}
						
						printf("All servos disabled");
					}
					if(cmd_buffer[3] == 'E')
					{
						for(o=0 ; o<SERVO_N ; o++)
						{
							servo[o][0] = 1;
						}
						
						printf("All servos enabled");
					}
				}
				else
				{
					// Normal mode, enable or disable servo XX
					
					// Eg. S12 -> 10*1 + 2 = 12
					// cmd_buffer[x]-48 -> take away the decimal number of ASCII char '0'
					// Eg. 3 = 51 ASCII -> 51-48 = 3
					int servo_number = 10*(cmd_buffer[1]-48) + (cmd_buffer[2]-48);
					
					if(0 <= servo_number && servo_number < SERVO_N)
					{
						if(cmd_buffer[3] == 'D')
						{
							servo[servo_number][0] = 0;
							printf("Servo %d disabled", servo_number);
						}
						if(cmd_buffer[3] == 'E')
						{
							servo[servo_number][0] = 1;
							printf("Servo %d enabled", servo_number);
						}
					}
					else
					{
						printf("Unknown servo id %d", servo_number);
					}
				}
			}
			else
			{
				printf("Unknown command");
			}
		}
		
		Uart_putchar('\n');
		Uart_putchar('\r');
		
		for(o=0 ; o<BUFFER_SIZE ; o++)
		{
			cmd_buffer[o] = 0;
		}
		
		o = 0;
		
		sei();
	}
	else if(controller_mode == 2)
	{
		// Mini SSC II/Seattlerobotics Controller Mode
		// 3 bytes of data, <startbyte 0xFF><servo id><position>, <0xFF><0x00-0xFF><0x00-0xFF>
		
		// If the first character is 0xFF, start buffering
		if(character == 0xFF && o == 0)
		{
			cmd_buffer[o] = character;
			o++;
		}
		else if(o != 0)
		{
			cmd_buffer[o] = character;
			o++;
			
			// Start processing after 3 bytes of data
			if(o == 3)
			{
				cli();
				
				int servo_number = cmd_buffer[1];
				
				if(0 <= servo_number && servo_number < SERVO_N)
				{
					// Converting 0-255 movement range to 0-180
					int servo_position = (SERVO_MAXPOS * (100*cmd_buffer[2]/255)) / 100;
					
					servo[servo_number][1] = servo_position;
				}
				
				for(o=0 ; o<BUFFER_SIZE ; o++)
				{
					cmd_buffer[o] = 0;
				}
				
				o = 0;
				
				sei();
			}
		}
	}
	else if(controller_mode == 3)
	{
		// Pololu Controller Mode
		// N/A
	}
	else if(controller_mode == 4)
	{
		// Parallax Controller Mode
		// 7 bytes of data, !SC<servo id><ramp><position.LOWBYTE><position.HIGHBYTE>
		
		// If the first character is !, start buffering
		if(character == '!' && o == 0)
		{
			cmd_buffer[o] = character;
			o++;
		}
		else if(o != 0)
		{
			cmd_buffer[o] = character;
			o++;
			
			// Start processing after 7 bytes of data
			if(o == 7)
			{
				cli();
				
				int servo_number = cmd_buffer[3];
				int servo_ramp = cmd_buffer[4];
				
				// !SC****
				if(0 <= servo_number && servo_number < SERVO_N && cmd_buffer[0] == '!' && cmd_buffer[1] == 'S' && cmd_buffer[2] == 'C')
				{
					int servo_position = (SERVO_MAXPOS * (100 * ((cmd_buffer[5] + (cmd_buffer[6]*256)) - 250) / 1000)) / 100;
					
					printf("[ %d ]", servo_position);
					
					servo[servo_number][1] = servo_position;
					servo[servo_number][2] = servo_ramp;
				}
				
				for(o=0 ; o<BUFFER_SIZE ; o++)
				{
					cmd_buffer[o] = 0;
				}
				
				o = 0;
				
				sei();
			}
		}
	}
	else if(controller_mode == 5)
	{
		// Motion - Motion Tracking, stepper motor to servo conversion
		// N/A
	}
}

void Init_USART(void)
{
	// Initialize UART
	
	// USART Mode: Asynchronous
	UBRRH = 0x00; // write high byte first
	UBRRL = ((XTAL/16)/BAUD)-1; // baud rate 9600
	UCSRA = 0x00;
	UCSRB |= (1 << 3); // TXEN ON, TX ON,        xxxx 1xxx
	UCSRB |= (1 << 4); // RXEN, RX ON,           xxx1 1xxx
	UCSRB |= (1 << 7); // RXIE, RX interrupt ON, 1xx1 1xxx
	UCSRC = 0x86; // 8 Data, 1 Stop, No Parity   1xxx x11x
	//SREG = (1 << 7); // global interrupt ON      1xxx xxxx
}

int main(void)
{
	DDRC = 0xFF;		// Port C output
	SERVO_PORT = 0x00;	// Set all outputs low
	
	// Set all servos ON
	servo[0][0] = 1;
	servo[1][0] = 1;
	servo[2][0] = 1;
	
	// Servo start positions (0-180, 90 = middle)
	servo[0][1] = SERVO_MAXPOS/2;
	servo[1][1] = SERVO_MAXPOS/2;
	servo[2][1] = SERVO_MAXPOS/2;
	
	// Disable global interrupts
	cli();
	
	// Initialize timers
	TCCR0 = 0x00;
	TCNT0 = 0x00;
	TCCR0 = 0b00000011;
	
	MCUCR = 0b00000000;
	GICR  = 0b00000000;
	TIMSK = 0b00000001;
	
	// Initialize UART
	Init_USART();
	
	// Enable global interrupts
	sei();
	
	// Enable output to UART for printf()
	fdevopen(Uart_putchar, NULL);
	
	// Everything ready
	printf("[ AK RS232 Servo Controller ]");
	Uart_putchar('\n');
	Uart_putchar('\r');
	
	while(1)
	{
		// Loop forever
	}
}