int main(void)
{

	// Initialize the serial port
	USART_Init();
	
	// Initialize encoder data
	Encoders_Init();

	// Initialize gyros
	Gyro_Init();

	// Initialize timers
	Timer_Init();

	// Enable global interrupts, sets l bit of SREG [334]
	sei();

	// Set switches data directional register as input
	// Defined in globals.h
	SW_DDR	=	0x00;
	
	// Set LED DDR as output. LEDs display debug mode
	LED_DDR	=	0xFF;

	// By default have the LEDs off
	LED_PORT	=	0x00;

	if(debug_mode == TRUE)
	{
		// Initialization message
		send_string("AVR board initialized...\r\n");
	}
	
	// Endless loop
	while(1)
	{
		USART_Transmit(0x10);
		USART_Transmit(0x00);
		
		// These values hold the values of the button and
		// the flag thats set when it's decided that it is
		// a valid fresh hit and not the user just holding
		// down the button.
		char button_value, button_push_valid, debug_sw_prev;

		button_value	=	(SW_PIN & 0b10000000)	==	0;

		// Make sure that the user isn't just holding down
		// the button
		button_push_valid	=	button_value && !debug_sw_prev;

		// Set the previous value to the current value, to
		// be used in the next loop.
		debug_sw_prev	=	button_value;
		
		// If this is a valid (not noise/held down) push
		if(button_push_valid)
		{
			// Flip flop modes
			if(debug_mode == TRUE)
			{
				// Lights off = debug mode off
				debug_mode	=	FALSE;
				LED_PORT		=	0xFF;
			}
			else
			{
				// Lights on = debug mode on
				debug_mode	=	TRUE;
				LED_PORT		=	0x00;
			}

		}
		
		// If the interrupt tripped, run the ISR.
		// We run it in the main loop as to not
		// miss as many encoder ticks.
		if(run_timer1_isr	==	TRUE)
		{

			// We sample the gyro every time 
			// (120Hz)
			Sample_Gyro();
			
			// Send a data packet every 3 times
			// (40Hz)
			if(packet_limiter	>=	3)
			{
				// If we are currently in debug mode, send a
				// packet that is easier for humans to read.
				// Else send the normal, RC optimized, packet.
				if(debug_mode == TRUE)
				{
					Send_Debug_Packet();
				}
				else
				{
				//	Send_RC_Packet();
				}
				
				// Reset the packet limiter
				packet_limiter	=	0;
			}
			
			// Reload the timer value so we can
			// control the interrupt frequency
			TCNT1	=	T1_LOAD;
			
			// Increment the limiter
			packet_limiter++;
			
		}

	}

}