/*
 * HoverTiedFinal.c
 *
 *  Author: Taylor, Jarrod, Evgeny
 */ 


#include <avr/io.h>
#include <math.h>

void Delay(int delay);

void Arm_ESC(void);

void Delay(int delay)
{
	int x, y;
	for (x = delay; x != 0; x--)
	{
		for (y = 1000; y != 0; y--)
		{
			asm volatile ("nop"::); //do nothing for a bit
		}
	}
}

void Arm_ESC(void){
	DDRB  = 0b00000110;   // PB1 and PB2 as outputs
	DDRD  = 0b01100000;   // PD5 and PD6 as outputs
		
	TCCR0A = _BV(COM0A1) | _BV(COM0B1) | _BV(WGM00) | _BV(WGM01);      // Non inverting mode on OC2A and OC2B, Mode Fast PWM, Bottom Start, 0xFF
	TCCR0B = _BV(CS01) | _BV(CS00);		  // Clock/64
		
	TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);      // Non inverting mode on OC1A and OC1B, Mode Fast PWM 8-bit, Bottom Start, 0x00FF
	TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10);          // Clock/64

	OCR1B = (unsigned char)(0);							// Zero out all the outputs
	OCR1A = (unsigned char)(0);
	OCR0A = (unsigned char)(0);
	OCR0B = (unsigned char)(0);
	Delay(4000);
	OCR1B = (unsigned char)(160);						// Arm motors by inputing lowest value
	OCR1A = (unsigned char)(160);
	OCR0A = (unsigned char)(150);
	OCR0B = (unsigned char)(150);
	Delay(4000);
	OCR1B = (unsigned char)(0);							// Init to zero before accelerating
	OCR1A = (unsigned char)(0);
	OCR0A = (unsigned char)(0);
	OCR0B = (unsigned char)(0);
	Delay(4000);

}

int main (void)
{
	Arm_ESC();
	
	while (1)
	{
		int i=0, j=0, k=0, z=0;
		
		for(i=0; i<40; i++){	//Start Motors slow
			if(i%2 == 0){
				 j++;			//Increment for M6
				 k++;			//Increment for M5
			}				 
			if(i%4==0){
				k++;			//Increment for M5
			}				
			
			OCR1B = (unsigned char)(143+i);			// Acceleration 
			OCR1A = (unsigned char)(143+i);
			OCR0A = (unsigned char)(130+k);
			OCR0B = (unsigned char)(193+j);
			Delay(300);
		}
		
		OCR1B = (unsigned char)(184);				// Hovering speed
		OCR1A = (unsigned char)(184);
		OCR0A = (unsigned char)(160);
		OCR0B = (unsigned char)(212);
		Delay(20000);								// Hover for "this" long
		
		i=0, j=0, k=0;
		
		for(i=0; i<8; i++){						// Decelerate for loop
			if(i%2 == 0){
				j++;
				k++;
			}
			if(i%4==0){
				k++;
			}				
			
			OCR1B = (unsigned char)(182-i);			// Decelerate
			OCR1A = (unsigned char)(184-i);
			OCR0A = (unsigned char)(160-k);
			OCR0B = (unsigned char)(212-j);
			Delay(250);
		}
		
		OCR1B = (unsigned char)(0);				// Stop motors
		OCR1A = (unsigned char)(0);
		OCR0A = (unsigned char)(0);
		OCR0B = (unsigned char)(0);
		
		return(0);
					
	}		
	
	return(0);
}