// hello.servo.44.2.c
//
// two-channel software PWM servo motor hello-world
//
// set lfuse to 0x5E for 20 MHz xtal
//
// Neil Gershenfeld
// 4/8/12
//
// (c) Massachusetts Institute of Technology 2012
// This work may be reproduced, modified, distributed,
// performed, and displayed for any purpose. Copyright is
// retained and must be preserved. The work is provided
// as is; no warranty is provided, and users accept all 
// liability.
//
// Shamelessly copied and mofied from Neil Gershenfeld's code by Rob Hart for purposes of instruction.
//
// Button  PA3 (pin 10).  
// Servo   PA7 (pin 6). 
#include <avr/io.h>
#include <util/delay.h>

#define output(directions,pin) (directions |= pin) // set port direction for output
#define input(directions,pin) (directions &= (~pin)) // set port direction for input
#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 input_port PORTA // The button is on port A
#define input_direction DDRB	//  DDRA defines input/output for port A
#define input_pin (1 << PA3)	//  The button is on pin 3 of port A
#define input_pins PINA	//  PINA is the register that is read to detect input high or low

#define loop_count 60

int main(void) {
   //
   // main
   //
   uint8_t i;
   //
   // 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);
   //
   // initialize input pins
   //
   set(input_port, input_pin); // turn on pull-up
   input(input_direction, input_pin);
   //
   // main loop
   //
   while (1) {
      //
      if (pin_test(input_pins,input_pin))
      //
      //
      // 1 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(1000);
	         clear(PWM_port,PWM_pin_0);
	         clear(PWM_port,PWM_pin_1);
	         _delay_us(19000);
	         }
      //
	else	
      //
      // 2.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(2500);
	         clear(PWM_port,PWM_pin_0);
	         clear(PWM_port,PWM_pin_1);
	         _delay_us(17500);
         }

      }
   }