/*!
*	\file
*	\details This is main program file.
*
*	\ingroup WheelchairControl
*	@{
*
*	\mainpage Wheelchair Control MCU firmware documentation
*
*	\section ABOUT Control of a Wheelchair - Diploma thesis, 2012 - 2013.
*
*	\author Ondrej Vozda, Brno university of technology.
*
*	Developed for ATMEGA16 @ 16 MHz.
*
*	AVR Sudio 4 wih AVR gcc 4.3.3.

*/

//Includes
#include "WheelchairControlMain.h"

//Debug mode warning
#ifdef DEBUG
	#warning "DEBUG defined!"
#endif

t_ConfigStruct ConfigStructRAM;		/*!<Configuration variables.*/
t_ConfigStruct EEMEM ConfigStructEEPROM; /*!<Configuration variables (stored in eeprom).*/
t_ISRsSharedStruct ISRsSharedStruct;	/*!<Structure inter-ISRs shared variables.*/

//*******************************************************************************************
//*******************************************************************************************
//*********************************** MAIN **************************************************
//*******************************************************************************************
//*******************************************************************************************
/*!
* Main - program entry point. Superloop (odometry and regulators) is run here.
*/
int16_t main(void)
{
	InitializeMCU();	//Init all 

	BLINK_LEDS_MS(200);	//Blinks LEDs to symbolize successful initialization and system start

	while(TRUE)	//Main loop - calculate odometry and regulators' outputs when no ISRs are being run
	{
		CalculateOdometry();
		
		RunRegulators();
	}
	
	return TRUE;
}

//*******************************************************************************************
//*******************************************************************************************
//*************************** MAIN SUBROUTINES **********************************************
//*******************************************************************************************
//*******************************************************************************************

void InitializeMCU(void)		//Initializes interrupts and peripherals
{
	WATCHDOG_INITIALIZE();	//Watchdog initialize (reset interval aapr. 0.5s)

	Init_ISRsSharedstruct(&ISRsSharedStruct);	//Initializes inter-ISRs shared structure

	EEPROM_LoadVariables(); //load variables from EEPROM to RAM

	AC_Disable();	//disable analog comparator

	ADC_Init();		//initialize A/D converter

	PORTS_Init();	//initialize I/O port A - D (input/output, pull-ups etc.)

	UART_Init(); 	//initialize UART

	I2C_Init();		//initialize I2C

	SRF08_Write(ConfigStructRAM.SRF08_Front,0x01, SRF08_GAIN);	//set front sensor gain
	SRF08_Write(ConfigStructRAM.SRF08_Front,0x02, (SRF08_RANGE/43)-1);	//set front sensor range (in mm)

	SRF08_Write(ConfigStructRAM.SRF08_Rear,0x01, SRF08_GAIN);	//set rear sensor gain
	SRF08_Write(ConfigStructRAM.SRF08_Rear,0x02, (SRF08_RANGE/43)-1);	//set rear sensor range (in mm)

	//Optionally set first safety SRF08 sensor gain
	if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
	{
		SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x01, SRF08_GAIN);	//Set sensor gain
		SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x02, (SRF08_RANGE/43)-1);	//Set sensor range (in mm)
	}

	//Optionally set second safety SRF08 sensor gain
	if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
	{
		SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x01, SRF08_GAIN);	//Set sensor gain
		SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x02, (SRF08_RANGE/43)-1);	//Set sensor range (in mm)
	}

	TIMER0_Init();	//initialize Timer0 (see this function description for TIMER0 parameters)
	TIMER1_Init();	//initialize Timer1 (see this function description for TIMER1 parameters)
	TIMER2_Init();	//initialize Timer2 (see this function description for TIMER2 parameters)

	EXT_INT_Init();	//initialize external interrupts

	GLOBAL_INTERRUPTS_ENABLE();	//enable interrupts	
}

void CalculateOdometry(void)	//Reads data from sensors and calculate position
{
	if(ISRsSharedStruct.BufferOdoR || ISRsSharedStruct.BufferOdoL)	//Calculate odometry only if buffers are not empty
	{
		int16_t trackM; 	//Track of midpoint
		int32_t tempPhi;	//Phi
		int16_t trackR = (int16_t)(((int32_t)ISRsSharedStruct.BufferOdoR * (int32_t)ConfigStructRAM.RightWheelIncrement) / (int32_t)WHEEL_INCREMENT_SCALE);	//Right wheel track [mm]
		int16_t trackL = (int16_t)(((int32_t)ISRsSharedStruct.BufferOdoL * (int32_t)ConfigStructRAM.LeftWheelIncrement) / (int32_t)WHEEL_INCREMENT_SCALE);		//Left wheel track [mm]

		//Clear buffers
		ISRsSharedStruct.BufferOdoR = 0;
		ISRsSharedStruct.BufferOdoL = 0;
		
		//Calculate track of midpoint, new phi (in mRads!)
		trackM = (trackR + trackL) / 2;
		tempPhi = (int32_t)ISRsSharedStruct.Phi + (int32_t)((((int32_t)trackR - (int32_t)trackL) * 100) / ((int32_t)ConfigStructRAM.Track));
		tempPhi %=  TWO_PI_MRAD;
		
		if(tempPhi < 0)	//Only positive values of phi are stored
		{
			tempPhi += TWO_PI_MRAD;
		}
		
		
		//Save odometry results atomically
		TIMER2_OVF_INT_DISABLE();
			
			ISRsSharedStruct.Phi = tempPhi;
			ISRsSharedStruct.X += (int16_t)((double)trackM * cos(ISRsSharedStruct.Phi / 1000.0));
			ISRsSharedStruct.Y += (int16_t)((double)trackM * sin(ISRsSharedStruct.Phi / 1000.0));

		TIMER2_OVF_INT_ENABLE();
	}
}

void RunRegulators(void) //Regulators (wall following and position regulator)
{
	if (ISRsSharedStruct.EnablePositionRegulator) //Position regulator enabled, run it
	{
		int32_t tempSpR, tempSpL;	//Temporary variables to calculate speeds
		int32_t	vTemp, wTemp;		//Forward  and angular speed [cm/s],[mRad/s]
		double errDist, errAngle;	//Error of distance [mm] and angle [mRad]

		if(ISRsSharedStruct.RegulateAngleOnly)	//Angular regulation enabled
		{
			//Regulate angle only - angle value is stored in the variable
			errAngle = (double)((ISRsSharedStruct.RegulateAngleOnly / 1000.0) - (ISRsSharedStruct.Phi / 1000.0));

			//Normalize to <-pi;pi> range
			errAngle = errAngle - (2 * M_PI * floor((errAngle + M_PI) / (2 * M_PI)));

			//Stop forward movement
			vTemp = 0;

			//Calculate angular speed
			if (fabs(errAngle) <= (ConfigStructRAM.RegGamma / 10.0))
			{
				wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * errAngle);
			}
			else
			{
				wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * (ConfigStructRAM.RegGamma / 10.0) * (errAngle > 0 ? 1.0 : -1.0));
			}

			//Target angle reached, disable regulator
			if(fabs(errAngle) <= REG_ANGLE_EPSILON)
				ISRsSharedStruct.EnablePositionRegulator = FALSE;
		}
		else
		{
			//Calculate distance error
			errDist = sqrt(square( (double)(ISRsSharedStruct.Xdes - ISRsSharedStruct.X) ) + square( (double)(ISRsSharedStruct.Ydes - ISRsSharedStruct.Y) ));

			if ( (errDist > ConfigStructRAM.RegEpsilon))	//If target has not been reached yet
			{		
				//Calculate angle error (atan2 returns value in range <-pi;+pi>)
				errAngle = atan2((ISRsSharedStruct.Ydes - ISRsSharedStruct.Y),(ISRsSharedStruct.Xdes - ISRsSharedStruct.X)) - (ISRsSharedStruct.Phi / 1000.0);
			
				//Normalize to <-pi;pi> range
				errAngle = errAngle - (2 * M_PI * floor((errAngle + M_PI) / (2 * M_PI)));

				//Calculate speeds
				if (fabs(errAngle) <= (ConfigStructRAM.RegGamma / 10.0))
				{
					//Treat forward speed as constant -> better performance
					//vTemp = (int32_t)((ConfigStructRAM.RegPositionGain / 10.0) * (errDist / 10));	//CURRENTLY DISABLED
					vTemp = (int32_t)ConfigStructRAM.RegSpeed;

					wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * errAngle);
				}
				else
				{
					vTemp = 0;

					wTemp = (int32_t)(WTEMP_SCALE * (ConfigStructRAM.RegAngleGain / 10.0) * (ConfigStructRAM.RegGamma / 10.0) * (errAngle > 0 ? 1.0 : -1.0));
				} 
			}
			else
			{
				vTemp = 0;
				wTemp = 0;

				//Target reached, disable position regulator
				ISRsSharedStruct.EnablePositionRegulator = FALSE;
			}
		}

		//Saturate omega
		//Disable saturation by setting OmegaThresh to 0
		if (ConfigStructRAM.OmegaThresh != 0) 
		{
			//Angular speed threshold
			if ((wTemp * WTEMP_SCALE) > ((int32_t)ConfigStructRAM.OmegaThresh * WTEMP_SCALE)) 
			{
				wTemp = (int32_t)ConfigStructRAM.OmegaThresh * WTEMP_SCALE;
			}

			if ((wTemp * WTEMP_SCALE) < (-1 * WTEMP_SCALE * (int32_t)ConfigStructRAM.OmegaThresh))
			{
				wTemp = (int32_t)(-1* WTEMP_SCALE * (int32_t)ConfigStructRAM.OmegaThresh); 
			}
		}

		//Calculate  wheels' speeds in cm/s
		tempSpR = (int32_t)( (vTemp) + (((int32_t)wTemp*(int32_t)ConfigStructRAM.Track)/(2.0 * WTEMP_SCALE)));
		tempSpL = (int32_t)( (vTemp) - (((int32_t)wTemp*(int32_t)ConfigStructRAM.Track)/(2.0 * WTEMP_SCALE)));

		//Prevent overflow / underflow
		if (tempSpR < -500)
		{
			tempSpR = -500;	
		}

		if (tempSpR > 500)
		{
			tempSpR = 500;	
		}

		if (tempSpL < -500)
		{
			tempSpL = -500;
		}

		if (tempSpL > 500)
		{
			tempSpL = 500;
		}

		tempSpR = ((tempSpR * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20*(int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
		tempSpL = ((tempSpL * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20*(int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset

		TIMER2_OVF_INT_DISABLE();	//Write speeds atomically

			ISRsSharedStruct.MomSpeedR = (uint8_t)tempSpR; //actualize speed of the right wheel
			ISRsSharedStruct.MomSpeedL = (uint8_t)tempSpL; //actualize speed of the left wheel

		TIMER2_OVF_INT_ENABLE();
	}
	else if (ISRsSharedStruct.EnableWallFollowing)	//Wall following enabled, run it
	{
		int32_t tempSpR, tempSpL;	//temporary variables to calculate speeds
		int16_t omega, d, d1, d2, y0;	//variables of position regulator (angular speed, front and rear distance, d1 - d2), current y position

		TIMER2_OVF_INT_DISABLE();	//Temporarily disable timer2 interrupt to guarantee SRF08s' data consistency 

			//Model linearized for fi ~ 0°
			d1 = (int16_t)(ISRsSharedStruct.SRF08_DistanceFront + ConfigStructRAM.Track/2);		//front distance
			d2 = (int16_t)(ISRsSharedStruct.SRF08_DistanceRear + ConfigStructRAM.Track/2 + ((int16_t)ConfigStructRAM.SRF08_RearCorrection - REAR_CORRECTION_OFFSET) );	//rear distance

		TIMER2_OVF_INT_ENABLE();	//Reenable interrupt

		d = (int16_t)(d1 - d2);		//Deviance from zero angle

		y0 = (d1+d2)/2;				//Actual midpoint's y position

		//Calculate angular speed ( omega = Kfi*d - Ky*(y0_des - y0) ) 
		//Cast necessary to prevent signed and unsigned types collision
		//Warning! This calculation is true only if sensor are placed on the left side of the wheelchair, otherwise omega must by multiplied by -1!!!
		omega = (((int16_t)ISRsSharedStruct.Kfi * (int16_t)d) / 10 )
				- (((int16_t)ISRsSharedStruct.Ky * ((int16_t)ISRsSharedStruct.y0_des - (int16_t)y0)) / 10 );

		//Invert omega sign if steering wheel is on the left side
		if(STEERING_WHEEL_LEFT)
		{
			omega *= (int16_t)(-1);
		}

		//Saturate omega
		//Disable saturation by setting OmegaThresh to 0
		if (ConfigStructRAM.OmegaThresh != 0) //Disable thresholding by setting OmegaThresh to 0
		{
			//Angular speed threshold
			if (omega > ConfigStructRAM.OmegaThresh) 
			{
				omega = (int16_t)ConfigStructRAM.OmegaThresh;
			}

			if (omega < (-1*ConfigStructRAM.OmegaThresh))
			{
				omega = (int16_t)(-1*ConfigStructRAM.OmegaThresh); 
			}
		}

		//Calculate  wheels' speeds in cm/s
		tempSpR = (int32_t)( (ISRsSharedStruct.V_des - SPEED_OFFSET) + (((int32_t)omega * OMEGA_SCALE * (int32_t)ConfigStructRAM.Track)/2.0));
		tempSpL = (int32_t)( (ISRsSharedStruct.V_des - SPEED_OFFSET) - (((int32_t)omega * OMEGA_SCALE * (int32_t)ConfigStructRAM.Track)/2.0));

		//Prevent overflow / underflow
		if (tempSpR < -500)
		{
			tempSpR = -500;	
		}

		if (tempSpR > 500)
		{
			tempSpR = 500;	
		}

		if (tempSpL < -500)
		{
			tempSpL = -500;
		}

		if (tempSpL > 500)
		{
			tempSpL = 500;
		}

		tempSpR = ((tempSpR * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20 * (int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset
		tempSpL = ((tempSpL * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20 * (int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET ; //desired speed in pulses per 50 ms = (des. speed in cm/s * (impulses per round x2))/(speed sample frequency*circumference) + offset

		//Check whether specified distance has not been reached
		if(ISRsSharedStruct.Xdes && (fabs(ISRsSharedStruct.X) >= (fabs(ISRsSharedStruct.Xdes))))
		{
			//If Xdes is set, it acts as distance limit for wall-following algorithm
			//If traveled distance is greater than Xdes, wall following alorithm is stopped

			tempSpR = SPEED_OFFSET;
			tempSpL = SPEED_OFFSET;

			ISRsSharedStruct.EnableWallFollowing = FALSE;
		}

		TIMER2_OVF_INT_DISABLE();	//Write speeds atomically

			ISRsSharedStruct.MomSpeedR = (uint8_t)tempSpR; //actualize speed of the right wheel
			ISRsSharedStruct.MomSpeedL = (uint8_t)tempSpL; //actualize speed of the left wheel

		TIMER2_OVF_INT_ENABLE();
	}	
}

//*******************************************************************************************
//*******************************************************************************************
//*************************** EEPROM RW ROUTINES ********************************************
//*******************************************************************************************
//*******************************************************************************************

void EEPROM_LoadVariables(void)	//loads variables stored in EEPROM to RAM
{
	ConfigStructRAM.SRF08_Front = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Front);	//read address of front SRF08 sensor from EEPROM
	ConfigStructRAM.SRF08_Rear = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Rear);	//read address of rear SRF08 sensor from EEPROM

	ConfigStructRAM.SRF08_SafetyFirst = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_SafetyFirst); //Read address of first safety sensor
	ConfigStructRAM.SRF08_SafetySecond = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_SafetySecond); //Read address of first safety sensor

	ConfigStructRAM.ImpPerRoundR = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundR);	//read imp. per round of the right encoder
	ConfigStructRAM.ImpPerRoundL = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundL);	//read imp. per round of the left encoder

	ConfigStructRAM.MaxAcc = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.MaxAcc); //read max. acceleration

	ConfigStructRAM.Kp = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kp); //read Kp coefficient of the regulator
	ConfigStructRAM.Ki = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ki); //read Kp coefficient of the regulator
	ConfigStructRAM.Kd = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kd); //read Kp coefficient of the regulator

	ConfigStructRAM.RegErrorFilter = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegErrorFilter); //read regulator error threshold

	ConfigStructRAM.CircumR = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR);	//read right wheel circumference
	ConfigStructRAM.CircumL = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL);	//read left wheel circumference

	ConfigStructRAM.Track = eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track); //read track of wheels

	ConfigStructRAM.Omega0 = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Omega0); //read omega0
	ConfigStructRAM.L = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.L); //read L
	ConfigStructRAM.Ksi = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ksi); //read Ksi

	ConfigStructRAM.OmegaThresh = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.OmegaThresh); //read omega threshold

	ConfigStructRAM.SRF08_RearCorrection = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_RearCorrection); //Read correction factor

	ConfigStructRAM.RightWheelIncrement = (uint8_t)( ((uint32_t)ConfigStructRAM.CircumR * (uint32_t)10 * (uint32_t)WHEEL_INCREMENT_SCALE) / (uint32_t)((uint32_t)ConfigStructRAM.ImpPerRoundR * (uint32_t)2) );	//Right wheel track increment [mm]
	ConfigStructRAM.LeftWheelIncrement = (uint8_t)( ((uint32_t)ConfigStructRAM.CircumL * (uint32_t)10 * (uint32_t)WHEEL_INCREMENT_SCALE) /(uint32_t)((uint32_t)ConfigStructRAM.ImpPerRoundL * (uint32_t)2) );	//Left wheel track increment [mm]

	ConfigStructRAM.RegGamma = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegGamma);	//Read regulator's gamma
	ConfigStructRAM.RegEpsilon = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegEpsilon);	//Read regulator's epsilon
	ConfigStructRAM.RegAngleGain = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegAngleGain);	//Read regulator's angle gain
	ConfigStructRAM.RegPositionGain = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegPositionGain);	//Read regulator's position gain
	ConfigStructRAM.RegSpeed = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegSpeed);	//Read regulator's position gain
}

void EEPROM_WriteVariable(uint8_t* address, uint8_t data) //writes byte to EEPROM
{
	eeprom_update_byte(address, data);	//write the value to EEPROM

	EEPROM_LoadVariables();	//reloads variables from EEPROM to RAM
}

void EEPROM_WriteWord(uint16_t* address, uint16_t data) //writes word (2 bytes) to EEPROM
{
	eeprom_update_word(address, data);	//write the value to EEPROM

	EEPROM_LoadVariables();	//reloads variables from EEPROM to RAM
}

//*******************************************************************************************
//*******************************************************************************************
//*************************** INTERRUPT SERVICE ROUTINES ************************************
//*******************************************************************************************
//*******************************************************************************************

/*!
* UART Rx complete interrupt service routine.
*/
ISR(USART_RXC_vect)
{
	static uint8_t UARTReceivedData[4];	//Buffer for received data

	TIMER_0_RESET();				//Reset timer & prescaler
	ISRsSharedStruct.Timer0Prescaler = 0;
			
	UARTReceivedData[ISRsSharedStruct.UARTRxIndex++] = UDR;	//read data

	if(ISRsSharedStruct.UARTRxIndex >= RX_PACKET_LENGTH) //whole packet has been received
	{
		uint8_t checksum = (uint8_t)(UARTReceivedData[COMMAND_IDENTIFIER] + UARTReceivedData[DATA1] + UARTReceivedData[DATA2]);
		
		if(checksum == UARTReceivedData[RX_CHECKSUM_BYTE])	//if checksum is correct
		{
			PORTA |= LED_IN; //turn on LED_IN (PORTA.1) status to symbolize received packet

			ISRsSharedStruct.UARTWatchdog = 0; //reset the watchdog counter

			switch(UARTReceivedData[COMMAND_IDENTIFIER])	//analyze received data and perform the command
			{
				default: break;	//unrecognized command	

				//A -> Odometry

				case ODOMETRY_SET_X:	//Set X coordinate

							ISRsSharedStruct.X = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);

							break;

				case ODOMETRY_SET_Y:	//Set Y coordinate

							ISRsSharedStruct.Y = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);

							break;

				case ODOMETRY_SET_PHI:	//Set Y coordinate

							ISRsSharedStruct.Phi = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);

							break;
							

				case ODOMETRY_RESET_ALL:	//Reset all coordinates

							ISRsSharedStruct.BufferOdoL = 0;	
							ISRsSharedStruct.BufferOdoR = 0;

							ISRsSharedStruct.TotalImpulsesR = 0;
							ISRsSharedStruct.TotalImpulsesL = 0;

							ISRsSharedStruct.X = 0;
							ISRsSharedStruct.Y = 0;
							ISRsSharedStruct.Phi = 0;

							break;
				

				//C -> Motors control

				case MOTORS_RIGHT_MOMENTUM:	//right motor momentum (128 = 0, 255 = full forward, 0 = full backwards)

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
								
							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
							
							ISRsSharedStruct.EnableSpRegR = FALSE;	//disable speed regulator

							ISRsSharedStruct.RegIntegratorR = 0;	//reset the integrator

							ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA2];

							break;

				case MOTORS_LEFT_MOMENTUM:	//left motor momentum (128 = 0, 255 = full forward, 0 = full backwards)

							ISRsSharedStruct.EnablePositionRegulator = FALSE;; //Disable position regulator
								
							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
							
							ISRsSharedStruct.EnableSpRegL = FALSE;	//disable speed regulator

							ISRsSharedStruct.RegIntegratorL = 0;	//reset the integrator

							ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];

							break;

				case MOTORS_RIGHT_SPEED_CMPS:	//right motor speed in cm/s (-500 to + 500 cm per s, offset  +500 -> values 0 to 1000)
							{ 
							int32_t tempSp;		//temp for speed calculation

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator

							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm	
													
							ISRsSharedStruct.EnableSpRegR = TRUE;	//enable speed regulator

							tempSp = (int32_t)((UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);	//calculate the speed in cm/s
							
							if (tempSp < 0)	//prevent underflow
							{
								tempSp = 0;
							}

							if (tempSp > 1000)	//prevent overflow	
							{
								tempSp = 1000;
							}	

							tempSp -= 500; //substract the offset

							tempSp = (((int32_t)tempSp * 2 * (int32_t)ConfigStructRAM.ImpPerRoundR) / (20*(int32_t)ConfigStructRAM.CircumR)) + SPEED_OFFSET; //desired speed in pulses per 50 ms = (des. speed in cm/s * impulses per roud)/(speed sample frequency*circumference) + offset
							
							ISRsSharedStruct.MomSpeedR = (uint8_t)tempSp;	//actualize the speed

							break;

							}

				case MOTORS_LEFT_SPEED_CMPS:	//left motor speed  in cm/s (-500 to + 500 cm per s, offset  +500 -> values 0 to 1000)
							{
							int32_t tempSp;		//temp for speed calculation

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator

							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm
								
							ISRsSharedStruct.EnableSpRegL = TRUE; //enable speed regulator

							tempSp = (int32_t)((UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);	//calculate the speed in cm/s
							
							if (tempSp < 0)	//prevent underflow
							{
								tempSp = 0;
							}

							if (tempSp > 1000)	//prevent overflow	
							{
								tempSp = 1000;
							}

							tempSp -= 500; //substract the offset

							tempSp = ((tempSp * 2 * (int32_t)ConfigStructRAM.ImpPerRoundL) / (20*(int32_t)ConfigStructRAM.CircumL)) + SPEED_OFFSET; //desired speed in pulses per 50 ms = (des. speed in cm/s * impulses per roud)/(speed sample frequency*circumference) + offset
							
							ISRsSharedStruct.MomSpeedL = (uint8_t)tempSp;	//actualize the speed

							break;
							
							}

				case MOTORS_BOTH_MOMENTUM:	//right and left motor momentum (128 = 0, 255 = full forward, 0 = full backwards)
							
							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator
								
							ISRsSharedStruct.EnableSpRegR = FALSE; 	//disable speed regulator
							ISRsSharedStruct.EnableSpRegL = FALSE;	//disable speed regulator

							ISRsSharedStruct.RegIntegratorR = 0;	//reset the integrator
							ISRsSharedStruct.RegIntegratorL = 0;	//reset the integrator

							ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA1];
							ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];

							break;

				case MOTORS_BOTH_SPEED_IMPPS:	//right and left motor speed in impulses per 50ms ~ impps/20 (128 = 0, 255 = full forward, 0 = full backwards)
								
							ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator

							ISRsSharedStruct.EnableSpRegR = TRUE;	//enable speed regulator
							ISRsSharedStruct.EnableSpRegL = TRUE;	//enable speed regulator

							ISRsSharedStruct.MomSpeedR = UARTReceivedData[DATA1];
							ISRsSharedStruct.MomSpeedL = UARTReceivedData[DATA2];

							break;

				case MOTORS_WALL_FOLLOWING:	//starts wall-following algorithm using desired speed V (cm/s) and y0 (cm)

							ISRsSharedStruct.EnablePositionRegulator = FALSE; //Disable position regulator

							ISRsSharedStruct.EnableWallFollowing = TRUE; //enable wall following algorithm

							ISRsSharedStruct.EnableSpRegR = TRUE;	//enable speed regulator
							ISRsSharedStruct.EnableSpRegL = TRUE;	//enable speed regulator

							ISRsSharedStruct.V_des = UARTReceivedData[DATA1]; //desired speed in cm/s
							ISRsSharedStruct.y0_des = UARTReceivedData[DATA2];	//desired distance from wall in cm
							
							if(ISRsSharedStruct.V_des != SPEED_OFFSET)	//offset 128 -> 128 is equal to zero speed
							{
								ISRsSharedStruct.Ky = (int16_t)( ( ((int16_t)(ConfigStructRAM.Omega0) * (int16_t)(ConfigStructRAM.Omega0) / 2) ) / ((int16_t)(ISRsSharedStruct.V_des - SPEED_OFFSET) ) );	//Ky = (omega0)^2 / V
							}

							ISRsSharedStruct.Kfi = (int16_t)( ( ((int16_t)ConfigStructRAM.Ksi) * ((int16_t)ConfigStructRAM.Omega0) ) / ((int16_t)ConfigStructRAM.L) ); //Kfi = (Ksi * omega0)/L

							break;

				case MOTORS_POSITION_REGULATOR:	//Starts position regulation

							ISRsSharedStruct.EnablePositionRegulator = TRUE; //Enable position regulator

							ISRsSharedStruct.EnableWallFollowing = FALSE; //Disable wall following algorithm

							ISRsSharedStruct.RegulateAngleOnly = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);	//Angle / speed + angle regulator

							ISRsSharedStruct.EnableSpRegR = TRUE;	//Enable speed regulator
							ISRsSharedStruct.EnableSpRegL = TRUE;	//Enable speed regulator

							break;

				//D - > Regulators
				
				case REGULATORS_X_DESIRED:	//Sets desired X coordinate for the position regulator
							
							ISRsSharedStruct.Xdes = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);

							break;
				
				case REGULATORS_Y_DESIRED: 	//Sets desired Y coordinate for the position regulator

							ISRsSharedStruct.Ydes = (int16_t)(((int16_t)UARTReceivedData[DATA1] << 8) + UARTReceivedData[DATA2]);
							
							break;

				case REGULATORS_GAMMA: //Sets gamma (angle of view)

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegGamma, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case REGULATORS_EPSILON: //Sets epsilon (acceptable error of target position)

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegEpsilon, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case REGULATORS_ANGLE_GAIN:	//Set gain of angle feedback loop

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegAngleGain, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case REGULATORS_POSITION_GAIN:	//Set gain of position feedback loop

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegPositionGain, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case REGULATORS_SPEED:	//Set constant forward speed in case variable speed regulation is forbidden

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegSpeed, (uint8_t)UARTReceivedData[DATA2]);

							break;
				

				//E - > Configuration

				case CONFIGURATION_RIGHT_WHEEL_CIRCUM: 	//sets the right wheel circumference in cm
							
							EEPROM_WriteWord(&ConfigStructEEPROM.CircumR, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_LEFT_WHEEL_CIRCUM: 	//sets the left wheel circumference in cm
							
							EEPROM_WriteWord(&ConfigStructEEPROM.CircumL, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);

							break;
				
				case CONFIGURATION_TRACK: 	//sets track of wheels in cm
							
							EEPROM_WriteWord(&ConfigStructEEPROM.Track, (UARTReceivedData[DATA1]<<8) + UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_IMP_PER_ROUND:	//sets impulses per round

							EEPROM_WriteVariable(&ConfigStructEEPROM.ImpPerRoundR, (uint8_t)UARTReceivedData[DATA1]);
							EEPROM_WriteVariable(&ConfigStructEEPROM.ImpPerRoundL, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_SRF08_SAFETY_ADDRESSES:	//Safety sensors addresses

							EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_SafetyFirst, (uint8_t)UARTReceivedData[DATA1]);
							EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_SafetySecond, (uint8_t)UARTReceivedData[DATA2]);


							break;

				case CONFIGURATION_MAX_ACCELERATION: 	//sets maximal acceleration
							
							EEPROM_WriteVariable(&ConfigStructEEPROM.MaxAcc, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_KP: 	//sets Kp coefficient of the PSD regulator (must be multiplied by 10!!!)
							
							EEPROM_WriteVariable(&ConfigStructEEPROM.Kp, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_KD: 	//sets Kd coefficient of the PSD regulator	(must be multiplied by 10!!!)
							
							EEPROM_WriteVariable(&ConfigStructEEPROM.Kd, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_KI: 	//sets Ki coefficient of the PSD regulator	(must be multiplied by 10!!!)
							
							EEPROM_WriteVariable(&ConfigStructEEPROM.Ki, (uint8_t)UARTReceivedData[DATA2]);

							break;
				
				case CONFIGURATION_SRF08_ADDRESSES: //sets addresses of front and rear SRF08 sensor
							
							EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_Front, (uint8_t)UARTReceivedData[DATA1]);
							EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_Rear, (uint8_t)UARTReceivedData[DATA2]);

							break;

				case CONFIGURATION_REG_ERR_THRESH: //sets regulator error threshold

							EEPROM_WriteVariable(&ConfigStructEEPROM.RegErrorFilter, (uint8_t)UARTReceivedData[DATA2]);
				
							break;	
							
				case CONFIGURATION_OMEGA_0: //sets the omega0 value (multiplied by 10!!!)

							EEPROM_WriteVariable(&ConfigStructEEPROM.Omega0, (uint8_t)UARTReceivedData[DATA2]);
				
							break;	
							
				case CONFIGURATION_L: //sets the L (distance between SRF08 sensors/2 in cm)

							EEPROM_WriteVariable(&ConfigStructEEPROM.L, (uint8_t)UARTReceivedData[DATA2]);
				
							break;	
				
				case CONFIGURATION_KSI: //sets the ksi value (multiplied by 10!!)

							EEPROM_WriteVariable(&ConfigStructEEPROM.Ksi, (uint8_t)UARTReceivedData[DATA2]);
				
							break;	
							
				case CONFIGURATION_OMEGA_THRESH: //sets the omegaThresh value

							EEPROM_WriteVariable(&ConfigStructEEPROM.OmegaThresh, (uint8_t)UARTReceivedData[DATA2]);
				
							break;			

				case CONFIGURATION_REAR_CORRECTION:	//Sets rear sensor distance correction

							EEPROM_WriteVariable(&ConfigStructEEPROM.SRF08_RearCorrection, (uint8_t)UARTReceivedData[DATA2]);
					
							break;

				//F -> Send data

				case SEND_ODOMETRY: 	//Send odometry data
							{
							uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer

							PORTA |= LED_OUT;	//Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
							
							UARTTransmitData[0] = (uint8_t)(ISRsSharedStruct.X >> 8);	//X coordinate [mm] - High byte
							UARTTransmitData[1] = (uint8_t)(ISRsSharedStruct.X & 0x00FF);//X coordinate [mm] - Low byte

							UARTTransmitData[2] = (uint8_t)(ISRsSharedStruct.Y >> 8);	//Y coordinate [mm] - High byte
							UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Y & 0x00FF);//Y coordinate [mm] - Low byte

							UARTTransmitData[4] = (uint8_t)(ISRsSharedStruct.Phi >> 8);		//Phi [mRad] - High byte
							UARTTransmitData[5] = (uint8_t)(ISRsSharedStruct.Phi & 0x00FF);	//Phi [mRad] - Low byte

							UARTTransmitData[6] = (uint8_t)ISRsSharedStruct.SRF08_DistanceFront;	//Front distance measured by SRF08
							UARTTransmitData[7] = (uint8_t)ISRsSharedStruct.SRF08_DistanceRear;	//Rear distance measured by SRF08

							UARTTransmitData[8] = (uint8_t)(ISRsSharedStruct.EnablePositionRegulator); //Send position regulator status
							UARTTransmitData[9] = (uint8_t)(ISRsSharedStruct.EnableWallFollowing);	//Send wall following regulator status
							
							UARTTransmitData[10] = 0;

							UARTTransmitData[TX_CHECKSUM_BYTE] = 0;

							for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
							{
								UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
								
								UART_SendByte((uint8_t)UARTTransmitData[i]);	//send data via UART					
							}

							UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]);	//send checksum via UART

							break;

							}

				case SEND_CONFIG_1:	//send configuration data 1
							{
							uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer

							PORTA |= LED_OUT;	//Turn on LED_OUT (PORTA.2) to symbolize outgoing packet

							UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundR); //imp/round of the right wheel
							UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.ImpPerRoundL); //imp/round of the left wheel

							UARTTransmitData[2] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR)>>8); //right wheel circumference high byte
							UARTTransmitData[3] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumR) & 0x00FF); //right wheel circumference low byte

							UARTTransmitData[4] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL)>>8); //left wheel circumference high byte
							UARTTransmitData[5] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.CircumL) & 0x00FF); //left wheel circumference low byte

							UARTTransmitData[6] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track)>>8); //track high byte
							UARTTransmitData[7] = (eeprom_read_word((uint16_t*)&ConfigStructEEPROM.Track) & 0x00FF); //track low byte

							UARTTransmitData[8] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Front); //front SRF08 address
							UARTTransmitData[9] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_Rear); //rear SRF08 address

							UARTTransmitData[10] = (int8_t)(eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.SRF08_RearCorrection) - 128); //rear correction factor

							UARTTransmitData[TX_CHECKSUM_BYTE] = 0;	//reset checksum

							for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
							{
								UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
								
								UART_SendByte((uint8_t)UARTTransmitData[i]);	//send data via UART					
							}

							UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]);	//send checksum via UART

							break;

							}

				case SEND_CONFIG_2:	//send configuration data 2
							{
							uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer

							PORTA |= LED_OUT;	//Turn on LED_OUT (PORTA.2) to symbolize outgoing packet

							UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.MaxAcc); //max. acceleration
							UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kp);	//Kp
							UARTTransmitData[2] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Kd);	//Kd
							UARTTransmitData[3] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ki);	//Ki

							UARTTransmitData[4] = (int8_t)ISRsSharedStruct.ActualRPMR;	//actual right wheel speed (imp/50ms
							UARTTransmitData[5] = (int8_t)ISRsSharedStruct.ActualRPML;	//actual left wheel speed (imp/50ms)

							UARTTransmitData[6] = (uint8_t)(ISRsSharedStruct.TotalImpulsesR>>8); //buffer high byte
							UARTTransmitData[7] = (uint8_t)(ISRsSharedStruct.TotalImpulsesR & 0x00FF); //buffer low byte

							UARTTransmitData[8] = (uint8_t)(ISRsSharedStruct.TotalImpulsesL>>8); //buffer high byte
							UARTTransmitData[9] = (uint8_t)(ISRsSharedStruct.TotalImpulsesL & 0x00FF); //buffer low byte

							UARTTransmitData[10] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegErrorFilter);	//reg. error threshold

							UARTTransmitData[TX_CHECKSUM_BYTE] = 0;	//reset checksum

							for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
							{
								UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
								
								UART_SendByte((uint8_t)UARTTransmitData[i]);	//send data via UART					
							}

							UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]);	//send checksum via UART
				
							break;	

							}
							
				case SEND_SRF08:	//SRF08 & position regulator info
							{
							uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer

							PORTA |= LED_OUT;	//Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
							
							UARTTransmitData[0] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Omega0); //omega0
							UARTTransmitData[1] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.L); //distance between sensors
							UARTTransmitData[2] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.Ksi); //Ksi

							UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Ky>>8);	//high byte of Ky
							UARTTransmitData[4] = (uint8_t)(ISRsSharedStruct.Ky & 0x00FF); //low byte of Ky 

							UARTTransmitData[5] = (uint8_t)(ISRsSharedStruct.Kfi>>8);	//high byte of Kfi
							UARTTransmitData[6] = (uint8_t)(ISRsSharedStruct.Kfi & 0x00FF);	//low byte of Kfi

							UARTTransmitData[7] = (uint8_t)ISRsSharedStruct.V_des; //desired speed
							UARTTransmitData[8] = (uint8_t)ISRsSharedStruct.y0_des; //desired distance from the wall
						
							UARTTransmitData[9] = (uint8_t)ISRsSharedStruct.SRF08_DistanceFront;	//send front distance measured by SRF08
							UARTTransmitData[10] =(uint8_t)ISRsSharedStruct.SRF08_DistanceRear;	//send rear distance measured by SRF08

							UARTTransmitData[TX_CHECKSUM_BYTE] = 0;	//reset checksum

							for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
							{
								UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
								
								UART_SendByte((uint8_t)UARTTransmitData[i]);	//send data via UART					
							}

							UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]);	//send checksum via UART

							break;	
							}	
							
				case SEND_REGULATORS:	//Position regulators info
							{
							uint8_t UARTTransmitData[TX_PACKET_LENGTH]; //Data buffer

							PORTA |= LED_OUT;	//Turn on LED_OUT (PORTA.2) to symbolize outgoing packet
							
							UARTTransmitData[0] = (uint8_t)(ISRsSharedStruct.Xdes>>8);	//Xdes HB
							UARTTransmitData[1] = (uint8_t)(ISRsSharedStruct.Xdes & 0x00FF);	//Xdes LB

							UARTTransmitData[2] = (uint8_t)(ISRsSharedStruct.Ydes>>8);	//Ydes HB
							UARTTransmitData[3] = (uint8_t)(ISRsSharedStruct.Ydes & 0x00FF);	//Ydes LB

							UARTTransmitData[4] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegGamma);	//Gamma
							UARTTransmitData[5] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegEpsilon);//Epsilon
							UARTTransmitData[6] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegAngleGain);	//Angle feedback loop gain
							UARTTransmitData[7] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegPositionGain); //Position feedback gain
							UARTTransmitData[8] = eeprom_read_byte((uint8_t*)&ConfigStructEEPROM.RegSpeed);	//Regulator constant speed
							
							UARTTransmitData[9] = 0;	//Empty
							UARTTransmitData[10] = 0;

							UARTTransmitData[TX_CHECKSUM_BYTE] = 0;	//reset checksum

							for(uint8_t i = 0; i < TX_CHECKSUM_BYTE; i++)
							{
								UARTTransmitData[TX_CHECKSUM_BYTE] += UARTTransmitData[i]; //calculate checksum
								
								UART_SendByte((uint8_t)UARTTransmitData[i]);	//send data via UART					
							}

							UART_SendByte((uint8_t)UARTTransmitData[TX_CHECKSUM_BYTE]);	//send checksum via UART
							
							break;
							}						
			}			
		}	

		ISRsSharedStruct.UARTRxIndex = 0;	//Reset the Rx index

		PORTA &= ~( LED_IN | LED_OUT ); 	//Turn off LED_IN (PORTA.1) & LED_OUT (PORTA.2)
	}
}

/*!
* TIMER0 interrupt service routine (period = 1 ms).
* Rx index reset.
*/
ISR(TIMER0_OVF_vect)	
{
	if(++ISRsSharedStruct.Timer0Prescaler >= 4)	//every 4 ms
	{
		ISRsSharedStruct.UARTRxIndex = 0;	//reset RXindex, period between two bytes received via UART must be shorter than 4 ms, otherwise it is considered to be a new packet
		ISRsSharedStruct.Timer0Prescaler = 0;	//reset prescaler
	}
}

/*!
* TIMER2 interrupt service routine (period = 2 ms).
* Measures the range every 50 ms, processes acquired data, PSD motors' regulators are run here.
*/
ISR(TIMER2_OVF_vect)
{
	static uint8_t Timer2prescaler = 0;	//Counter of software prescaling
	static int16_t RegErrorR_Prev = 0, RegErrorL_Prev = 0; //Previous error
	static uint8_t RegOutputR_Prev = SPEED_OFFSET, RegOutputL_Prev = SPEED_OFFSET;	//Previous regulator outputs to calculate acceleration ramp (must add the offset (128)!) 
	static uint8_t DistanceSafetySensorFirst = 0, DistanceSafetySensorSecond = 0;	//Distances mesured by safety sensors

	switch(Timer2prescaler)
	{
		default: 	//Increase the prescaler
					Timer2prescaler++; 
					
					break;

		
		case TIMER2_SEND_SRF08_COMMAND:	//Generate write command (measurement in centimeters), check UART watchdog
					Timer2prescaler++;	

					//SRF08 start measurement sequence	
					SRF08_Write(ConfigStructRAM.SRF08_Front,0x00, 0x51);	//front SRF08 command to start meas.
					SRF08_Write(ConfigStructRAM.SRF08_Rear,0x00, 0x51);		//rear SRF08 command to start meas.

					//Optionally start measuring using first safety sensor
					if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
					{
						SRF08_Write(ConfigStructRAM.SRF08_SafetyFirst,0x00, 0x51);		//rear SRF08 command to start meas.
					}

					//Optionally start measuring using second safety sensor
					if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
					{
						SRF08_Write(ConfigStructRAM.SRF08_SafetySecond,0x00, 0x51);		//rear SRF08 command to start meas.
					}

					//Check UART watchdog status
					if(++ISRsSharedStruct.UARTWatchdog >= UART_WATCHDOG_TIMEOUT) //no data received for specified interval -> stop motors
					{
						ISRsSharedStruct.EnableWallFollowing = FALSE; //disable wall following algorithm

						ISRsSharedStruct.EnablePositionRegulator = FALSE; //disable position regulator
							
						ISRsSharedStruct.EnableSpRegR = FALSE; 	//disable speed regulator
						ISRsSharedStruct.EnableSpRegL = FALSE;	//disable speed regulator

						ISRsSharedStruct.RegIntegratorR = 0;	//reset the integrator
						ISRsSharedStruct.RegIntegratorL = 0;	//reset the integrator

						ISRsSharedStruct.MomSpeedR = SPEED_OFFSET; //zero speed
						ISRsSharedStruct.MomSpeedL = SPEED_OFFSET; //zero speed

					}					

					break;
		
		case TIMER2_READ_SRF08_DATA:	//Read data from SRF08 sensors
					Timer2prescaler++;

					ISRsSharedStruct.SRF08_DistanceFront = SRF08_Read(ConfigStructRAM.SRF08_Front,0x03);	//read data from front sensor
					ISRsSharedStruct.SRF08_DistanceRear = SRF08_Read(ConfigStructRAM.SRF08_Rear,0x03);	//read data from rear sensor

					//Optionally start measuring using first safety sensor
					if(ConfigStructRAM.SRF08_SafetyFirst >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetyFirst <= SRF08_HIGHEST_ADDR)
					{
						DistanceSafetySensorFirst = SRF08_Read(ConfigStructRAM.SRF08_SafetyFirst,0x03);
					}
					else
					{
						DistanceSafetySensorFirst = 0;
					}

					//Optionally start measuring using second safety sensor
					if(ConfigStructRAM.SRF08_SafetySecond >= SRF08_LOWEST_ADDR && ConfigStructRAM.SRF08_SafetySecond <= SRF08_HIGHEST_ADDR)
					{
						DistanceSafetySensorSecond = SRF08_Read(ConfigStructRAM.SRF08_SafetySecond,0x03);
					}
					else
					{
						DistanceSafetySensorSecond = 0;
					}

					break;	

			case TIMER2_SPEED_REGULATORS:	//PSD speed regulators
					{
					int16_t RegOutputR, RegOutputL;	//output signal of PSD regulators
					int16_t RegErrorR, RegErrorL; //errors (desired - actual values)

					Timer2prescaler = 0;

					//Safety feedback (front sensor) - if distance measured by front SRF08 is lower than specified, stop motors
					if((ISRsSharedStruct.SRF08_DistanceFront <= SRF08_SAFETY_RANGE) && (ISRsSharedStruct.SRF08_DistanceFront))
					{
						//Stop motors immediately
						OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
						OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);

						break;
					}

					//Safety feedback (rear sensor)
					if((ISRsSharedStruct.SRF08_DistanceRear <= SRF08_SAFETY_RANGE) && (ISRsSharedStruct.SRF08_DistanceRear))
					{
						//Stop motors immediately
						OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
						OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);

						break;
					}

					//Safety feedback (first safety sensor)
					if((DistanceSafetySensorFirst <= SRF08_SAFETY_RANGE) && DistanceSafetySensorFirst)
					{
						//Stop motors immediately
						OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
						OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);

						break;
					}

					//Safety feedback (second safety sensor)
					if((DistanceSafetySensorSecond <= SRF08_SAFETY_RANGE) && DistanceSafetySensorSecond)
					{
						//Stop motors immediately
						OCR1A = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);
						OCR1B = (unsigned int)(SPEED_OFFSET + UCHAR_TO_RC_SIGNAL);

						break;
					}


					ISRsSharedStruct.ActualRPMR = ISRsSharedStruct.BufferSpeedR;	//impulses per 50 ms
					ISRsSharedStruct.ActualRPML = ISRsSharedStruct.BufferSpeedL;	//impulses per 50 ms

					ISRsSharedStruct.BufferSpeedR = 0;	//reset the buffer
					ISRsSharedStruct.BufferSpeedL = 0;	//reset the buffer

					if(ISRsSharedStruct.EnableSpRegR)	//if right motor regulator is enabled RegOutput must be calculated, else it is equal to desired momentum
					{
						RegErrorR = (int16_t)((ISRsSharedStruct.MomSpeedR - SPEED_OFFSET) - ISRsSharedStruct.ActualRPMR); //calculate error (substract the offset!)

						if((abs(RegErrorR) <= ConfigStructRAM.RegErrorFilter) && ISRsSharedStruct.MomSpeedR != SPEED_OFFSET)	//filter the error (disable if too close to zero speed)
						{
							RegErrorR = 0;
						}

						ISRsSharedStruct.RegIntegratorR += RegErrorR;	//integrate error

						if(ISRsSharedStruct.RegIntegratorR > INTEGRATOR_MAX)	//prevent integrator overflow
						{
							ISRsSharedStruct.RegIntegratorR = INTEGRATOR_MAX;	
						}

						if(ISRsSharedStruct.RegIntegratorR < INTEGRATOR_MIN)	//prevent integrator underflow
						{
							ISRsSharedStruct.RegIntegratorR = INTEGRATOR_MIN;	
						}

						RegOutputR = (int16_t)(((ConfigStructRAM.Kp * RegErrorR) + (ConfigStructRAM.Ki * ISRsSharedStruct.RegIntegratorR) + (ConfigStructRAM.Kd * (RegErrorR - RegErrorR_Prev)))/10);	//calculate regulator output (must be divided by 10, constants are multiplied by 10!)

						RegErrorR_Prev = RegErrorR;	//save previous error

						RegOutputR += SPEED_OFFSET;	//add the offset (128 = zero speed)
					}
					else
					{
						RegOutputR = (int16_t)ISRsSharedStruct.MomSpeedR;
					}

					if(ISRsSharedStruct.EnableSpRegL)	//if left motor regulator is enabled RegOutput must be calculated, else it is equal to desired momentum
					{
						RegErrorL = (int16_t)((ISRsSharedStruct.MomSpeedL - SPEED_OFFSET) - ISRsSharedStruct.ActualRPML); //calculate error (substract the offset!)

						if((abs(RegErrorL) <= ConfigStructRAM.RegErrorFilter) && ISRsSharedStruct.MomSpeedL != SPEED_OFFSET)	//filter the error (disable if too close to zero speed)
						{	
							RegErrorL = 0;
						}

						ISRsSharedStruct.RegIntegratorL += RegErrorL;	//integrate error

						if(ISRsSharedStruct.RegIntegratorL > INTEGRATOR_MAX)	//prevent integrator overflow
						{
							ISRsSharedStruct.RegIntegratorL = INTEGRATOR_MAX;	
						}

						if(ISRsSharedStruct.RegIntegratorL < INTEGRATOR_MIN)	//prevent integrator underflow
						{
							ISRsSharedStruct.RegIntegratorL = INTEGRATOR_MIN;	
						}

						RegOutputL = (int16_t)(((ConfigStructRAM.Kp * RegErrorL) + (ConfigStructRAM.Ki * ISRsSharedStruct.RegIntegratorL) + (ConfigStructRAM.Kd * (RegErrorL - RegErrorL_Prev)))/10);	//calculate regulator output (must be divided by 10, constants are multiplied by 10!)

						RegErrorL_Prev = RegErrorL;	//save previous error

						RegOutputL += SPEED_OFFSET;	//add the offset (128 = zero speed)
					}
					else
					{
						RegOutputL = (int16_t)ISRsSharedStruct.MomSpeedL;
					}

					if (abs(RegOutputR - RegOutputR_Prev) > ConfigStructRAM.MaxAcc)	//right motor acceleration ramp
					{
						if(RegOutputR > RegOutputR_Prev)		//difference cannot be greater than MaxAcc value
						{	
							RegOutputR = RegOutputR_Prev + ConfigStructRAM.MaxAcc;
						}		
						else
						{
							RegOutputR = RegOutputR_Prev - ConfigStructRAM.MaxAcc;			
						}
					}

					if (abs(RegOutputL - RegOutputL_Prev) > ConfigStructRAM.MaxAcc)	//left motor acceleration ramp
					{
						if(RegOutputL > RegOutputL_Prev)		//difference cannot be greater than MaxAcc value
						{	
							RegOutputL = RegOutputL_Prev + ConfigStructRAM.MaxAcc;
						}	
						else
						{
							RegOutputL = RegOutputL_Prev - ConfigStructRAM.MaxAcc;
						}		
					}

					if(RegOutputR > REG_OUTPUT_MAX)	//right motor saturation & anti-windup
					{
						//ISRsSharedStruct.RegIntegratorR -= (int16_t)(RegOutputR - REG_OUTPUT_MAX);

						RegOutputR = REG_OUTPUT_MAX;					
					}

					if(RegOutputR < REG_OUTPUT_MIN)		//right motor saturation & anti-windup
					{
						//ISRsSharedStruct.RegIntegratorR -= RegOutputR;

						RegOutputR = REG_OUTPUT_MIN;
					}

					
					if(RegOutputL > REG_OUTPUT_MAX) 	//left motor saturation & anti-windup
					{
						//ISRsSharedStruct.RegIntegratorL -= (int16_t)(RegOutputL - REG_OUTPUT_MAX);

						RegOutputL = REG_OUTPUT_MAX;
					}

					if(RegOutputL < REG_OUTPUT_MIN)		//left motor saturation & anti-windup
					{
						//ISRsSharedStruct.RegIntegratorL -= RegOutputL;

						RegOutputL = REG_OUTPUT_MIN;
					}

					RegOutputR_Prev = RegOutputR;	//save previous reg. output
					RegOutputL_Prev = RegOutputL;	//save previous reg. output

					OCR1A = (unsigned int)(RegOutputL + UCHAR_TO_RC_SIGNAL);	//calculate values of OCR1A(left wheel) signal generator registers	
					OCR1B = (unsigned int)(RegOutputR + UCHAR_TO_RC_SIGNAL);	//calculate values of OCR1B (right wheel) signal generator registers	
					
					break;

					}	
	}

	WATCHDOG_RESET();	//watchdog reset
}

/*!
* External interrupt 0 (left wheel encoder signal) service routine.
*/
ISR(INT0_vect)	
{
	if(LEFT_WHEEL_STEP)	//Rising edge
	{
		if(LEFT_WHEEL_DIR)		//90 degrees phase shifted signal on pin LEFT_WHEEL_DIR determines the direction
		{
			ISRsSharedStruct.BufferOdoL--;
			ISRsSharedStruct.BufferSpeedL--;
			ISRsSharedStruct.TotalImpulsesL--;
		}
		else
		{
			ISRsSharedStruct.BufferOdoL++;
			ISRsSharedStruct.BufferSpeedL++;
			ISRsSharedStruct.TotalImpulsesL++;
		}
	}
	else	//Falling edge
	{
		if(LEFT_WHEEL_DIR)		//90 degrees phase shifted signal on pin LEFT_WHEEL_DIR determines the direction
		{
			ISRsSharedStruct.BufferOdoL++;
			ISRsSharedStruct.BufferSpeedL++;
			ISRsSharedStruct.TotalImpulsesL++;
		}
		else
		{
			ISRsSharedStruct.BufferOdoL--;
			ISRsSharedStruct.BufferSpeedL--;
			ISRsSharedStruct.TotalImpulsesL--;
		}
	}

	if(GIFR & (1 << INTF0)) //debounce - if INT0 flag has been set, clear it (is cleared when 1 is written)
	{
		GIFR &= (1 << INTF0);
	}
}

/*!
* External interrupt 1 (right wheel encoder signal) service routine.
*/
ISR(INT1_vect)
{	
	if(RIGHT_WHEEL_STEP)	//Rising edge
	{
		if(RIGHT_WHEEL_DIR)	//90 degrees phase shifted signal on pin RIGHT_WHEEL_DIR determines the direction
		{
			ISRsSharedStruct.BufferOdoR++;
			ISRsSharedStruct.BufferSpeedR++;
			ISRsSharedStruct.TotalImpulsesR++;
		}
		else
		{
			ISRsSharedStruct.BufferOdoR--;
			ISRsSharedStruct.BufferSpeedR--;
			ISRsSharedStruct.TotalImpulsesR--;
		}
	}
	else	//Falling edge
	{
		if(RIGHT_WHEEL_DIR)	//90 degrees phase shifted signal on pin RIGHT_WHEEL_DIR determines the direction
		{
			ISRsSharedStruct.BufferOdoR--;
			ISRsSharedStruct.BufferSpeedR--;
			ISRsSharedStruct.TotalImpulsesR--;
		}
		else
		{
			ISRsSharedStruct.BufferOdoR++;
			ISRsSharedStruct.BufferSpeedR++;
			ISRsSharedStruct.TotalImpulsesR++;
		}
	}
	
	if(GIFR & (1 << INTF1))	//debounce - if INT1 flag has been set, clear it (is cleared when 1 is written)
	{
		 GIFR &= (1 << INTF1);
	}
}

/*!
* External interrupt 2 (emergency stop button) service routine.
*
* \warning This ISR runs in infinite loop until MCU reset to prevent PC app from controlling the wheelchair in case of emergency!
*/
ISR(INT2_vect)
{
	GLOBAL_INTERRUPTS_DISABLE();	//disable all interrupts	

	PORTC &= ~(0xFC);	//turn all power outputs off (pins 0/1 are used as SCL/SDA of I2C interface)

	OCR1A = SPEED_OFFSET + UCHAR_TO_RC_SIGNAL;	//left motor stop
	OCR1B = SPEED_OFFSET + UCHAR_TO_RC_SIGNAL;	//right motor stop

	TIMER_1_OFF();	//TIMER1 off - turn off R/C signal
	
	while(TRUE)	//infinite loop (blink the LEDs)
	{
		BLINK_LEDS_MS(100);	//Blink LEDs

		WATCHDOG_RESET();	//Reset watchdog
	}
}
