/******************************************************************************
 * 
 * Hand Motor Control, by Zach Taylor and Raffi Krikorian - 12/12/04
 *
 * hand_motor.c -- Control motor 3 in the hand.
 * 
 *****************************************************************************/


#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/ina90.h>

// LCD Definitions
#define E PD3
#define RS PD1
#define DB7 PD7
#define DB6 PD6
#define DB5 PD5
#define DB4 PD4

#define lcd_ddr DDRD
#define lcd_port PORTD

// LED Definitions
#define on_ddr DDRD
#define on_port PORTD
#define on_pin PD0

#define contact_ddr DDRD
#define contact_port PORTD
#define contact_pin PD2

#define blink1_ddr DDRB
#define blink1_port PORTB
#define blink1_pin PB5

#define blink2_ddr DDRB
#define blink2_port PORTB
#define blink2_pin PB7

#define rx_led_ddr DDRB
#define rx_led_port PORTB
#define rx_led_pin PB6

// Button Definitions
#define button_ddr DDRB
#define button_port PINB
#define button_pin PB4

// Stepper Definitions
//#define duration 5000
#define duration 30000


void led_on();
void contact_on();
void blink1_on();
void blink1_off();
void blink2_on();
void blink2_off();
void rx_led_on();
void rx_led_off();
void button_call();
void led_check();

void pulse (uint8_t pin);
void step1 ();
void step2 ();
void step3 ();

#define orange_motor3 PC3
#define pink_motor3 PC2
#define blue_motor3 PC1
#define yellow_motor3 PC0

int main()
{
    uint32_t i;

    OSCCAL = 0xFF;

    // disable jtag
    MCUCSR |= ( 1 << JTD );
    MCUCSR |= ( 1 << JTD );

    DDRC = ( 1 << orange_motor3 ) |
	   ( 1 << pink_motor3 ) |
	   ( 1 << blue_motor3 ) |
	   ( 1 << yellow_motor3 );
    PORTC = 0;

    while( 1 ) 
    { 
        PORTC = ( 1 << orange_motor3 ) |
                ( 1 << yellow_motor3 );
	for( i=0;i<duration;i++);
     
        PORTC = ( 1 << orange_motor3 ) | 
                ( 1 << blue_motor3 );
	for( i=0;i<duration;i++);

	PORTC = ( 1 << pink_motor3 ) |
                ( 1 << blue_motor3 );
	for( i=0;i<duration;i++);

	PORTC = ( 1 << pink_motor3 ) |
		( 1 << yellow_motor3 );
	for( i=0;i<duration;i++);

    }    
    

/*
    DDRB = ( 1 << PB5) | ( 1 << PB6 ) | ( 1 << PB7) & ~( 1 << PB4 ) ;
    DDRD = ( 1 << PD0) | ( 1 << PD2 ) ;
    DDRC = 0xFF;
    PORTC = 0;

    led_on();

    while (1) {
        step1();
    }
    while ( !(button_port & ( 1 << button_pin)) ){
        step1();
    }
    blink1_off();
    blink2_on();
    while ( button_port & ( 1 << button_pin) ) {
        step2();
    }
    while ( !(button_port & ( 1 << button_pin)) ){
        step2();
    }
    blink1_on();
    while ( button_port & ( 1 << button_pin) ) {
        step3();
    }
    while ( !(button_port & ( 1 << button_pin)) ){
        step3();
    }
    blink1_off();
    blink2_off();
*/
}

void step1 ()
{
    uint32_t i;    

    PORTC |= ( 1 << PC0 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC0 );

    PORTC |= ( 1 << PC1 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC1 );

    PORTC |= ( 1 << PC2 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC2 );

    PORTC |= ( 1 << PC3 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC3 );
}

void step2 ()
{
    uint32_t i;    

    PORTC |= ( 1 << PC0 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC0 );

    PORTC |= ( 1 << PC1 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC1 );

    PORTC |= ( 1 << PC3 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC3 );

    PORTC |= ( 1 << PC2 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC2 );
}

void step3 ()
{
    uint32_t i;    

    PORTC |= ( 1 << PC0 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC0 );

    PORTC |= ( 1 << PC3 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC3 );

    PORTC |= ( 1 << PC1 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC1 );

    PORTC |= ( 1 << PC2 );
    for (i = 0; i < duration; i++) {}
    PORTC &= ~( 1 << PC2 );
}

void led_check()
{
    button_call();
    blink1_on();
    button_call();
    blink1_off();
    button_call();
    blink2_on();
    button_call();
    blink2_off();
    button_call();
    rx_led_on();
    button_call();
    rx_led_off();
}

void led_on()
{
    on_port |= ( 1 << on_pin);
}

void contact_on()
{
    contact_port |= ( 1 << contact_pin);
}

void blink1_on()
{
    blink1_port |= ( 1 << blink1_pin);
}

void blink1_off()
{
    blink1_port &= ~( 1 << blink1_pin);
}

void blink2_on()
{
    blink2_port |= ( 1 << blink2_pin);
}

void blink2_off()
{
    blink2_port &= ~( 1 << blink2_pin);
}

void rx_led_on()
{
    rx_led_port |= ( 1 << rx_led_pin);
}

void rx_led_off()
{
    rx_led_port &= ~( 1 << rx_led_pin);
}

void button_call()
{
    while ( button_port & ( 1 << button_pin) ) {}
    while ( !(button_port & ( 1 << button_pin)) ){}

}