//
//
// h-step.c
//
// H-bridge + Step Response
// Sam Calisch
// 11/20/12
//
// Based on:
// Neil Gershenfeld
// 11/18/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 on_delay() _delay_us(3) // PWM on time
#define fast_off_delay() _delay_us(1) // PWM fast off time
#define medium_off_delay() _delay_us(3) // PWM medium off time
#define slow_off_delay() _delay_us(5) // PWM slow off time
#define settle_delay() _delay_us(100) // settle delay
#define PWM_count 20000 // number of PWM cycles
#define cycle_count 1 // number of speed cycles
#define nloop 25 // loops to accumulate

#define bridge_port PORTA // H-bridge port
#define bridge_direction DDRA // H-bridge direction
#define IN1 (1 << PA3) // IN1
#define IN2 (1 << PA2) // IN2

#define transmit_port PORTB
#define transmit_pin (1 << PB2) //transmit for txrx

int main(void) {
   //
   // main
   //
   static uint16_t count;
   static uint8_t cycle;
   static uint16_t up,down,value,speed;
   //
   // set clock divider to /1
   //
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   // initialize H-bridge pins
   //
   clear(bridge_port, IN1);
   output(bridge_direction, IN1);
   clear(bridge_port, IN2);
   output(bridge_direction, IN2);

   //
   // init A/D
   //
   ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref
      | (0 << ADLAR) // right adjust
      | (0 << MUX5) | (0 << MUX4) | (0 << MUX3) | (1 << MUX2) | (1 << MUX1) | (1 << MUX0); // PA7
   ADCSRA = (1 << ADEN) // enable
      | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // prescaler /128

   //
   // main loop
   //
   while (1) {
      //
      // accumulate
      //
      up = 0;
      down = 0;
      for (count = 0; count < nloop; ++count) {
         //
         // settle, charge
         //
         settle_delay();
         set(transmit_port, transmit_pin);
         //
         // initiate conversion
         //
         ADCSRA |= (1 << ADSC);
         //
         // wait for completion
         //
         while (ADCSRA & (1 << ADSC))
            ;
         //
         // save result
         //
         up += ADC;
         //
         // settle, discharge
         //
         settle_delay();
         clear(transmit_port, transmit_pin);
         //
         // initiate conversion
         //
         ADCSRA |= (1 << ADSC);
         //
         // wait for completion
         //
         while (ADCSRA & (1 << ADSC))
            ;
         //
         // save result
         //
         down += ADC;
      }

      //take difference and bin to determine speed
      if (up > down){
        value = (up - down)/25.;
      }
      else{
        value = 0;
      }

      if (value < 1){
        speed = 0;
      }
      else if (value < 3){
        speed = 1;
      }
      else{
        speed = 2;
      }


      for (cycle = 0; cycle < cycle_count; ++cycle) {
         clear(bridge_port, IN1);
         set(bridge_port, IN2);
         for (count = 0; count < PWM_count; ++count) {
            set(bridge_port, IN2);
            on_delay();
            clear(bridge_port, IN2);
            if(speed==2){
              fast_off_delay();
            }
            if(speed==1){
              medium_off_delay();
            }
            if(speed==0){
              slow_off_delay();
            }
         }
         clear(bridge_port, IN2);
         set(bridge_port, IN1);
         for (count = 0; count < PWM_count; ++count) {
            set(bridge_port, IN1);
            on_delay();
            clear(bridge_port, IN1);
            if(speed==2){
              fast_off_delay();
            }
            if(speed==1){
              medium_off_delay();
            }
            if(speed==0){
              slow_off_delay();
            }
         }
      }
   }
}