From: http://garya.org.uk/software/embedded-c/antweight-speed-controller

/*
 * SpeedController.c
 *
 *  Created on: 19 Aug 2013
 *      Author: Gary Aylward
 *   Copyright: Gary Aylward, 2013
 *     Website: http://garya.org.uk
 *     License: Creative Commons BY-SA 3.0
 *
 *     http://creativecommons.org/licenses/by-sa/3.0/
 *
 *     Pololu Orangutan library (c) Pololu Corporation, licensed under CC-BY-SA 3.0
 *
 *     1.0 19/08/2013 Original release
 *     1.1 25/08/2013 Fixed battery monitor filter bug
 */

/*
 * Antweight R/C speed controller using Pololu Baby Orangutan board
 *
 * Set linker to use pololu_atmega328p library.
 *
 * Module pinouts are defined below, the settings used allow other pins to be used for
 * further R/C channels, extra PWM outputs, UART etc.
 * The battery voltage is fed to the ADC via a potential divider so that the ADC sees
 * the voltage of a single cell. If the under-voltage lockout is not required, connect
 * the ADC input to Vcc or redefine dCELL_MV below.
 *
 * Mixing is set up for an R/C transmitter with the following characteristics:
 * Channel 1 = Left/Right, Right = longer pulses
 * Channel 2 = Forwards/Reverse, Reverse = longer pulses
 *
 * Motors are connected with +ve terminal connected to M1B / M2B, -ve to M1A / M2A
 * Motors turn clockwise (when viewed from shaft end) with positive voltage
 */

#include <pololu/orangutan.h>

#ifndef F_CPU
#define F_CPU	20000000	// Baby Orangutan runs at 20MHz
#endif

#define dCHANNEL1	IO_C0	// R/C Ch1 input pin
#define dCHANNEL2	IO_C1	// R/C Ch2 input pin
#define	dLED		IO_D1	// LED output pin
#define dADC_CHANNEL	6	// ADC battery monitor channel

#define dZERO		1500	// Nominal pulse width
#define dMAX		2000	// Max pulse width
#define dMIN		1000	// Min pulse width
#define dMAX_PULSE	2200	// Pulse width for error detection
#define dMIN_PULSE	800		// Pulse width for error detection
#define dTIMEOUT	30000	// 30ms timeout on R/C pulses
#define dMAX_SPEED	250		// Max speed setting
#define dMIN_SPEED	-250	// Min speed setting

#define dFILTER_SIZE	600	// Length of under-voltage detection filter, 6 steps per ms
#define dCELL_MV	2750	// Minimum cell voltage in mV


int main()
{
	unsigned int vVoltage = 0;			// Battery cell voltage measurement
	unsigned char vUnderVolt = 0;		// Under-voltage error flag
	unsigned long vFilter = 0;			// Under-voltage filter counter
	struct PulseInputStruct vPulseInfo;	// Orangutan library pulse structure
	unsigned long vPulseLength = 0;		// Pulse length in ticks
	unsigned char vState = 0;			// Current pulse state
	unsigned long vPulseCh1 = dZERO;	// Channel 1 pulse in ms
	unsigned long vPulseCh2 = dZERO;	// Channel 2 pulse in ms
	int vLeftSpeed;						// Left motor speed
	int vRightSpeed;					// Right motor speed
	unsigned char vError = 1;			// Error flag

	// Configure pins as pulse input channels
	pulse_in_start((unsigned char[]) {dCHANNEL1, dCHANNEL2}, 2);
	// Enable internal pull-ups on R/C input channels
	set_digital_input(dCHANNEL1, PULL_UP_ENABLED);
	set_digital_input(dCHANNEL2, PULL_UP_ENABLED);
	// Initialise LED to off
	set_digital_output(dLED, 0);
	//Set ADC to 10-bit mode
	set_analog_mode(MODE_10_BIT);

	while(1)
	{
		if (analog_is_converting() == 0)
		{
			// Start a new conversion if one isn't already running
			start_analog_conversion(dADC_CHANNEL);
		}
		// Get time since last edge on R/C channel 1
		get_current_pulse_state(0, &vPulseLength, &vState);
		if (pulse_to_microseconds(vPulseLength) >= dTIMEOUT)
		{
			vError = 1; // R/C timeout - signal lost
		}
		else
		{
			get_pulse_info(0, &vPulseInfo); // get pulse info for R/C channel 1
			if (vPulseInfo.newPulse & HIGH_PULSE)
			{
				vPulseCh1 = pulse_to_microseconds(vPulseInfo.lastHighPulse);
				if ((vPulseCh1 > dMAX_PULSE) || (vPulseCh1 < dMIN_PULSE))
				{
					vError = 1;	// Pulse is too long or too short
				}
				else
				{
					vError = 0;
					if (vPulseCh1 > dMAX)
					{
						vPulseCh1 = dMAX;
					}
					if (vPulseCh1 < dMIN)
					{
						vPulseCh1 = dMIN;
					}
				}
			}
		}
		// Get time since last edge on R/C channel 2
		get_current_pulse_state(1, &vPulseLength, &vState);
		if (pulse_to_microseconds(vPulseLength) >= dTIMEOUT)
		{
			vError = 1;	// R/C timeout - signal lost
		}
		else
		{
			get_pulse_info(1, &vPulseInfo); // get pulse info for R/C channel 2
			if (vPulseInfo.newPulse & HIGH_PULSE)
			{
				vPulseCh2 = pulse_to_microseconds(vPulseInfo.lastHighPulse);
				if ((vPulseCh2 > dMAX_PULSE) || (vPulseCh2 < dMIN_PULSE))
				{
					vError = 1;	// Pulse is too long or too short
				}
				else
				{
					vError = 0;
					if (vPulseCh2 > dMAX)
					{
						vPulseCh2 = dMAX;
					}
					if (vPulseCh2 < dMIN)
					{
						vPulseCh2 = dMIN;
					}
				}
			}
		}
		if (analog_is_converting() == 0)
		{
			// If ADC conversion is finished, check the battery voltage
			vVoltage = analog_conversion_result_millivolts();
			if (vVoltage < dCELL_MV)
			{
				if (vFilter >= dFILTER_SIZE)
				{
					// Filter has timed out, leave the fault flag set
					vFilter = dFILTER_SIZE;	// Prevent filter count from overflowing
					vUnderVolt = 1;	// Set fault flag
				}
				else
				{
					vFilter++;	// Increment filter count
					vUnderVolt = 0;	// Clear fault flag
				}
			}
			else
			{
				vFilter = 0;	// Reset filter if voltage is above limit
			}
		}
		// Calculate motor speeds
		vLeftSpeed = ((vPulseCh2 - dZERO) - (vPulseCh1 - dZERO)) / 2;
		vRightSpeed = (-(vPulseCh2 - dZERO) - (vPulseCh1 - dZERO)) / 2;
		// Limit maximum speeds
		if (vLeftSpeed > dMAX_SPEED)
		{
			vLeftSpeed = dMAX_SPEED;
		}
		else if (vLeftSpeed < dMIN_SPEED)
		{
			vLeftSpeed = dMIN_SPEED;
		}
		if (vRightSpeed > dMAX_SPEED)
		{
			vRightSpeed = dMAX_SPEED;
		}
		else if (vRightSpeed < dMIN_SPEED)
		{
			vRightSpeed = dMIN_SPEED;
		}
		if ((vUnderVolt == 1) || (vError == 1))
		{
			// Stop motors and light LED if a fault exists
			set_m1_speed(0);
			set_m2_speed(0);
			set_digital_output(dLED, 1);
		}
		else
		{
			// Set motor speeds and turn off LED
			set_m1_speed(vLeftSpeed);
			set_m2_speed(vRightSpeed);
			set_digital_output(dLED, 0);
		}
	}
}