//
// hello.servo.44.2.c
//
// two-channel software PWM servo motor hello-world
//
// set lfuse to 0x7E for 20 MHz xtal
//
// Neil Gershenfeld
// 4/8/12
//
// (c) Massachusetts Institute of Technology 2012
// Permission granted for experimental and personal use;
// license for commercial sale available from MIT.
//

#include <avr/io.h>
#include <util/delay.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 position_delay() _delay_ms(1000)

#define PWM_port PORTA
#define PWM_direction DDRA
#define PWM_pin_0 (1 << PA6)
#define PWM_pin_1 (1 << PA7)

#define loop_count 3

int main(void) {
   //
   // main
   //
  uint8_t i,j,k;
   static uint16_t sense0;
   static uint16_t sense1;
   int posn; 
   posn = 0; 
   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // set PWM pins to output
   //
   clear(PWM_port, PWM_pin_0);
   output(PWM_direction, PWM_pin_0);
   clear(PWM_port, PWM_pin_1);
   output(PWM_direction, PWM_pin_1);
   //
   // main loop
   //
   while (1) {
     //
     // init A/D
     //
     ADCSRB =  (1 << ADLAR); // left adjust
     ADMUX = (0 << MUX3) | (0 << MUX2) | (0 << MUX1) | (0 << MUX0); // ADC0 = PA0
     ADCSRA = (1 << ADEN) // enable
       | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128
      //
      // initiate conversion
      //
      ADCSRA |= (1 << ADSC);
      //
      // wait for completion
      //
      while (ADCSRA & (1 << ADSC))
         ;
      //
      // record result
      //
      sense0 = ADCH;
     //
     // init A/D
     //
     ADCSRB =  (1 << ADLAR); // left adjust
     ADMUX = (0 << MUX3) | (0 << MUX2) | (0 << MUX1) | (1 << MUX0); // ADC1 = PA1
     ADCSRA = (1 << ADEN) // enable
       | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128
      //
      // initiate conversion
      //
      ADCSRA |= (1 << ADSC);
      //
      // wait for completion
      //
      while (ADCSRA & (1 << ADSC))
         ;
      //
      // record result
      //
      sense1 = ADCH;
      void delay_us(uint16_t n) {
	while(n--) {
	  _delay_us(1);
	}
      }
      /* if (sense0<sense1) { */
      /* 	for (i = 0; i < loop_count; ++i) { */
      /*    set(PWM_port,PWM_pin_0); */
      /*    set(PWM_port,PWM_pin_1); */
      /* 	 delay_us(1510); */
      /*    clear(PWM_port,PWM_pin_0); */
      /*    clear(PWM_port,PWM_pin_1); */
      /* 	 _delay_us(18490);} */
      /* }; */
      /* if (sense0>sense1) { */
      /* 	for (i = 0; i < loop_count; ++i) { */
      /*    set(PWM_port,PWM_pin_0); */
      /*    set(PWM_port,PWM_pin_1); */
      /* 	 _delay_us(1490); */
      /*    clear(PWM_port,PWM_pin_0); */
      /*    clear(PWM_port,PWM_pin_1); */
      /* 	 _delay_us(18510);} */
      /* }; */
      int dp = 40;  
      if (sense0>sense1 & posn<500) {posn = posn+dp;};
      if (sense1>sense0 & posn>-500) {posn = posn-dp;};
      //
      //
      //
      for (i = 0; i < loop_count; ++i) {
         set(PWM_port,PWM_pin_0);
         set(PWM_port,PWM_pin_1);
      	 j = 0;
      	 delay_us(1500+posn);
         clear(PWM_port,PWM_pin_0);
         clear(PWM_port,PWM_pin_1);
	 delay_us(18500-posn);}
      };

      /* // */
      /* // 1.5 ms on time, both */
      /* // */
      /* for (i = 0; i < loop_count; ++i) { */
      /*    set(PWM_port,PWM_pin_0); */
      /*    set(PWM_port,PWM_pin_1); */
      /*    _delay_us(1500); */
      /*    clear(PWM_port,PWM_pin_0); */
      /*    clear(PWM_port,PWM_pin_1); */
      /*    _delay_us(18500); }*/
};