/*
 * Quadcopter
 *
 */ 

#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);
	OCR1A = (unsigned char)(0);
	OCR0A = (unsigned char)(0);
	OCR0B = (unsigned char)(0);
	Delay(4000);
	OCR1B = (unsigned char)(160);
	OCR1A = (unsigned char)(160);
	OCR0A = (unsigned char)(150);
	OCR0B = (unsigned char)(150);
	Delay(4000);
	OCR1B = (unsigned char)(0);
	OCR1A = (unsigned char)(0);
	OCR0A = (unsigned char)(0);
	OCR0B = (unsigned char)(0);
	Delay(4000);

}

int main (void)
{
	Arm_ESC();
	
	while (1)
	{
		OCR1B = (unsigned char)(143);
		OCR1A = (unsigned char)(143);
		OCR0A = (unsigned char)(130);
		OCR0B = (unsigned char)(193);
	}		
	
	return(0);
}