//	This program is fully and completely released to the Public Domain by Terry Fritz 2011 for the good of the airsoft community.

/*	SW-Lion Platform Factory Control Program
	WWW.EXTREME-FIRE.COM    WWW.UNCONVENTIONAL-AIRSOFT.COM    WWW-3ONEAIRSOFT.COM
	Target controller = ATmega1284P-20AU microcontroller from ATMEL.
	Clock speed = 20MHZ with Ceramic resonator.
	Devolopment Platform = AVR Studio 4 Version 4.19 build 623.
	C Compiler = AVR ToolChain 3.3.0.710
	GCC Optimization -Os
	Programmer = AVRISP mkII
	Hardware Schematic = SW-Lion-2N
	Hardware Layout = SW-Lion-2N
	Software Architecture = Majel Platform
	Internet Forum = http://forums.airsoftmechanics.com
	ME = sales@extreme-fire.com  :D

*/

/*
	Chip Setup

	ISP programming frequency is 125kHz.  Must be greater than 5kHz.
	BOD (Brown out detection) is 4.3V in fuses.
	Turn off JTAG and other unused interfaces in fuses.
	Turn off CLK/8
	Full swing ceramic 20MHz resonator in fuses.  (Be careful!!  It will lock the chip if you select the wrong one!)
		If you mess up and lock the chip due to wrong oscillatior.  Force 8MHz clock into pin XTAL1 to force chip on
			so you can select the right Oscillator type.
		An 8MHz ATTINY85 with CLKOUT on PB1 (in fuses) should do it.  There is a special tap under the board for this.
		Guess how I know all about this... - Terry
*/


/*
	History
*/


/*
	Known issues
*/


/*
	Future Changes
*/


//	Include Libraries
	#include <avr/io.h>	//For all the AVR IO standard data types
	#include <util/delay.h>  //For _delay_ms(100) and _delay_us(100)
	#include <avr/interrupt.h>
	#include <avr/sleep.h>
	#include <stdio.h>  //  For LCD output
	#include <avr/pgmspace.h>  //  For LCD output support


//Version Number in Year, Month, Day format
	#define MainVersionNumber 20111029

	#define F_CPU 20000000UL

	#define Aux0 0
	#define Aux1 1
	#define Aux2 2
	#define Drain 5
	#define Trigger 6
	#define Ground 10


//Function Prototypes

void OutputLCD(void);
void CheckActivePowerLimit(void);
void CheckMotorRunningCurrent(void);
void TurnMotorOn(unsigned char motorspeed);
void CheckShutdownConditions(void);
void LED(unsigned char Light);
void AUX(unsigned char port, unsigned char number);
void SetAUXPorts(void);
void Timer(unsigned long Time);
void EnergizeFETdrive(void);
void Brake(char b1);
volatile long GetVoltage(unsigned char Sensor, unsigned char ADCRange);
void EEPROM_write(unsigned int uiAddress, unsigned char ucData);
unsigned char EEPROM_read(unsigned int uiAddress);
void StartClock(void);
void MotorPWM(char PWMduty);
void LionHalt(void);
void Vibrate(unsigned long Time, unsigned long Frequency, unsigned long Volume);
void Alert(unsigned int number);
void CheckMotor(char PWMduty);
void DefaultValues(void);
void UserProgramming(void);
void VibrateSignal(int Number);
int uart_putchar(char c, FILE *stream);
void uart_contchar(char c);
char uart_getchar(void);
void USART_Init(unsigned int baud);
void StoreVersion(unsigned int number1,unsigned long value1);
unsigned long GetVersion(unsigned int number11);
void StoreHistory(unsigned int number2,unsigned char value2);
void StoreRunning(unsigned int number3, unsigned char value3);
unsigned char GetRunning(unsigned int number4);


//Global Variables
	//  For LCD display
	volatile long Data0 = 0;
	volatile long Data1 = 0;
	volatile long Data2 = 0;
	volatile long Data3 = 0;

	unsigned long CycleTime = 0;
	long TriggerVoltage = 0;

	unsigned long MotorRunTime = 0;
	unsigned long PrecockStartTime = 0;
	unsigned long LCDStartTime = 0;
	unsigned int Error = 0;
	long i = 0;
	long j = 0;
	long k = 0;

	long BatteryVoltage = 0;
	unsigned char MotorSpeedLimited = 255;
	unsigned long MotorLimitSpeed = 10000;
	unsigned long CurrentCheckIntervalTime = 0;
	unsigned long TriggerONDebounce = 0;
	unsigned long TriggerOFFDebounce = 0;
	unsigned int TriggerStatus = 0;

	unsigned long PowerLimitFactor = 100;
	unsigned long ShotTime = 0;
	unsigned long PrecockTime = 0;
	unsigned long TriggerCounter = 0;
	unsigned long BatteryVoltageHighLimit = 20000;

	volatile long Voltage = 0;

	volatile unsigned long RealClock = 0;

	int timenull = 0;

	volatile char MotorStatus = 0;
	volatile char BrakeStatus = 0;
	volatile unsigned long MotorStartTime = 0;
	volatile unsigned long MotorStopTime = 0;
	volatile unsigned long BrakeStartTime = 0;
	volatile unsigned long BrakeStopTime = 0;

	volatile long MotorCheckCurrent = 0;

	//  These are the factory default values
	unsigned char GunFunction = 1;
	unsigned char PrecockingEnable = 1;
	unsigned char ActiveBrakingEnable = 1;

	unsigned long SniperDelay = 0;
	unsigned char MotorSpeed = 255;
	unsigned long AverageCurrentLimit = 70;
	unsigned long PrecockTriggerDelay = 70;
	unsigned long BurstFactor = 100;


int main(void)
{

Timer(2E6);  //  Two second start delay


//Store Version Information
	StoreVersion(1,MainVersionNumber);

//  Increment power up counter
	i = GetVersion(0); i++; StoreVersion(0,i);

//Start Real Time Clock
	StartClock();

//Setup AUX Ports
	SetAUXPorts();
	LED(1);  //0=off, 1=green, 2=red
	AUX(0,2);  //0=low, 1=high, 2=input
	AUX(1,2);
	AUX(2,2);
	
//Energize FET Drive
	EnergizeFETdrive();

//Detect System Voltage and Test
	Brake(1);
	BatteryVoltage = GetVoltage(Drain,0);
	Brake(0);
	StoreHistory(0,BatteryVoltage / 100);  //  Store power up voltage to EEPROM
	if (BatteryVoltage < 6000) {LED(2); Alert(1); StoreHistory(2,BatteryVoltage / 100); LionHalt();}  //  Battery too low
	if (BatteryVoltage > BatteryVoltageHighLimit) {LED(2); Alert(2); StoreHistory(2,BatteryVoltage / 100); LionHalt();}  //  Battery too high
	TriggerVoltage = (GetVoltage(Trigger,0) * 5 / 4);  //  Correct for 5K protection Resistor 
	if (TriggerVoltage > 500) {LED(2); Alert(3); LionHalt();}  //  Trigger voltage too high


//Serial Port Setup (if used)
	//  See page 197 of ATmega644P data sheet
	USART_Init(129);  //  129 is the code for 9600 baud with a 20MHz clock
	FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_RW);
	stdin = stdout = &uart_stream;
	uart_contchar(22);  //  Turn on dsiplay with cursor hidden


//Read EEPROM
	
	if (GetRunning(0) != 1) {DefaultValues();}  //  If EEPROM is not programmed - fix it

	GunFunction = GetRunning(1);
	PrecockingEnable = GetRunning(2);
	ActiveBrakingEnable = GetRunning(3);
	SniperDelay = GetRunning(32);
	MotorSpeed = GetRunning(33);
	AverageCurrentLimit = GetRunning(34);
	PrecockTriggerDelay = GetRunning(35);
	BurstFactor = GetRunning(36);

//User Programming
	UserProgramming();

//  Ready
Vibrate(2000,30,25);  //Set down from 50 for Hunter
PowerLimitFactor = 80;


//########## MAIN OPERATING LOOP END ########## //

CycleLoop:

	//  Loop time control
	//  Sync cycles to Real Clock every 1mS
			while (CycleTime == RealClock) {;}  //  Hold till next clock cycle
			CycleTime = RealClock;  

	//  Find motor run time
	if (MotorStatus == 1) {MotorRunTime = RealClock - MotorStartTime;} else {MotorRunTime = MotorStopTime - MotorStartTime;}

	//  Find pre-cocking time
	if ((BrakeStatus == 0) && (MotorStatus == 0) && (TriggerCounter == 5))
		{PrecockTime = MotorRunTime - PrecockTriggerDelay; ShotTime = MotorRunTime;}


	//  Error Handling
			if (Error > 0) {LED(2);} else {LED(1);}
			if (Error > 1) {StoreHistory(1,Error); Alert(Error); Error = 0;}
				// 1 = Voltage droop
				// 2 = Voltge droop shutdown
				// 3 = Over peak current 
				// 4 = Over running current

	//  Check for excessive battery voltage loss and running current
//			CheckActivePowerLimit();
			CheckMotorRunningCurrent();
			CheckShutdownConditions();

	//  Check for Trigger
			TriggerVoltage = (GetVoltage(Trigger,0) * 5 / 4);  //  Correct for 5K protection Resistor 
				if (TriggerVoltage >= 6500) {TriggerONDebounce++;} else {TriggerONDebounce = 0;}
				if (TriggerVoltage <= 4500) {TriggerOFFDebounce++;} else {TriggerOFFDebounce = 0;}
				//  Turn motor on
					if ((TriggerONDebounce >= 3)  && (MotorStatus == 0)  && (Error == 0)) 
					{TurnMotorOn(MotorSpeed); TriggerCounter++; TriggerStatus = 1;}
				//  Power Start
					if ((MotorStatus == 1) && (RealClock <= MotorStartTime + 10)) {MotorPWM(255);}
					if ((MotorStatus == 1) && (RealClock > MotorStartTime + 10)) {MotorPWM(MotorSpeed);}
				//  Turn motor off
					if ((TriggerOFFDebounce >= 3)  && (MotorStatus == 1) && (TriggerStatus == 1) && (RealClock >= (MotorStartTime + 20)) && (ShotTime < (MotorRunTime + PrecockTime + 10)))
					{PrecockStartTime = RealClock; TriggerStatus = 0;}

					if ((RealClock >= PrecockStartTime + PrecockTime) && (MotorStatus == 1) && (TriggerStatus == 0)) {MotorPWM(0); Brake(1);}  //  Stop Motor
					if ((RealClock >= BrakeStartTime + 100) && (BrakeStatus == 1) && (TriggerStatus == 0)) {Brake(0);}  //  Turn off brake


// OutputLCD();


goto CycleLoop;

//########## MAIN OPERATING LOOP END ########## //

return(0);
} //  End of Main


void OutputLCD(void)
	{
	if (RealClock < (LCDStartTime + 1000)) {return;}
	LCDStartTime = RealClock;
	uart_contchar(22);  //  Turn on dsiplay with cursor hidden
	uart_contchar(128);  //  Set cursor to 0,0
	printf_P(PSTR("Data0=%ld     \r"),Data0);
	printf_P(PSTR("Data1=%ld     \r"),Data1);
	printf_P(PSTR("Data2=%ld     \r"),Data2);
	printf_P(PSTR("Data3=%ld     \r"),Data3);
	}


void CheckActivePowerLimit(void)
	{
	if ((TriggerVoltage <= 6000) && (MotorStatus == 1) && (TriggerStatus == 1) && (MotorRunTime > 25))
	//  Motor is running but battery supply voltge has drooped too long
		//  Reduce motor 25%
		{
		Timer(100);
		if (TriggerVoltage > 6000) {return;}

		MotorLimitSpeed = (MotorLimitSpeed * PowerLimitFactor / 100);
		Error = 1;  //  Voltage Droop
		}
		else
		//  If it is better, speed up motor to normal
		{
		MotorLimitSpeed = (MotorLimitSpeed * 100 / PowerLimitFactor);
		Error = 0;
		}
	//  Check a lot of limits
	if (MotorLimitSpeed > 100000) {MotorLimitSpeed = 100000;};
	if (MotorLimitSpeed < 100) {MotorLimitSpeed = 100;};
	MotorSpeedLimited = (255 * MotorLimitSpeed / 100000);
	if (MotorSpeedLimited < 2) {MotorSpeedLimited = 2;}
	if (MotorSpeedLimited > MotorSpeed) {MotorSpeedLimited = MotorSpeed;}
	if (MotorStatus ==1) {MotorPWM(MotorSpeedLimited);}   
	//  If it droops too much, shut it down
	if ((MotorStatus == 1) && (MotorSpeedLimited <= 5)) {MotorPWM(0); Error = 2;}
	}


void CheckMotorRunningCurrent(void)
	{
	if ((MotorStatus == 1) && (RealClock - CurrentCheckIntervalTime) >= 200)  //  Check every 200mS
		{
		CurrentCheckIntervalTime = CurrentCheckIntervalTime + 200;
		CheckMotor(MotorSpeed);
		//  Check for high running current
		if (MotorCheckCurrent > AverageCurrentLimit) {MotorPWM(0); Error = 4; StoreHistory(4,MotorCheckCurrent / 10);}
		}
	}


void TurnMotorOn(unsigned char motorspeed)
		{
		CurrentCheckIntervalTime = RealClock;
		CheckMotor(MotorSpeed);
		if (MotorCheckCurrent >= 905) {MotorPWM(0); Error = 3; StoreHistory(3,MotorCheckCurrent / 10);} //  Over peak current
			else
			{  //  Turn motor on
			MotorPWM(MotorSpeed);  //  Normal speed
			} 
		}


void CheckShutdownConditions(void)
		{
		//  Shut down after 10 hours of non use.
			if ((RealClock - MotorStartTime) >= 36000000) {LionHalt();}
		//  Shut down if battery can not maintain 7 volts after 60 seconds on non use
			if ((MotorStatus == 0) && ((RealClock - MotorStartTime) >= 60000) && (GetVoltage(Drain,0) <= 7000)) {LionHalt();}
		//  Shut down if motor runs more than 20 seconds
			if ((MotorStatus == 1) && ((RealClock - MotorStartTime) >= 20000)) {LionHalt();}
		}


//LED - Controls LED Light  //0=off, 1=green, 2=red
//Note - FET drive is on PB3 and PB4 so be careful!!!

void LED(unsigned char Light)
{
if (Light == 0) {PORTB = (PORTB & 0b11111001);}  //Off
if (Light == 1) {PORTB = (PORTB & 0b11111011); PORTB = (PORTB | 0b00000010);}  //Green
if (Light == 2) {PORTB = (PORTB & 0b11111101); PORTB = (PORTB | 0b00000100);}  //Red
}


//AUX - Controls AUX Ports  //0=low, 1=high, 2=input
void AUX(unsigned char port, unsigned char number)
{
// PORTB5, PORTB6, PORTB7 are used only for programming
// Set AUX for ouput if needed
if ((port == 0) && ((number == 0) || (number == 1)) && (DDRA && 0b00010000 == 0)) {PORTA = (PORTA & 0b11101111); DDRA = (DDRA | 0b00010000);} 
if ((port == 1) && ((number == 0) || (number == 1)) && (DDRA && 0b00100000 == 0)) {PORTA = (PORTA & 0b11011111); DDRA = (DDRA | 0b00100000);} 
if ((port == 2) && ((number == 0) || (number == 1)) && (DDRA && 0b01000000 == 0)) {PORTA = (PORTA & 0b10111111); DDRA = (DDRA | 0b01000000);}
//Set AUX = 0
if ((port == 0) && (number == 0)) {PORTA = (PORTA & 0b11101111);} 
if ((port == 1) && (number == 0)) {PORTA = (PORTA & 0b11011111);} 
if ((port == 2) && (number == 0)) {PORTA = (PORTA & 0b10111111);} 
//Set AUX = 1
if ((port == 0) && (number == 1)) {PORTA = (PORTA | 0b00010000);} 
if ((port == 1) && (number == 1)) {PORTA = (PORTA | 0b00100000);} 
if ((port == 2) && (number == 1)) {PORTA = (PORTA | 0b01000000);} 
//Set AUX for input
if ((port == 0) && (number == 2)) {PORTA = (PORTA & 0b11101111); DDRA = (DDRA & 0b11101111);}
if ((port == 1) && (number == 2)) {PORTA = (PORTA & 0b11011111); DDRA = (DDRA & 0b11011111);}
if ((port == 2) && (number == 2)) {PORTA = (PORTA & 0b10111111); DDRA = (DDRA & 0b10111111);}
}


//Setup AUX ports and LED
void SetAUXPorts(void)
{
DDRA = 0;
DDRB = 0;
PORTA = 0;  //No Pullups
PORTB = 0;  //No Pullups
DDRA = 0b01110000;  //Enable AUX ports for output and other A ports for input or High-Z
PORTA = PORTA & 0b10001111;  //AUX outputs = 0
DDRB = 0b00000110;  //Enable LED but not FET drive
PORTB = PORTB & 0b11111001;  //LED outputs =0
}


//Timer - General Purpose Timer in uS = 1/1000000 Second
//You will over run the varibles greater than 2000 seconds!
void Timer(unsigned long Time)
{
unsigned long counter1 = 0;
Time = Time * 120 / 59;
for (counter1 = 0; counter1 <= Time; counter1++) {timenull = 0;}
}


//Enable the Drive FETs - This is a 2N version!
void EnergizeFETdrive(void)
{
//Note that the FET RC time constant is 5uS.
DDRB = DDRB & 0b11100111;  //Be sure FET drive is off
Timer(10);  //Give FETs 10uS to react
PORTB = PORTB & 0b11100111;  //Be sure drive pullupa are off
PORTB = PORTB | 0b00000000;
DDRB = DDRB | 0b00010000;  //Enable braking FET
PORTB = PORTB | 0b00000000;  //Turn braking FET off (0=0ff 1=On)
Timer(10);  //Give braking FET 10uS to react
DDRB = DDRB | 0b00001000;  //Enable drive FET
PORTB = PORTB & 0b11100111;  //Turn drive FET off
Timer(10);  //Give drive FET 10uS to react
}


//Brake drive control - This is a 2N version!

void Brake(char b1)
{
	if ((b1 != 0) && (BrakeStatus == 0)) {BrakeStartTime = RealClock;}
	if ((b1 == 0) && (BrakeStatus == 1)) {BrakeStopTime = RealClock;}
if (b1 == 0)
	{
	PORTB = PORTB & 0b11101111;  //Turn off brake
	BrakeStatus = 0; BrakeStopTime = RealClock;
	}
if (b1 == 1)
	{
	PORTB = PORTB & 0b11110111;  //Turn off motor
	MotorPWM(0);
	Timer(50);
	if (ActiveBrakingEnable == 1) {PORTB = PORTB | 0b00010000;}  //Turn on brake
	BrakeStatus = 1; BrakeStartTime = RealClock;
	}
}


//A/D Convertor Control
 volatile long GetVoltage(unsigned char Sensor, unsigned char ADCRange)
{
	ADCSRA = 0b10000111;  //  Setup A/D 156.25kHz clock
	Voltage = 0;
	/* Select input Source */
	if (ADCRange == 0)								/* 0-5V or 0-20V range */
		{
		if (Sensor == Aux0) {ADMUX = 0b01000100;}
		if (Sensor == Aux1) {ADMUX = 0b01000101;}
		if (Sensor == Aux2) {ADMUX = 0b01000110;}
		if (Sensor == Drain) {ADMUX = 0b01000010;}
		if (Sensor == Trigger) {ADMUX = 0b01000011;}
		if (Sensor == Ground) {ADMUX = 0b01011111;}
		}
	if (ADCRange == 1)								/* 0-2.56V or 0-10.24V range */
		{
		if (Sensor == Aux0) {ADMUX = 0b11000100;}
		if (Sensor == Aux1) {ADMUX = 0b11000101;}
		if (Sensor == Aux2) {ADMUX = 0b11000110;}
		if (Sensor == Drain) {ADMUX = 0b11000010;}
		if (Sensor == Trigger) {ADMUX = 0b11000011;}
		if (Sensor == Ground) {ADMUX = 0b11011111;}
		}
	if (ADCRange == 2)								/* 0-1.1V or 0-4.4V range */
		{
		if (Sensor == Aux0) {ADMUX = 0b10000100;}
		if (Sensor == Aux1) {ADMUX = 0b10000101;}
		if (Sensor == Aux2) {ADMUX = 0b10000110;}
		if (Sensor == Drain) {ADMUX = 0b10000010;}
		if (Sensor == Trigger) {ADMUX = 0b10000011;}
		if (Sensor == Ground) {ADMUX = 0b10011111;}
		}

	/* Start A/D conversion */
	ADCSRA = ADCSRA | 0b01000000;  //Start A/D conversion (bit 6 is set)
	while (ADCSRA & 0b01000000) {;}  //  Test if done or wait

	Voltage = ADCW;									/* This word is the 10 bit A/D result */

	if ((Sensor == Drain) || (Sensor == Trigger))  //  Rdivider = 1/4
	{
		if (ADCRange == 0)  //  20.0V
		{
			Voltage = ((Voltage * 20000) / 1023);	/* Convert A/D full range (1023) to 20000mV or 20.0V */
		}
		if (ADCRange == 1)  //  10.23V
		{
			Voltage = ((Voltage * 10240) / 1023);	/* Convert A/D full range (1023) to 10240mV or 10.24V */
		} 
		if (ADCRange == 2)  //  4.4V
		{
			Voltage = ((Voltage * 4400) / 1023);	/* Convert A/D full range (1023) to 4400mV or 4.400V */
		} 
	}

	else  //  No voltage divider
	{
		if (ADCRange == 0)  //  5.0V
		{
			Voltage = ((Voltage * 5000) / 1023);	/* Convert A/D full range (1023) to 5000mV or 5.0V */
		}
		if (ADCRange == 1)  //  2.56V
		{
			Voltage = ((Voltage * 2560) / 1023);	/* Convert A/D full range (1023) to 2560mV or 2.56V */
		} 
		if (ADCRange == 2)  //  1.1V
		{
			Voltage = ((Voltage * 1100) / 1023);	/* Convert A/D full range (1023) to 1100mV or 1.100V */
		} 
	}
return (Voltage);
}


/* EEPROM stuff copied directly from ATMEL data sheet */

void EEPROM_write(unsigned int uiAddress, unsigned char ucData)
	{
	if (uiAddress >= 2048) {return;}  //  Limit address for ATmega644p
	/* Set up address and data registers */
	EECR = (0<<EEPM1 | 0<<EEPM0);
	EEAR = uiAddress;
	EEDR = ucData;
	cli();  //  Disable Interrupts - Real Time Clock is off now
	/* Write logical one to EEMPE */
	EECR |= (1<<EEMPE);
	/* Start eeprom write by setting EEPE */
	EECR |= (1<<EEPE);
		while (EECR & (1<<EEPE)) {}
	sei();  //  Enable Interrupts
	}


unsigned char EEPROM_read(unsigned int uiAddress)
	{
	if (uiAddress >= 2048) {return(0);}  //  Limit address for ATmega644p
	/* Set up address register */
	EEAR = uiAddress;
	cli();  //  Disable Interrupts - Real Time Clock is off now
	/* Start eeprom read by writing EERE */
	EECR |= (1<<EERE);
	/* Return data from data register */
	sei();  //  Enable Interrupts
	return (EEDR);
	}


void StartClock(void)
{
//  1mS real time clock setup on 16 bit timer 1
	TCCR1A = 0b00000000;
	TCCR1B = 0b00001001;
	OCR1A  = 20000;  //  20MHz clock / 20000 = 1mS
	TIMSK1 = 0b00100010;
	RealClock = 0;  //  Be sure time starts at zero
	sei();  //  Enable interrupts on chip
}


//Real time clock update routein on 1mS interrupt
	ISR(TIMER1_COMPA_vect)
	{
	RealClock++;
	}


void MotorPWM(char PWMduty)
	/* Controlling OC0A which is pin 43 PB3 to the motor drive */
	{
	if ((PWMduty != 0) && (MotorStatus == 0)) {MotorStartTime = RealClock;}
	if ((PWMduty == 0) && (MotorStatus == 1)) {MotorStopTime = RealClock;}
	Brake(0); Timer(50);  //Turn off brake
	if (PWMduty != 0) {TCCR0A = 0b10000011; MotorStatus = 1;}  //  Start Motor
	else {TCCR0A = 0b00000011; MotorStatus = 0;}  // Stop Motor
	OCR0A = (char)PWMduty;  //  This is the PWM comparator test value
	TCCR0B = 0b10000010;  //  9.765625kHz clock
	}


void LionHalt(void)
	/* Shut the unit down completely (~2mA) */
	{
	Brake(0);  //  Shut down FETs and Ports
	DDRA = 0b00000000; DDRB = 0b00000000; DDRC = 0b00000000; DDRD = 0b00000000;
	cli();  //  Disable interrupts
	set_sleep_mode(SLEEP_MODE_PWR_DOWN);
	sleep_mode();
	}


void Vibrate(unsigned long Time, unsigned long Frequency, unsigned long Volume)
	// Time in mS, Frequency = 1 to 1000 Hz, Volume = 0 to 100 %
	{
	Brake(0); MotorPWM(0);
	if (Frequency > 1000) {Frequency = 10000;}
	if (Volume > 100) {Volume = 100;}
	unsigned long StartTime = 0;
	StartTime = RealClock;
	while (RealClock <= StartTime + Time)
		{
		PORTB = PORTB | 0b00001000;  //Turn on motor
		Timer(Volume * 4);
		PORTB = PORTB & 0b11110111;  //Turn off motor
		Timer(1000000 / (Frequency + 1));
		}
	}


void Alert(unsigned int number)
	{
	unsigned int k = 0;
	for (k = 1; k <= number; k++)
		{
		Timer(500000);
		Vibrate(500,30,50);
		}
	Timer(500000);		
	}


void CheckMotor(char PWMduty)
	{
	//  Check motor current
	MotorPWM(255);
	Timer(2000);
	MotorCheckCurrent = GetVoltage(Drain,2);
	MotorCheckCurrent = (MotorCheckCurrent * 1074 / 1000);  //  Divided buy RDSon in uOhms for Amps
	MotorPWM(PWMduty);
	}


void DefaultValues(void)
	{
	StoreRunning(1, GunFunction);
	StoreRunning(2, PrecockingEnable);
	StoreRunning(3, ActiveBrakingEnable);

	StoreRunning(32, SniperDelay);
	StoreRunning(33, MotorSpeed);
	StoreRunning(34, AverageCurrentLimit);
	StoreRunning(35, PrecockTriggerDelay);
	StoreRunning(36, BurstFactor);
	StoreRunning(0, 1);  //  Valid values flag
	}


void UserProgramming(void)
{

Vibrate(1000,30,25);

	volatile unsigned char TriggerHold = 0;  //  Debounce flag
	unsigned long TriggerWindow = 3000;  //  Programming wait time
	volatile unsigned long TriggerWindowStart = 0;

	volatile unsigned long Counter = 0;


//Program Mode
		//  1 = Enter program mode
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 1) {Counter = 1;}
		}
	}
	if (Counter == 0) {return;}

//Gun Function
	VibrateSignal(1);
		//  1 = Normal, 2 = Burst, 3 = SemiOnly
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 3) {Counter = 3;}
		}
	}
	if (Counter != 0) {StoreRunning(1, Counter);}

//PreCocking
	VibrateSignal(2);
		//  1 = Precocking off, 2 = 10mS, 3 = 20mS, 4 = 30mS, 5 = 40mS, 6 = 50mS, 7 = 60mS, 8 = 70mS
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 8) {Counter = 8;}
		}
	}
	if (Counter != 0) {StoreRunning(35, ((Counter * 10) - 10)); StoreRunning(2, 1);}
	if (Counter == 1) {StoreRunning(2, 0);}

//Motor Speed
	VibrateSignal(3);
		//  1 = 100%, 2 = 90%, 3 = 80%,...  10 = 10%
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 11) {Counter = 11;}
		}
	}
	if (Counter != 0) {StoreRunning(33, ((255 * (11 - Counter)) / 10));}

//Average Current Limit
	VibrateSignal(4);
		//  1 = 20A, 2 = 30A, 3 = 40A,...  6 = 70A
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 6) {Counter = 6;}
		}
	}
	if (Counter != 0) {StoreRunning(34, ((Counter + 1) * 10));}

//Sniper Delay
	VibrateSignal(5);
		//  1 = None, 2 = 1 Sec, 3 = 2 Sec,... 11 = 10 Sec 
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 11) {Counter = 11;}
		}
	}
	if (Counter != 0) {StoreRunning(32, Counter);}

//Increase Burst Time
	VibrateSignal(6);
		//  2% per pull 
	Counter = GetRunning(36);
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 250) {Counter = 250;}
		}
	}
	if (Counter != GetRunning(36)) {StoreRunning(36, Counter);}

//Decrease Burst Time
	VibrateSignal(7);
		//  2% per pull 
	Counter = GetRunning(36);
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter--;
		if (Counter < 10) {Counter = 10;}
		}
	}
	if (Counter != GetRunning(36)) {StoreRunning(36, Counter);}

//Active Braking
	VibrateSignal(8);
		//  1 = on  2 = off
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 2) {Counter = 2;}
		}
	}
	if (Counter != 0) {StoreRunning(3, Counter);}

//Reset
	VibrateSignal(9);
		//  3 pulls = reset
	Counter = 0;
	TriggerWindowStart = RealClock;
	while (RealClock <= (TriggerWindowStart + TriggerWindow))
	{
	if (GetVoltage(Trigger,0) < 1000) {TriggerHold = 0;} 
	if (((GetVoltage(Trigger,0) > 5000) && (TriggerHold == 0))) 
		{
		TriggerHold = 1; TriggerWindowStart = RealClock;
			Counter++;
		if (Counter > 3) {Counter = 3;}
		}
	}
	if (Counter == 3) {StoreRunning(0, 0);}


}


void VibrateSignal(int Number)
//  Vibrates x number of times
	{

	volatile unsigned long Counter1 = 0;

	unsigned long VibTime = 300;
	unsigned long OffTime = 200;
	unsigned long VibFrequncy = 200;
	unsigned long VibVolume = 20;

		Vibrate(VibTime,VibFrequncy,VibVolume);
		for (Counter1 = 1; Counter1 < Number; Counter1++)
		{
			Timer(OffTime * 1000);
			Vibrate(VibTime,VibFrequncy,VibVolume);
		}
	}




//  Send serial data to USART
int uart_putchar(char c, FILE *stream)
	{
	loop_until_bit_is_set(UCSR0A, UDRE0);
	UDR0 = c;
	return(0);
	}


//  Send serial single command to USART
void uart_contchar(char c)
	{
	loop_until_bit_is_set(UCSR0A, UDRE0);
	UDR0 = c;
	}


//  Get serial data from USART
//  WARNING!  I have never used this so it might be wrong.  "FILE *stream"???
char uart_getchar(void)
	{
	while ( !(UCSR0A & (1<<RXC0)) )
	;
	return UDR0;
	}


//  Serial data USART 0 control and data handling setup
void USART_Init(unsigned int baud)
	//  See page 197 of ATmega644P data sheet
	//  129 is the code for 9600 baud with a 20MHz clock
	//  USART_Init(129);  is the typical command
	{
	/* Set baud rate */
	UBRR0H = (unsigned char) (baud>>8);
	UBRR0L = (unsigned char) baud;
	/* Enable receiver and transmitter */
	UCSR0B = (1<<RXEN0) | (1<<TXEN0);
	/* Set frame format: 8-data, no parity, 1-stop bit (8N1) */
	UCSR0C = (3<<UCSZ00);

	// setup file streams
//	FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
//	stdin = stdout = &uart_stream;
	}

//  Uses first 512 out of 2048 EEPROM bytes

/*
StoreVersion Map
	StoreVersion 0 = Number of power ups
	StoreVersion 1 = MainVersionNumber

StoreHistory Map
	StoreHistory 0 = Powerup BatteryVoltage
	StoreHistory 1 = Error
	StoreHistory 2 = Battery Voltage Error
	StoreHistory 3 = Over Peak Current Error
	StoreHistory 4 = Average Current Error

StoreRunning Map
	StoreRunning 0 = 
	StoreRunning 1 = Gun Function byte
	StoreRunning 2 = Precocking Enable
	StoreRunning 3 = Power Start Enable
	StoreRunning 4 = Cycle Completion Enable
	StoreRunning 32 = SniperDelay (seconds)
	StoreRunning 33 = Motor Speed  (0 - 255)
	StoreRunning 34 = Average Current Limit (Amps)
	StoreRunning 35 = Precock Trigger Delay (mS)
	StoreRunning 36 = BurstAddedTime (mS)

*/




void StoreVersion(unsigned int number1,unsigned long value1)
	//	Stores upto 16 four byte numbers such as dates, powerups, triggerpulls, etc.  Bytes 0 - 63.
	{
		if (number1 > 15) {return;}
		EEPROM_write((number1 * 4 + 0),((value1 & 0xFF000000) / 0x1000000));
		EEPROM_write((number1 * 4 + 1),((value1 & 0x00FF0000) / 0x10000));
		EEPROM_write((number1 * 4 + 2),((value1 & 0x0000FF00) / 0x100));
		EEPROM_write((number1 * 4 + 3),(value1 & 0x000000FF));
	}

unsigned long GetVersion(unsigned int number11)
	//	Gets upto 16 four byte numbers such as dates, powerups, triggerpulls, etc.  Bytes 0 - 63.
	{
		if (number11 > 15) {return(0);}
		volatile unsigned int index12 = 0;
		index12 = EEPROM_read(number11 * 4 + 0) * 0x01000000;
		index12 = index12 + EEPROM_read(number11 * 4 + 1) * 0x00010000;
		index12 = index12 + EEPROM_read(number11 * 4 + 2) * 0x00000100;
		index12 = index12 + EEPROM_read(number11 * 4 + 3) * 0x00000001;
		return(index12);
	}


void StoreHistory(unsigned int number2,unsigned char value2)
	//	Stores last 15 byte values in array 14 units deep.  Byte 0 is the index number. Bytes  64 - 255.
	{
		volatile unsigned int index2 = 0;
		if (number2 > 13) {return;}
		index2 = EEPROM_read(number2 * 16 + 64);
		index2++;
		if ((index2 > 13) || (index2 == 0)) {index2 = 1;}
		EEPROM_write((number2 * 16 + 64), index2);
		EEPROM_write((number2 * 16 + 64 + index2), value2);
	}


void StoreRunning(unsigned int number3, unsigned char value3)
	//	Stores single byte data 256 deep.  Bytes 256-511.
	{
		if (number3 > 255) {return;}
		EEPROM_write((number3 + 256),value3); 
	}


unsigned char GetRunning(unsigned int number4)
	//	Gets single byte data 256 deep.  Bytes 256-511.
	{
		if (number4 > 255) {return(0);}
		return(EEPROM_read(number4 + 256));
	}