// bot0.c
// prototype controller firmware for experimental robot 0
// written by dale wheat - 17 april 2008

// notes:
//	AVR Studio 4.14, Windows XP Pro, WinAVR 20070525, GCC 4.1.2
//	device = ATmega644-20PU
//	clock = 20 MHz crystal

//	conventions:
//		units of measure:
//			linear:  inches
//			angular:  radians
//			velocity:  inches per second

//		trigonometric notation is used for navigation, i.e.:
//			angles are in radians
//			0 radians is due east
//			counter-clockwise rotation as viewed from above is positive
//			motor [+] high and [-] low produces clockwise rotation and positive odometer values
//			right motor has 0 degree phase; correction factor of cos(0)
//			left motor has 180 degree phase; correction factor of cos(pi)

//	USART0 (was) connected to SparkFun Electronics BlueSmirf module (discontinued) (before it died)
//	default baud rate is 9600 (default is 9600bps, 8 Data Bits, No Parity, 1 Stop Bit, and hardware flow control enabled)
//	AT commands:
//		+++ enter command mode
//		ATMD return to data mode
//		ATVER,ver1 returns "Ver 2.8.1.4.0"
//		ATSI,0 returns BlueRadios AT
//		ATSI,1 returns 00A096127F03 (product ID code, BlueTooth address ID)
//		ATSI,8 returns 0027,0000,0000 (UART parameters in hex)
//		ATSW20,1024,0,0,0 sets baud rate to 250 Kbps
//	module defected on 20 april 2008 - will be replaced with Digi XBee modules

//	wheel information
//		motors:  faulhaber 1524S006 6V coreless DC motors with precious metal commutation
//		encoders:  magnetic quadrature 1:1 on motor output
//		quadrature encoding produces ~282 steps per wheel revolution
//		gearbox:  ~141:1 reduction (140.759183:1 per David Cook)
//		wheels:
//			right OD = 2.265", 7.116" cir, 0.0252" per odometer tick
//				revised:  2.2665", 7.120419749 cir, 0.025292914 per odometer tick
//			left OD = 2.255", 7.084" cir, 0.0251" per odometer tick
//				revised:  2.2575", 7.092145415" cir, 0.025192479 per odometer tick
//		wheelbase = 5.528" overall less average wheel thickness 0.280" = 5.248" effective

//	problems:
//		*fixed* intermittant power failures - suspect positive battery lead - not seen since power switch installed
//		*fixed* motor spins during ISP - suggest pull-down resistor on PWM driver pin - fix: 10 K pulldowns on PWM lines
//		right wheel is wobbly
//		startup delay - first timer tick is missed, but only after ISP or extended power off - BOR causes this

//	TODO
//		*done* add overhead timer to RTC heartbeat service
//		*done* caster - 3rd wheel
//		*done* wire bail jack stand
//		*done* power switch
//		*done* battery charge connector
//		*done* battery mount
//		radio mount
//		battery voltage input
//		*done* battery volt meter LCD-DVM
//		battery thermistor input
//		command interpreter
//		I2C interface for Wiimote nunchuk, classic controller
//		power control for SN754410
//		*done* interrupt-driven serial interface
//		*done* beeper/speaker
//		*in process* motion control alorithms
//		add blue LED for radio connection status
//		*done* add blue LED for general user interface
//		wiimote triangulation via sensor bar
//		IR obstacle avoidance + remote control
//		bump switches
//		accelerometer
//		balancing tests
//		'return to zero' demo
//		*done* add red 'error' LED, error() function and codes defined
//		charge detector circuit & 'submit patiently & monitor charge' behavior
//		add 'thinking' (CPU load) LED indicator
//		replace wire bail with more substantial kickstand
//		*done* increase RTC rate to 16 Hz
//		*done* add linear+angular objects
//		determine wheelbase, wheel diameters dynamically
//		add adjustable PID coefficients
//		*done* separate motor and motion objects
//		add error detection & stats to serial communication
//		*done* change sample frequency to 8 Hz
//		*done* double encoder resolution
//		*done* determine cause of reset
//		*done* multiple operation modes via reset button
//		*done* move hardware initialization routines to .init3 section
//		pin-point navigation
//		add tiered navigation code for 90 degree problem

#ifndef F_CPU
#define F_CPU 20000000
#endif // #ifdef F_CPU

#define sbi(port, pin) (port) |= (1 << (pin))
#define cbi(port, pin) (port) &= ~(1 << (pin))

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

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

// mechanical constants

#define WHEEL_MOTOR_GEAR_RATIO 140.759183 // to 1
#define ENCODER_TICK_RATIO 4 // number of ticks produced by one complete quadrature encoder cycle

#define RIGHT_WHEEL_OD 2.2665 // right wheel outside diameter in inches, including o-ring tire
#define RIGHT_WHEEL_CIRCUMFERENCE RIGHT_WHEEL_OD * M_PI // right wheel circumference in inches
#define RIGHT_WHEEL_TICK RIGHT_WHEEL_CIRCUMFERENCE / (WHEEL_MOTOR_GEAR_RATIO * ENCODER_TICK_RATIO) // inches per odometer tick

#define LEFT_WHEEL_OD 2.2575 // left wheel outside diameter in inches, including o-ring tire
#define LEFT_WHEEL_CIRCUMFERENCE LEFT_WHEEL_OD * M_PI // left wheel circumference in inches
#define LEFT_WHEEL_TICK LEFT_WHEEL_CIRCUMFERENCE / (WHEEL_MOTOR_GEAR_RATIO * ENCODER_TICK_RATIO) // inches per odometer tick

//#define WHEELBASE 5.248 // effective wheelbase dimension in inches - original value - shows early on test
//#define WHEELBASE 5.250 // test value slightly larger - still early, but gets there slower
#define WHEELBASE 5.252 // in the middle
//#define WHEELBASE 5.255 // more bigger -maybe a little too big

// program constants

#define PWM_PERIOD 100 // for wheel motor power, 16 bit max value
#define TICKS_PER_SECOND 8 // RTC & sampling period frequency:  4, 8 or 16 Hz supported
#define TPS TICKS_PER_SECOND // shorthand

// operational modes

typedef enum {
	MODE_STANDBY,
	MODE_SHORT_TRIP,
	MODE_SLALOM,
	MODE_SLALOM8,
	MODE_LONG_TRIP,
	MODE_LONG_RH_SQUARE,
	MODE_LONG_LH_SQUARE,
	MODE_DEBUG,
	MODE_MAX
} MODE;

// global variables

volatile unsigned char reset __attribute__ ((section(".noinit"))); // cause of reset bitmask
volatile unsigned char mode __attribute__ ((section(".noinit"))); // operational mode

//volatile unsigned char motor_control_flag = 1; // adjust power to motors to achieve desired velocity

// real time clock
volatile unsigned char ticks;
volatile unsigned char seconds;
volatile unsigned char minutes;
volatile unsigned char hours;

// floating-point odometry

volatile double x = -12.0, y = 12.0; // absolute coordinates in inches; +x=east, +y=north
volatile double theta = 0.0; // absolute orientation in radians, looking down

// delay() - user-perceptible delay in 1/4 increments
//		note:  requires interrupts to be enabled - uses RTC tick handler

//volatile unsigned char rtc_overhead; // 1/(256 * TPS) second ticks consumed in RTC service routine

volatile unsigned int foreground_downcounter; // for timing & delay functions

void delay(unsigned int n) {

	foreground_downcounter = n;

	while(foreground_downcounter); // variable gets decremented in RTC tick handler every 1/4 second
}

#define delay_seconds(n) delay(n * TPS)

// delay24() - 24 bit delay subroutine:  kills 5n + 10 cycles

void delay24(unsigned long n) __attribute__ ((naked));
void delay24(unsigned long n) {

	asm("1:	subi	r22, 1");
	asm("	sbci	r23, 0");
	asm("	sbci	r24, 0");
	asm("	brne	1b");
	asm("	ret");
}

// LED functions

#define error_led_on() sbi(PORTC, PC2)
#define error_led_off() cbi(PORTC, PC2)

typedef enum {
	ERROR_NONE			= 0,	// no error
	ERROR_RESET			= 1,	// bad reset state - probably caused by software runaway and re-execution of init() function
	ERROR_EXCEPTION		= 2,	// unhandled exception encountered - bad interrupt or stack overflow

	ERROR_TEST			= 9,	// *** debug *** test error code
} ERROR;

prog_char error_msg_none[] = "";
prog_char error_msg_reset[] = "bad reset state";
prog_char error_msg_exception[] = "unhandled exception";

PGM_P error_msg[] = {
	error_msg_none, // no error
	error_msg_reset,
	error_msg_exception,
};

void error(ERROR e) __attribute__ ((naked, noreturn));
void error(ERROR e) {

	unsigned char i;

	cli(); // stop responding to interrupts

	if(e == ERROR_NONE) {

		error_led_on(); // no error indicated (this is an error), so turn on error LED and halt
		while(1);

	} else {

		// a specific error has occurred
		fprintf_P(stderr, PSTR("[%S - system halted]"), error_msg[e]); // tell the world about it
		
		// blink the error LED
		while(1) {
			for(i = 0; i < e; i++) {
				error_led_on();
				delay24(1000000); // ~1/4 second delay
				error_led_off();
				delay24(1000000); // ~1/4 second delay
			}
			delay24(4000000); // ~1 second delay
		}
	}
}

#define user_led_on() sbi(PORTC, PC3)
#define user_led_off() cbi(PORTC, PC3)
#define user_led_toggle() sbi(PINC, PC3)

// speaker/beeper functions

void silence(void) {

	OCR0A = 0; // ahh, blessed silence
}

void tick(void) {

	OCR0A = 6; // shrill, piercing tone
	delay24(10000); // 
	silence();
}

void chirp(void) {

	OCR0A = 6; // shrill, piercing tone
	delay24(50000); // 
	silence();
}

void beep(void) {

	OCR0A = 6; // shrill, piercing tone
	delay24(500000); // ~1/8 second
	silence();
}

void buzz(void) {

	OCR0A = 50;
	delay24(10000);
	silence();
}

// USART functions

#define USART_BUFFER_SIZE 256

char usart_txd_buffer[USART_BUFFER_SIZE];
char usart_rxd_buffer[USART_BUFFER_SIZE];
volatile unsigned char usart_txd_in;
volatile unsigned char usart_txd_out;
volatile unsigned char usart_rxd_in;
volatile unsigned char usart_rxd_out;

// usart_putchar() - schedule a single character to be transmitted

static int usart_putchar(char c, FILE *stream) {

	if(c == '\n') usart_putchar('\r', stream);

	while((unsigned char)(usart_txd_in + 1) == usart_txd_out); // wait for room in the queue
	usart_txd_buffer[usart_txd_in] = c; // store in buffer
	usart_txd_in++;
	sbi(UCSR0B, UDRIE0); // schedule an interrupt when the current transmission (if any) is complete to start this transmission

	return 0; // report unconditional success
}

// usart_putchar_polled() - transmit a single character without interrupts

static int usart_putchar_polled(char c, FILE *stream) {

	if(c == '\n') usart_putchar('\r', stream);

	while(bit_is_clear(UCSR0A, UDRE0)); // wait for USART to be ready to transmit
	UDR0 = c; // transmit character

	return 0; // this always works
}

// usart_puthex() - print 2 hex digits

const char hex[] = "0123456789ABCDEF";

void usart_puthex(unsigned char h) {

	usart_putchar(hex[h >> 4], stdout); // upper 4 bits
	usart_putchar(hex[h & 0x0f], stdout); // lower 4 bits
}

// usart_getchar() - receive a single character

static int usart_getchar(FILE *stream) {

	while(usart_rxd_in == usart_rxd_out); // wait for something to arrive
	return (int) usart_rxd_buffer[usart_rxd_out++];
}

// usart_getchar_polled() - receive a single character without buffering or interrupts

//static int usart_getchar_polled(FILE *stream) {
//
//	while(bit_is_clear(UCSR0A, RXC0)); // wait for character to arrive
//	return UDR0; // return it
//}

// usart_getline

signed char usart_getline(FILE *stream, char *buffer, const char *prompt) {

	signed char result = 0;

	printf_P(PSTR("\n%S"), prompt); // display prompt

	strcpy(buffer, "test"); // *** debug *** return string "test"
	result = 1; // *** debug *** fake return status

	return result;
}

static FILE USART0 = FDEV_SETUP_STREAM(usart_putchar, usart_getchar, _FDEV_SETUP_RW); // stdout & stdin
//static FILE USART0 = FDEV_SETUP_STREAM(usart_putchar, usart_getchar_polled, _FDEV_SETUP_RW); // stdout (buffered) & stdin (polled)
static FILE USART0_POLLED = FDEV_SETUP_STREAM(usart_putchar_polled, NULL, _FDEV_SETUP_WRITE); // stderr (polled)

// motor power functions

typedef struct {
	double power; // value between -1.0 (full reverse) and +1.0 (full forward)
	int pwm_power; // combines sign & magnitude 
	int odometer; // in encoder ticks
	int previous_odometer; // previous odometer reading in encoder ticks
	int velocity; // in encoder ticks per sample period
} WHEEL;

volatile WHEEL right_wheel, left_wheel; // the two powered wheels

typedef struct {
	double odometer; // in inches
	double previous_odometer; // previous odometer reading in inches
	double velocity_setpoint; // requested velocity in inches per sample period
	double velocity; // actual velocity in inches per sample period
	double p_error; // proportional error
	double i_error; // integral error
	double d_error; // derivative error
} MOTION;

volatile MOTION right, left, angular, linear; // motions

void right_wheel_power(double power) {

	// constrain power value
	if(power > 1.0) {
		power = 1.0; // max
	} else if(power < -1.0) {
		power = -1.0; // min
	}

	if(power > 0.0) {
		sbi(PORTB, PB7);
		cbi(PORTD, PD7);
		right_wheel.pwm_power = power * PWM_PERIOD;
	} else if(power < 0.0) {
		cbi(PORTB, PB7);
		sbi(PORTD, PD7);
		right_wheel.pwm_power = power * -PWM_PERIOD;
	} else {
		cbi(PORTB, PB7);
		cbi(PORTD, PD7);
		right_wheel.power = 0;
	}
	OCR1A = right_wheel.pwm_power;
	right_wheel.power = power;
}

void left_wheel_power(double power) {

	// constrain power value
	if(power > 1.0) {
		power = 1.0; // max
	} else if(power < -1.0) {
		power = -1.0; // min
	}

	if(power > 0.0) {
		sbi(PORTB, PB6);
		cbi(PORTD, PD6);
		left_wheel.pwm_power = power * PWM_PERIOD;
	} else if(power < 0.0) {
		cbi(PORTB, PB6);
		sbi(PORTD, PD6);
		left_wheel.pwm_power = power * -PWM_PERIOD;
	} else {
		cbi(PORTB, PB6);
		cbi(PORTD, PD6);
		left_wheel.pwm_power = 0;
	}
	OCR1B = left_wheel.pwm_power;
	left_wheel.power = power;
}

// motor_control() - adjust motor power to maintain linear & angular velocities

// angular PID coefficients
//#define Pangular 0.01
//#define Iangular 0.0002
//#define Dangular 0.02

// 8 Hz
#define Pangular 0.25
#define Iangular 0.0010
#define Dangular 0.500

// linear PID coefficients
//#define Plinear 0.01
//#define Ilinear 0.0002
//#define Dlinear 0.02

// these work OK @ 16 Hz
//#define Plinear 0.015
//#define Ilinear 0.0001
//#define Dlinear 0.2

// 8 Hz
#define Plinear 0.2
#define Ilinear 0.0010
#define Dlinear 0.5000

void motor_control(void) {

	double angular_error, linear_error, previous_error;

//	if(motor_control_flag) {

		// calculate angular velocity error
		previous_error = angular.p_error;
		angular.p_error = angular.velocity_setpoint - angular.velocity; // proportional error
		angular.i_error += angular.p_error; // integral error
		angular.d_error = angular.p_error - previous_error; // derivative error
		angular_error = (Pangular * angular.p_error) + (Iangular * angular.i_error) + (Dangular * angular.d_error); // wighted sum of error terms

		// calculate linear velocity error
		previous_error = linear.p_error;
		linear.p_error = linear.velocity_setpoint - linear.velocity; // proportional error
		linear.i_error += linear.p_error; // integral error
		linear.d_error = linear.p_error - previous_error; // derivative error
		linear_error = (Plinear * linear.p_error) + (Ilinear * linear.i_error) + (Dlinear * linear.d_error); // weighted sum of error terms

		right_wheel_power(right_wheel.power + (angular_error + linear_error));
		left_wheel_power(left_wheel.power + (angular_error - linear_error));
//	}
}

// navigation functions

// hypot() - returns length of a right triangle's hypotenuse given lengths of sides a & b

double hypot(double a, double b) {

	return sqrt(square(a) + square(b));
}

// distance() - returns straight-line distance between points (x0,y0) and (x1,y1)

double distance(double x0, double y0, double x1, double y1) {

	return hypot(x0 - x1, y0 - y1); // distance formula always returns positive result
}

// go() - specify heading in terms of angular & linear velocities, both in inches per second

void go(double angular_velocity, double linear_velocity) {

	angular.velocity_setpoint = angular_velocity / TPS; // suggest angular velocity
	linear.velocity_setpoint = linear_velocity / TPS; // suggest linear velocity
}

// normal() - return similar angle in radians in range -pi to +pi

double normal(double a) {

	double result;

	result = fmod(a, M_PI * 2.0); // 0-2pi
	if(result > M_PI) result -= M_PI; // -pi to +pi

	return result;
}

// target_angle() - returns relative angle in radians to target point (target_x, target_y)

double target_angle(double target_x, double target_y) {

	return atan2(target_y - y, target_x - x); // relative to present position
}

// target_distance() - returns distance in inches to target point (target_x, target_y)

double target_distance(double target_x, double target_y) {

	return distance(target_x, target_y, x, y);
}

// go_to() - navigate to target point (target_x, target_y) at velocity in inches per second

void go_to(double target_x, double target_y, double velocity) {

	double navigation_tolerance;
	double angular_error, linear_error, velocity_error;

	// calculate navigation tolerance based on suggested velocity and sampling frequency
	navigation_tolerance = (velocity / TICKS_PER_SECOND) * 1.25; // with some built-in overlap

	// arrive within initial navigational tolerance
	do {
		delay(1); // wait for odometry update
		printf_P(PSTR("go_to(% 7.3f,% 7.3f, % 6.3f): "), target_x, target_y, velocity); // parameters
		angular_error = target_angle(target_x, target_y) - theta; // the relative need to turn
		linear_error = target_distance(target_x, target_y); // how far away
		velocity_error = velocity - (linear.velocity * TICKS_PER_SECOND); // required change in velocity in inches per second
		printf_P(PSTR("A% 7.3f L% 7.3f V% 7.3f "), angular_error, linear_error, velocity_error); // errors

		go(cos(angular_error) * sin(angular_error) * velocity, cos(angular_error) * velocity);
		printf_P(PSTR("go(% 7.3f,%7.3f)\n"), cos(angular_error) * sin(angular_error) * velocity, cos(angular_error) * velocity);

		/*if(angular_error > 1.0) {
			go(1.0, 0.0); // turn left
			printf_P(PSTR("1 go(% 7.3f, 0)\n"), 1.0);
		} else if(angular_error < -1.0) {
			go(-1.0, 0.0); // turn right
			printf_P(PSTR("2 go(% 7.3f, 0)\n"), -1.0);
		} else if(velocity_error > 1.0) {
			go(angular_error, linear.velocity * TPS + 1.0); // ramp forward
			printf_P(PSTR("3 go(% 7.3f,%7.3f)\n"), angular_error, linear.velocity * TPS + 1.0);
		} else if(velocity_error < -1.0) {
			go(angular_error, linear.velocity * TPS - 1.0); // ramp
			printf_P(PSTR("4 go(% 7.3f,%7.3f)\n"), angular_error, linear.velocity * TPS - 1.0);
		} else if(linear_error > 1.0) {
			go(angular_error, velocity);
			printf_P(PSTR("5 go(% 7.3f,%7.3f)\n"), angular_error, velocity);
		} else {
			go(angular_error, velocity * linear_error);
			printf_P(PSTR("6 go(% 7.3f,%7.3f)\n"), velocity * linear_error);
		}*/
	} while(linear_error > navigation_tolerance);

//	chirp(); // *** debug *** announce arrival at target coordinates
}

// *** initialization functions ***

// init_reset() - determine cause of reset

void init_reset(void) __attribute__ ((naked, section(".init3")));
void init_reset(void) {

	reset = MCUSR; // collect reset cause
	MCUSR = 0; // clear reset flags

	if(reset == 0) {
		// not supposed to happen, but does
		// suspect a software malfunction
		error(ERROR_RESET); // blink error LED
	} else if(reset & 1<<PORF) {
		// power on reset
		mode = MODE_STANDBY; // reset operational mode
	} else if(reset & 1<<BORF) {
		// brown-out reset
	} else if(reset & 1<<EXTRF) {
		// external reset (reset button pressed)
		mode++; // increment operational mode
		if(mode == MODE_MAX) mode = 0; // wrap back to the beginning
	} else if(reset & 1<<WDRF) {
		// watchdog reset
	} else if(reset & 1<<JTRF) {
		// JTAG reset
	} else {
		// ??? unknown/unexpected reset source
	}
}

// init_io() - initialize ATmega644 input & output ports
//  note:  pin numbers are for DIP40 package

//void init_io(void) __attribute__ ((naked, section(".init3")));
void init_io(void) {

	// PORTA

		// PA0 pin 40 PCINT0/ADC0
		// PA1 pin 39 PCINT1/ADC1
		// PA2 pin 38 PCINT2/ADC2
		// PA3 pin 37 PCINT3/ADC3
		// PA4 pin 36 PCINT4/ADC4
		// PA5 pin 35 PCINT5/ADC5
		// PA6 pin 34 PCINT6/ADC6
		// PA7 pin 33 PCINT7/ADC7

	PORTA = 0<<PORTA7 | 0<<PORTA6 | 0<<PORTA5 | 0<<PORTA4 | 0<<PORTA3 | 0<<PORTA2 | 0<<PORTA1 | 0<<PORTA0;
	DDRA = 0<<DDA7 | 0<<DDA6 | 0<<DDA5 | 0<<DDA4 | 0<<DDA3 | 0<<DDA2 | 0<<DDA1 | 0<<DDA0;

	// PORTB

		// PB0 pin 1  PCINT8/XCK0/T0		right wheel motor encoder phase B
		// PB1 pin 2  PCINT9/CLKO/T1		left wheel motor encoder phase B
		// PB2 pin 3  PCINT10/INT2/AIN0
		// PB3 pin 4  PCINT11/OC0A/AIN1		speaker/beeper output
		// PB4 pin 5  PCINT12/OC0B/-SS		SPI wheel encoder power output
		// PB5 pin 6  PCINT13/MOSI			SPI/ISP
		// PB6 pin 7  PCINT14/MISO			SPI/ISP left wheel motor direction output + 3A
		// PB7 pin 8  PCINT15/SCK			SPI/ISP right wheel motor direction output + 1A

	PORTB = 0<<PORTB7 | 0<<PORTB6 | 0<<PORTB5 | 1<<PORTB4 | 0<<PORTB3 | 0<<PORTB2 | 0<<PORTB1 | 0<<PORTB0;
	DDRB = 1<<DDB7 | 1<<DDB6 | 0<<DDB5 | 1<<DDB4 | 1<<DDB3 | 0<<DDB2 | 0<<DDB1 | 0<<DDB0;

	// PORTC

		// PC0 pin 22 PCINT16/SCL
		// PC1 pin 23 PCINT17/SDA
		// PC2 pin 24 PCINT18/TCK			JTAG red 'error' LED output
		// PC3 pin 25 PCINT19/TMS			JTAG blue 'user interface' LED
		// PC4 pin 26 PCINT20/TDO			JTAG
		// PC5 pin 27 PCINT21/TDI			JTAG
		// PC6 pin 28 PCINT22/TOSC1			32KHz crystal
		// PC7 pin 29 PCINT23/TOSC2			32KHz crystal

	PORTC = 0<<PORTC7 | 0<<PORTC6 | 0<<PORTC5 | 0<<PORTC4 | 0<<PORTC3 | 0<<PORTC2 | 0<<PORTC1 | 0<<PORTC0;
	DDRC = 0<<DDC7 | 0<<DDC6 | 0<<DDC5 | 0<<DDC4 | 1<<DDC3 | 1<<DDC2 | 0<<DDC1 | 0<<DDC0;

	// PORTD

		// PD0 pin 14 PCINT24/RXD0			USART0 RXD
		// PD1 pin 15 PCINT25/TXD0			USART0 TXD
		// PD2 pin 16 PCINT26/INT0			right wheel motor encoder phase A
		// PD3 pin 17 PCINT27/INT1			left wheel motor encoder phase A
		// PD4 pin 18 PCINT28/OC1B			left wheel motor PWM power output 3,4EN - requires external 10 K pulldown
		// PD5 pin 19 PCINT29/OC1A			right wheel motor PWM power output 1,2EN - requires external 10 K pulldown
		// PD6 pin 20 PCINT30/OC2B/ICP		left wheel motor direction output - 4A
		// PD7 pin 21 PCINT31/OC2A			right wheel motor direction output - 2A

	PORTD = 0<<PORTD7 | 0<<PORTD6 | 0<<PORTD5 | 0<<PORTD4 | 0<<PORTD3 | 0<<PORTD2 | 0<<PORTD1 | 0<<PORTD0;
	DDRD = 1<<DDD7 | 1<<DDD6 | 1<<DDD5 | 1<<DDD4 | 0<<DDD3 | 0<<DDD2 | 0<<DDD1 | 0<<DDD0;
}

// init_timers() - initialize on-chip timer/counters

//void init_timers(void) __attribute__ ((naked, section(".init3")));
void init_timers(void) {

	// timer/counter0 (8 bit) - speaker/beeper

	TCCR0A = 0<<COM0A1 | 1<<COM0A0 | 0<<COM0B1 | 0<<COM0B0 | 0<<WGM01 | 1<<WGM00;
	TCCR0B = 0<<FOC0A | 0<<FOC0B | 1<<WGM02 | 1<<CS02 | 0<<CS01 | 0<<CS00;
	OCR0A = 0; // silence

	// timer/counter1 (16 bit) - motor power PWM

	ICR1 = PWM_PERIOD; // PWM period in double F_CPU units, i.e., 1000 = 100 us/10 KHz
	OCR1A = 0;
	OCR1B = 0;
	TCCR1A = 1<<COM1A1 | 0<<COM1A0 | 1<<COM1B1 | 0<<COM1B0 | 0<<WGM11 | 0<<WGM10;
	TCCR1B = 0<<ICNC1 | 0<<ICES1 | 1<<WGM13 | 0<<WGM12 | 0<<CS12 | 0<<CS11 | 1<<CS10;
	TCCR1C = 0<<FOC1A | 0<<FOC1B;
	TIMSK1 = 0<<ICIE1 | 0<<OCIE1B | 0<<OCIE1A | 0<<TOIE1; // interrupts

	// timer/counter2 (8 bit) - RTC

	ASSR = 0<<EXCLK | 1<<AS2; // clock via external 32768 Hz crystal
	if(TICKS_PER_SECOND == 4) {
		TCCR2A = 0<<COM2A1 | 0<<COM2A0 | 0<<COM2B1 | 0<<COM2B0 | 0<<WGM21 | 0<<WGM20;
		TCCR2B = 0<<FOC2A | 0<<FOC2B | 0<<WGM22 | 0<<CS22 | 1<<CS21 | 1<<CS20;
	} else if(TICKS_PER_SECOND == 8) {
		TCCR2A = 0<<COM2A1 | 0<<COM2A0 | 0<<COM2B1 | 0<<COM2B0 | 0<<WGM21 | 1<<WGM20;
		TCCR2B = 0<<FOC2A | 0<<FOC2B | 0<<WGM22 | 0<<CS22 | 1<<CS21 | 0<<CS20;
	} else if(TICKS_PER_SECOND == 16) {
		TCCR2A = 0<<COM2A1 | 0<<COM2A0 | 0<<COM2B1 | 0<<COM2B0 | 0<<WGM21 | 0<<WGM20;
		TCCR2B = 0<<FOC2A | 0<<FOC2B | 0<<WGM22 | 0<<CS22 | 1<<CS21 | 0<<CS20;
	}
	TIMSK2 = 0<<OCIE2B | 0<<OCIE2A | 1<<TOIE2; // interrupts
}

// init_interrupts() - initialize external interrupts

//void init_interrupts(void) __attribute__ ((naked, section(".init3")));
void init_interrupts(void) {

	// external interrupts INT0 (right) and INT1 (left) are used for motor encoder A inputs
	EICRA = 0<<ISC21 | 0<<ISC20 | 0<<ISC11 | 1<<ISC10 | 0<<ISC01 | 1<<ISC00;
	EIMSK = 0<<INT2 | 1<<INT1 | 1<<INT0;

	// pin change interrupts PCINT8 (right) and PCINT9 (left) are used for motor encoder B inputs
	PCICR = 0<<PCIE3 | 0<<PCIE2 | 1<<PCIE1 | 0<<PCIE0;
	PCMSK1 = 1<<PCINT9 | 1<<PCINT8; // just enable these
}

// init_usart() - initialize on-chip USART

#define BAUD_RATE 115200
#define BAUD_RATE_DIVISOR (F_CPU / (BAUD_RATE * 16.0) - 0.5)

//void init_usart(void) __attribute__ ((naked, section(".init3")));
void init_usart(void) {

	UCSR0A = 1<<TXC0 | 0<<U2X0 | 0<<MPCM0;
	UCSR0B = 1<<RXCIE0 | 0<<TXCIE0 | 0<<UDRIE0 | 1<<RXEN0 | 1<<TXEN0 | 0<<UCSZ02 | 0<<TXB80;
	//UCSR0B = 0<<RXCIE0 | 0<<TXCIE0 | 0<<UDRIE0 | 1<<RXEN0 | 1<<TXEN0 | 0<<UCSZ02 | 0<<TXB80;
	UCSR0C = 0<<UMSEL01 | 0<<UMSEL00 | 0<<UPM01 | 0<<UPM00 | 0<<USBS0 | 1<<UCSZ01 | 1<<UCSZ00 | 0<<UCPOL0;
	UBRR0 = BAUD_RATE_DIVISOR;
}

// init_stdio() - initialize standard I/O

void init_stdio(void) {

	stdin =	stdout = stderr = &USART0; // stdin & stdout use interrupt-driven USART code
	stderr = &USART0_POLLED; // stderr uses polled mode USART code
}

// init_modem() - initialize Bluetooth modem

//void init_modem(void) {
//
//	usart_puts("+++\n");
//	usart_puts("ATSW20,1024,0,0,0\n"); // 250 Kbps
//	usart_puts("ATMD\n");
//	ATSW21,2560,11,2560,11 for lower power
//}

// init_motor_encoders() - initialize motor encoders

#define MOTOR_ENCODER_POWER_PORT PORTB
#define MOTOR_ENCODER_POWER_PIN PB4

#define motor_encoder_power_on() MOTOR_ENCODER_POWER_PORT |= 1<<MOTOR_ENCODER_POWER_PIN
#define motor_encoder_power_off() MOTOR_ENCODER_POWER_PORT &= ~(1<<MOTOR_ENCODER_POWER_PIN)

//void init_motor_encoders(void) {
//
//	motor_encoder_power_on(); // apply power to both motor encoders
//}

// init() - initialize everything

void init(void) {

	//init_reset(); // determine cause of reset
	init_io(); // initialize on-chip input & output ports
	init_timers(); // initialize on-chip timer/counters
	init_interrupts(); // initialize external interrupts
	init_usart(); // initialize on-chip USART

	init_stdio(); // initialize standard I/O
//	init_modem(); // initialize Bluetooth modem
//	init_motor_encoders(); // initialize motor encoders

	sei(); // enable global interrupts

//	printf_P(PSTR("INIT: "));
//	switch(reset) {
//		case 1<<PORF: printf_P(PSTR("PORF")); break;
//		case 1<<EXTRF: printf_P(PSTR("EXTRF")); break;
//		case 1<<BORF: printf_P(PSTR("BORF")); break;
//		case 1<<WDRF: printf_P(PSTR("WDRF")); break;
//		case 1<<JTRF: printf_P(PSTR("JTRF")); break;
//		default: printf_P(PSTR("invalid code %i"), reset); break;
//	}

//	printf_P(PSTR("\nExperimental Robot #0 Daphne ready!\n")); // let's go!
}

// main() - main program function

void main(void) __attribute__((naked, noreturn));
void main(void) {

	char c;
	int i, j;

	init(); // intialize everything

	// announce operational mode
	for(i = 0; i < mode; i++) {
		user_led_on();
		chirp();
		user_led_off();
		delay(1);
	}
	delay_seconds(2); // wait 2 seconds before starting

	// execute operational mode
	switch(mode) {

		case MODE_STANDBY: // stand by
			printf_P(PSTR("Standby mode\n"));
			//go(-0.5, 1.0);
			break;

		case MODE_SHORT_TRIP: // indoor quick trip contest
			printf_P(PSTR("Quick Trip\n"));
			go_to(2, 0, 2);
			go_to(6, 0, 5);
			go_to(216.0, 0, 10.0); // 18 ft there
			printf_P(PSTR("There... "));
			go_to(215, 0, 1);
			go_to(214, 0, 2);
			go_to(200, 0, 5);
			go_to(0.0, 0.0, 10.0); // & back
			printf_P(PSTR("& back\n"));
			go(0.0, 0.0); // stop
			chirp();
			printf_P(PSTR("Quick Trip complete\n"));
			break;

		case MODE_SLALOM: { // indoor slalom contest - fastest time

			/*double points[][2] = { // points for slalom with 1ft clearance
				{ -1, 1 }, // start
				{ 1, 1 }, // round first cone
				{ 7, -1 }, // cross over in front of cone #2
				{ 9, -1 }, // along cone #2
				{ 15, 1 }, // cross over in front of cone #3
				{ 16, 1 }, // along side of cone #3
				{ 17, 0 }, // far side of cone #3
				{ 16, -1 }, // along side of cone #3
				{ 15, -1 }, // ready to cross over
				{ 9, 1 }, // cross over in front of cone #2
				{ 7, 1 }, // along side cone #2
				{ 1, -1 }, // cross over in front of cone #1
				{ -1, -1 } // behind cone #1
			};*/

			/*double points[][2] = { // points for slalom with 2ft clearance
				{ -2, 2 }, // start
				{ 2, 2 }, // round first cone
				{ 6, -2 }, // cross over in front of cone #2
				{ 10, -2 }, // along cone #2
				{ 14, 2 }, // cross over in front of cone #3
				{ 16, 2 }, // along side of cone #3
				{ 18, 0 }, // far side of cone #3
				{ 16, -2 }, // along side of cone #3
				{ 14, -2 }, // ready to cross over
				{ 10, 2 }, // cross over in front of cone #2
				{ 6, 2 }, // along side cone #2
				{ 2, -2 }, // cross over in front of cone #1
				{ -2, -2 } // behind cone #1
			};*/

			printf_P(PSTR("Slalom\n"));
			x = -12.0; y = 12.0; // beginning position
			//for(i = 0; i < 13; i++) {
			//	printf_P(PSTR("\nSlalom: point #%i (% 7.3f,% 7.3f)..."), i, points[i][0] * 12, points[i][1] * 12);
			//	//go_to(points[i][0] * 12.0, points[i][1] * 12.0, 10.0);
			//}

			//go_to(-12, 12, 10);
			go_to(1, 12, 1);
			go_to(2, 12, 2);
			go_to(12, 12, 10);
			go_to(84, -12, 10);
			go_to(108, -12, 10);
			go_to(180, 12, 10);
			go_to(192, 12, 10);
			go_to(204, 0, 10);
			go_to(192, -12, 10);
			go_to(180, -12, 10);
			go_to(108, 12, 10);
			go_to(84, 12, 10);
			go_to(12, -12, 10);
			go_to(-12, -12, 10);
			go(0.0, 0.0); // stop
			chirp(); // done
			printf_P(PSTR("Slalom complete\n"));
			break; }

		case MODE_SLALOM8: // indoor slalom contest - most graceful figure 8
			break;

		case MODE_LONG_TRIP: // outdoor long trip
			printf_P(PSTR("Long Haul\n"));
			go_to(1200.0, 0.0, 10.0); // there
			go_to(0.0, 0.0, 10.0); // & back
			go(0.0, 0.0); // stop
			chirp();
			break;

		case MODE_LONG_RH_SQUARE: // right hand UMBmark 100 feet
			printf_P(PSTR("RH UMBmark\n"));
			go_to(1200.0, 0.0, 10.0);
			go_to(1200.0, -1200.0, 10.0);
			go_to(0.0, -1200.0, 10.0);
			go_to(0.0, 0.0, 10.0);
			go(0.0, 0.0); // stop
			chirp();
			break;

		case MODE_LONG_LH_SQUARE: // left hand UMBmark 100 feet
			printf_P(PSTR("LH UMBmark\n"));
			go_to(1200.0, 0.0, 10.0);
			go_to(1200.0, 1200.0, 10.0);
			go_to(0.0, 1200.0, 10.0);
			go_to(0.0, 0.0, 10.0);
			go(0.0, 0.0); // stop
			chirp();
			break;

		case MODE_DEBUG: // debug
			//printf_P(PSTR("There & back 1 foot\n"));
			//while(1) {
			//	printf_P(PSTR("There:"));
			//	//go_to(12.0, 0.0, 1.0); // there
			//	//go_to(12.0, 0.0, 2.5); // there
			//	go_to(12.0, 0.0, 5.0); // there
			//	printf_P(PSTR(" X% 7.3f Y% 7.3f A% 7.3f L% 7.3f\n"), x, y, target_angle(12.0, 0.0), target_distance(12.0, 0.0));
			//	go(0.0, 0.0); // stop
			//	delay_seconds(1);
			//	printf_P(PSTR("&Back:"));
			//	//go_to(0.0, 0.0, 1.0); // & back
			//	//go_to(0.0, 0.0, 2.5); // & back
			//	go_to(0.0, 0.0, 5.0); // & back
			//	printf_P(PSTR(" X% 7.3f Y% 7.3f A% 7.3f L% 7.3f\n"), x, y, target_angle(0.0, 0.0), target_distance(0.0, 0.0));
			//	go(0.0, 0.0); // stop
			//	delay_seconds(1);
			//}
			go(1.0, 0.0);
			break;

		default: // ??? unknown/unexpected mode
			break;
	}

	while(1); // *** debug *** end of operational mode

	// *** debug *** test run of motors + odometry
	//user_led_on();
	//chirp();
	//user_led_off();

	// *** debug *** test heading function
	//go(0.0, 0.0); // halt
	//go(0.0, 1.0); // ahead slow
	//go(0.0, 2.5); // ahead faster
	//go(0.0, 5.0); // ahead half
	//go(0.0, 10.0); // ahead full
	//go(0.0, -1.0); // back slow
	//go(0.0, -2.5); // back faster
	//go(0.0, -5.0); // back half
	//go(0.0, -10.0); // back full

	//go(1.0, 0.0); // slow left spin
	//go(2.5, 0.0); // faster left spin
	//go(5.0, 0.0); // half left spin
	//go(WHEELBASE, 0.0); // left 2 rad/sec
	//go(10.0, 0.0); // full left spin
	//go(20, 0.0); // max left spin:  4.217 rad/sec on fresh battery

	//go(1.0, 1.0); // slow left arc; radius=wheelbase; i.e., left wheel pivot, aka forward left pivot
	//go(2.5, 2.5); // faster left arc, left wheel pivot
	//go(5.0, 5.0); // left arc half; left wheel pivot, right wheel max
	//go(10.0, 10.0); // left arc full;  tries but can't reach desired speed

	//go(1.0, 2.0); // slow left arc; radius = 1.5 * wheelbase
	//go(1.0, 3.0); // slow left arc; radius = 2 * wheelbase
	//go(1.0, 4.0); // slow arc left; radius = 2.5 * wheelbase

	//go(1.0, -1.0); // reverse left pivot; pivot on right wheel
	//go(1.0, -2.0); // slow reverse left arc;  radius = 1.5 * wheelbase, pivot point 1.0 wheelbase to right of center
	//go(1.0, -3.0); // slow reverse left arc;  radius = 2.0 * wheelbase

	//go(-1.0, 0.0); // slow right spin
	//go(-2.5, 0.0); // faster right spin
	//go(-5.0, 0.0); // right spin half
	//go(-10.0, 0.0); // full right spin
	//go(-WHEELBASE * 2, 0.0); // right spin @ -4rad/sec
	//go(-20, 0.0); // max right spin:  -4.217 rad/sec on freshly charged battery

	//go(-1.0, 1.0); // slow right arc;  right wheel pivot
	//go(-2.5, 2.5); // faster right arc
	//go(-5.0, 5.0); // right pivot half; left wheel=0 right wheel=max
	//go(-10.0, 10.0); // max right pivot;  does not reach set point

	//while(1); // *** debug *** end of heading function tests

	// *** debug *** test navigation routines
	//go_to(120.0, 0.0, 5.0); // ten feet straight ahead @ half speed
	//go_to(120.0, 0.0, 10.0); // ten feet straight ahead @ full speed - there is a problem with missing the goal at full speed
	//go_to(120.0, 12.0, 5.0); // ten feet over and one foot up @ half speed
	//go_to(120.0, 120.0, 5.0); // over ten and up ten, diagonally @ half speed
	//go_to(0.0, 120.0, 5.0); // turn left & go ten feet
	//go_to(-120.0, 120.0, 5.0); // left & up diagonally
	//go_to(-120, 0, 5); // back ten feet
	//go_to(0, -120, 5); // right & down ten feet
	//go_to(120, -120, 5); // right & diagonally across
	//while(1); // *** debug *** end navigation tests

	// *** debug *** 1 foot UMBmark
	//while(1) {
	//	go_to(12.0, 0.0, 2.5);
	//	go_to(12.0, 12.0, 2.5);
	//	go_to(0.0, 12.0, 2.5);
	//	go_to(0.0, 0.0, 2.5);
	//	go(0.0, 0.0); // & stop
	//	delay(5 * TPS); // pause 5 seconds
	//}

	// *** debug *** 4 foot UMBmark
	//while(1) {
	//	go_to(48.0, 0.0, 6.0);
	//	go_to(48.0, 48.0, 6.0);
	//	go_to(0.0, 48.0, 6.0);
	//	go_to(0.0, 0.0, 6.0);
	//	go(0.0, 0.0); // & stop
	//	//delay(5 * TPS); // pause 5 seconds
	//}

	// *** debug *** 10 foot left-hand UMBmark
	//while(1) {
	//	go_to(120.0, 0.0, 5.0);
	//	go_to(120.0, 120.0, 5.0);
	//	go_to(0.0, 120.0, 5.0);
	//	go_to(0.0, 0.0, 5.0);
	//	go(0.0, 0.0); // & stop
	//	//delay(5 * TPS); // pause 5 seconds
	//}

	// *** debug *** ten foot quick trip
	//while(1) {
	//	go_to(120, 0, 5);
	//	go_to(0, 0, 5);
	//}

	while(1) {

		c = getchar(); // get command

		switch(c) {

			// --- computer interaction ---

			case 'R': // real time clock time stamp
				printf_P(PSTR("RTC %i:%02i:%02i.%X"), hours, minutes, seconds, ticks);
				//usart_puthex(hours);
				//usart_puthex(minutes);
				//usart_puthex(seconds);
				//usart_puthex(ticks);
				break;

			case 'O': // odometry report
				printf_P(PSTR("X %f Y %f theta %f "), x, y, theta);
				break;

			case 'W': // wheel report
				printf_P(PSTR("W R %i %i L %i %i "), right_wheel.odometer, right_wheel.velocity, left_wheel.odometer, left_wheel.velocity);
				break;

			case 'M': // motion report
				//printf_P(PSTR("M R %f %f L %f %f A %f %f L %f %f "), right.odometer, right.velocity, left.odometer, left.velocity, angular.odometer, angular.velocity, linear.odometer, linear.velocity);
				//printf_P(PSTR("M A %f L %f "), angular.velocity, linear.velocity);
				i = angular.velocity * 1000;
				j = linear.velocity * 1000;
				printf_P(PSTR("M A %i L %i "), i, j);
				break;

			case '0': // STOP
				go(0.0, 0.0);
				break;

			case '1': // GO
				go(0.0, 1.0);
				break;

			case '2': go(0.0, 2.0); break;
			case '3': go(0.0, 3.0); break;
			case '4': go(0.0, 4.0); break;
			case '5': go(0.0, 5.0); break;
			case '6': go(0.0, 6.0); break;
			case '7': go(0.0, 7.0); break;
			case '8': go(0.0, 8.0); break;
			case '9': go(0.0, 9.0); break;
			case '-': go(0.0, 10.0); break;

			// --- human interaction ---

			case ' ': // space
				printf_P(PSTR(" "));
				break;

			case '\r': // carriage return
				printf_P(PSTR("\n")); // new line
				break;

			case 'r': // real time clock timestamp
				//rtc_stamp();
				break;

			case 'o': // odometry report
				//rtc_stamp(); // real time clock timestamp
				printf_P(PSTR("x%f y%f t%f\n"), x, y, theta);
				break;

			//case 's': // set point report
			//	rtc_stamp(); // real time clock timestamp
			//	printf_P(PSTR("r%f l%f b%f a%f\n"), right_wheel.velocity_setpoint, left_wheel.velocity_setpoint, linear.velocity_setpoint, angular.velocity_setpoint);
			//	break;

			//case 'v': // velocity report
			//	rtc_stamp(); // real time clock timestamp
			//	printf_P(PSTR("r%f l%f b%f a%f\n"), right_wheel.velocity, left_wheel.velocity, body.velocity, angular_velocity);
			//	break;

			case 'z': // zero - stop motors
				go(0.0, 0.0);
				break;

			case 'f': // forward, slow
				go(0.0, 1.0);
				break;

			case 'F': // forward, half
				go(0.0, 5.0);
				break;

			case 'a': // go to point A (0,0)
				go_to(0.0, 0.0, 1.0); // slowly now
				go(0,0); // & stop
				break;

			case 'b': // go to point B (12,0)
				go_to(12.0, 0.0, 1.0); // slowly now
				go(0,0); // & stop
				break;

			case 'u': // little UMBmark
				go_to(12.0, 0.0, 2.5);
				go_to(12.0, 12.0, 2.5);
				go_to(0.0, 12.0, 2.5);
				go_to(0.0, 0.0, 2.5);
				go(0.0, 0.0); // & stop
				break;

//			case 'm': // toggle motor control flag
//				if(motor_control_flag) {
//					motor_control_flag = 0;
//				} else {
//					motor_control_flag = 1;
//				}
//				break;

			default: // ??? unknown/unexpected command
				//printf_P(PSTR("\a*** unknown command [%c] ***\n"), c); // note:  the '/a' gets kind of annoying after a while
				break;
		}
	}
}

// external interrupt 0 handler:  right wheel encoder triggers integer odometry update

ISR(INT0_vect) {

	user_led_on(); // *** debug ***

	if(bit_is_clear(PIND, PD2) ^ bit_is_clear(PINB, PB0)) {
		right_wheel.odometer--; // counter-clockwise motion of the right wheel
	} else {
		right_wheel.odometer++; // clockwise motion of the right wheel
	}

	user_led_off(); // *** debug ***
}

// external interrupt 1 handler:  left wheel encoder triggers integer odometry update

ISR(INT1_vect) {

	user_led_on(); // *** debug ***

	if(bit_is_clear(PIND, PD3) ^ bit_is_clear(PINB, PB1)) {
		left_wheel.odometer--; // counter-clockwise motion of the left wheel
	} else {
		left_wheel.odometer++; // clockwise motion of the left wheel
	}

	user_led_off(); // *** debug ***
}

// pin change interrupt group 1 handler:  motor encoder phase B inputs

ISR(PCINT1_vect) {

	static unsigned char current_state, previous_state;
	unsigned char delta;

	current_state = PINB & 0x03; // capture new state
	delta = current_state ^ previous_state; // the difference between then & now

	// right side change
	if(delta & 0x01) {
		if(bit_is_clear(PIND, PD2) ^ bit_is_clear(PINB, PB0)) {
			right_wheel.odometer++; // clockwise motion of the right wheel
		} else {
			right_wheel.odometer--; // counter-clockwise motion of the right wheel
		}
	}

	// left side change
	if(delta & 0x02) {
		if(bit_is_clear(PIND, PD3) ^ bit_is_clear(PINB, PB1)) {
			left_wheel.odometer++; // clockwise motion of the left wheel
		} else {
			left_wheel.odometer--; // counter-clockwise motion of the left wheel
		}
	}

	previous_state = current_state; // remember this for next time
}

// timer/counter2 overflow interrupt handler - RTC heartbeat

ISR(TIMER2_OVF_vect) {

	int right_wheel_odometer_capture, left_wheel_odometer_capture;
	unsigned char motion_flag = 0;
	double distance;
	double angular_odometer_capture;

	// *** first things ***

	// capture current values for odometer readings
	// wheel odometer values are updated in an asynchronous process
	right_wheel_odometer_capture = right_wheel.odometer; // in encoder ticks per sample period
	left_wheel_odometer_capture = left_wheel.odometer; // in encoder ticks per sample period

	sei(); // re-enable interrupts for the duration of this handler

	user_led_on(); // *** debug ***  this tells the world how long this takes

	// update real time clock
	ticks++; // count ticks
	if(ticks == TICKS_PER_SECOND) {
		ticks = 0; // reset ticks
		seconds++; // count seconds
		if(seconds == 60) {
			seconds = 0; // reset seconds
			minutes++; // count minutes
			if(minutes == 60) {
				minutes = 0; // reset minutes
				hours++; // count hours
			}
		}
	}

	// *** subsequent things ***

	// calculate velocities & travel; update odometers
	if(right_wheel_odometer_capture != right_wheel.previous_odometer) {
		right_wheel.velocity = right_wheel_odometer_capture - right_wheel.previous_odometer; // in encoder ticks per sample period
		right.velocity = right_wheel.velocity * RIGHT_WHEEL_TICK; // in inches per sample period
		right.odometer += right.velocity; // update motion odometer reading
		right_wheel.previous_odometer = right_wheel_odometer_capture; // remember this
		motion_flag = 1; // right-side motion detected
	} else {
		right_wheel.velocity = 0;
		right.velocity = 0.0;
	}
	if(left_wheel_odometer_capture != left_wheel.previous_odometer) {
		left_wheel.velocity = left_wheel_odometer_capture - left_wheel.previous_odometer; // in encoder ticks
		left.velocity = left_wheel.velocity * LEFT_WHEEL_TICK; // in inches per sample period
		left.odometer += left.velocity; // update motion odometer reading
		left_wheel.previous_odometer = left_wheel_odometer_capture; // remember this
		motion_flag = 1; // left-side motion detected
	} else {
		left_wheel.velocity = 0;
		left.velocity = 0.0;
	}

	if(motion_flag) {

		//linear.velocity = (right.velocity - left.velocity) / 2; // average in inches per second
		linear.velocity = (right.velocity - left.velocity) / 2.0; // average in inches per sample period

		// update theta with Kuhnle optimization
		theta = (right.odometer + left.odometer) / WHEELBASE; // in radians

		// calculate distance in inches travelled during last sample period
		angular_odometer_capture = angular.odometer;
		angular.odometer += (right.velocity + left.velocity); // how far 'round we've gone
		angular.previous_odometer = angular_odometer_capture; // remember this
		distance = (right.velocity - left.velocity) / 2.0;

		linear.odometer += distance; // accumulate distance travelled

		// update x & y positions
		x += cos(theta) * distance;
		y += sin(theta) * distance;

		// calculate angular velocity
		angular.velocity = (right.velocity + left.velocity) / 2.0; // average in inches per sample period at the wheel radius
	} else {
		angular.velocity = 0.0;
		linear.velocity = 0.0;
	}

	motor_control(); // adjust motor power to achieve desired velocity

	// *** last things ***

	if(foreground_downcounter) foreground_downcounter--; // maintain downcounter (maybe)

	// performance metric
	//rtc_overhead = TCNT2; // read this as very last thing, just before exiting service routine

	user_led_off(); // *** debug ***
}

// USART0 receive complete interrupt handler

ISR(USART0_RX_vect) {

	usart_rxd_buffer[usart_rxd_in++] = UDR0;
}

// USART0 UDRE interrupt handler

ISR(USART0_UDRE_vect) {

	if(usart_txd_in != usart_txd_out) {
		UDR0 = usart_txd_buffer[usart_txd_out];
		usart_txd_out++;
	} else {
		cbi(UCSR0B, UDRIE0); // clear the interrupt
	}
}

// default interrupt vector

ISR(__vector_default) __attribute ((naked, noreturn));
ISR(__vector_default) {

	error(ERROR_EXCEPTION); // this incident will be reported
}

// [end-of-file]