#define F_CPU 20000000

/*all commented out sections are for serial com*/

#include <util/delay.h>
#include <avr/io.h>
/*
#include <avr/pgmspace.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
#define bit_delay_time 8.5 // bit delay for 115200 with overhead
#define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay
#define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay
#define char_delay() _delay_ms(10) // char delay

#define serial_port PORTA
#define serial_direction DDRA
#define serial_pins PINA
#define serial_pin_in (1 << PA0)
#define serial_pin_out (1 << PA1)
*/

#define servo1 PA5
#define servo2 PA6
#define angle1 0
#define angle2 0

int map(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max);
int constrain(int val, int low, int high);
void delay_ms(int millis);
void delay_us(int micros);
/*void put_char(volatile unsigned char *port, unsigned char pin, char txchar);
void put_decimal(uint16_t value);*/

int main(void) {

CLKPR = (1 << CLKPCE); //set clkps register to not divide clock speed by 8
CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);

/*set(serial_port, serial_pin_out);
output(serial_direction, serial_pin_out);*/

DDRA |= (1<<servo1);
DDRA |= (1<<servo2);

while(1) {

int pw1 = map(angle1,-45,45,1000,2000);
int pw1_actual = constrain(pw1,1250,1750);
int pw2 = map(angle2,-45,45,1000,2000);
int pw2_actual = constrain(pw2,1250,1750);

PORTA |= (1<<servo1);
delay_us(pw1_actual);
PORTA &= ~(1<<servo1);
PORTA |= (1<<servo1);
delay_us(pw2_actual);
PORTA &= ~(1<<servo1); 
delay_ms(22);

/*put_decimal(pw_actual);
put_char(&serial_port, serial_pin_out, 10); // new line
delay_ms(1000);*/
}

return(0);

}



///////////////////////////////////////


int constrain(int val, int low, int high) {
 if (val < low) val = low;
 if (val > high) val = high;
 return(val);
 }
 
int map(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void delay_ms(int millis) {
 while ( millis ) {
 _delay_ms(1);
 millis--;
 }
}

void delay_us(int micros) {
while(micros) {
_delay_us(1);
micros--;
}
}

/*
void put_decimal(uint16_t value){
	uint8_t temp[6];
	int8_t p = 0;
	
	while(value > 0) {
		uint8_t cur_digit = value %10;
		temp[p++] = cur_digit + '0';
		value = value/10;
	}
	
	while(p>0){
		put_char(&serial_port, serial_pin_out,temp[--p]);
		char_delay();
	}
}

void put_char(volatile unsigned char *port, unsigned char pin, char txchar) {
   //
   // send character in txchar on port pin
   //    assumes line driver (inverts bits)
   //
   // start bit
   //
   clear(*port,pin);
   bit_delay();
   //
   // unrolled loop to write data bits
   //
   if bit_test(txchar,0)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,1)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,2)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,3)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,4)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,5)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,6)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   if bit_test(txchar,7)
      set(*port,pin);
   else
      clear(*port,pin);
   bit_delay();
   //
   // stop bit
   //
   set(*port,pin);
   bit_delay();
   //
   // char delay
   //
   bit_delay();
   }
   */