//
//
// DC-Pumps.c
//
// DC Pumps motor(s) control based on distance and direction of the SONAR
//
// Neil Gershenfeld & Ravi Tejwani
//
// (c) Massachusetts Institute of Technology 2010
// 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.
//
#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 PWM_delay() _delay_ms(1000) // PWM delay
#define on_delay() _delay_us(5000) // PWM on time

#define port_b PORTB // DC Motor Pump - 1 
#define port_b_direction DDRB // DC Motor Pump - 1 direction

#define port_a PORTA // DC Motor Pump - 2
#define port_a_direction DDRA // DC Motor Pump - 2 direction

#define pump1 (1 << PB2) // DC Motor Pump - 1 pin
#define pump2 (1 << PA7) // DC Motor Pump - 2 pin

#define en_port PORTA // Enable pin
#define en_direction DDRA 
#define en_pin (1 << PA0) 
#define en_inputs PINA

#define dir_port PORTA // Directional pin
#define dir_direction DDRA
#define dir_pin (1 << PA1)
#define dir_inputs PINA

int main(void) {
    //
    // main
    //

    //
    // set clock divider to /1
    //
    CLKPR = (1 << CLKPCE);
    CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
    //
    // initialize DC motor pump pins
    //
    clear(port_b, pump1);
    output(port_b_direction, pump1);

    clear(port_a, pump2);
    output(port_a_direction, pump2);

    //
    // main loop
    //
    while (1) {

        // Turn the dc motor pumps ON or OFF based on enable and directional pins
        if(pin_test(en_inputs,en_pin)) {
            // Enable pin is ON so check the directional pin
            // If the directional pin is set then turn ON the pump1 else turn oON the pump2
            if(pin_test(dir_inputs,dir_pin)) {
                // Turn ON the pump1 since directional pin is ON
                set(port_a,pump2);
                clear(port_b,pump1);
            } else {
                // Turn ON the pump2 since directional pin is OFF
                clear(port_a,pump2);
                set(port_b,pump1);
            }
        } else {
            // Enable pin if OFF so turn OFF all the pumps
            clear(port_a,pump2);
            clear(port_b,pump1);
        }

//        if (pin_test(en_inputs,en_pin)) {
//            set(port_b,pump1);
//        } else {
//            clear(port_b,pump1);  
//
//        }
//        if (pin_test(dir_inputs,dir_pin)) {
//            set(port_a,pump2);
//        } else {
//            clear(port_a,pump2);  
//
//        }

    }
}