/******************************************************************************
 * 
 * Hand, by Zach Taylor - 12/12/04
 *
 * hand.c -- Control 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 29000
#define orange_motor3 PC3
#define pink_motor3 PC2
#define blue_motor3 PC1
#define yellow_motor3 PC0

#define orange_motor2 PC7
#define pink_motor2 PC6
#define blue_motor2 PC5
#define yellow_motor2 PC4

// RF In Definitions
#define rf_in_port PINA
#define rf_in PA6

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 delay();
void init_lcd();
void lcd_tx( uint8_t );
void lcd_tx_char( uint8_t );
void lcd_tx_int( uint8_t );
void lcd_tx_int16( uint16_t );
void lcd_clear();
void lcd_2nd_line();
void lcd_zero_pos();

void pulse (uint8_t pin);
void motor3_f ();
void motor2_f ();
void motor3_b ();
void motor2_b ();

int main()
{
    OSCCAL = 0xFF;

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

    DDRA &= ~( 1 << rf_in);
    DDRB |= ( 1 << PB5 ) | ( 1 << PB6 ) | ( 1 << PB7 ) & ~( 1 << PB4 ) ;
    DDRD |= ( 1 << PD0 ) | 
            ( 1 << PD2 ) |
            ( 1 << E ) |
            ( 1 << RS ) |
            ( 1 << DB7 ) |
            ( 1 << DB6 ) |
            ( 1 << DB5 )|
            ( 1 << DB4 );

    DDRC |= ( 1 << orange_motor3 ) |
	   ( 1 << pink_motor3 ) |
	   ( 1 << blue_motor3 ) |
	   ( 1 << yellow_motor3 ) |
           ( 1 << orange_motor2 ) |
	   ( 1 << pink_motor2 ) |
	   ( 1 << blue_motor2 ) |
	   ( 1 << yellow_motor2 );
    PORTC = 0;

    led_on();
    contact_on();

    while (1) {
        if (PINA & ( 1 << PA6) ) {
            rx_led_on();
        } if ( !(PINA & ( 1 << PA6) ) ) {
            rx_led_off();
        }
    }

/*
    init_lcd();
    lcd_zero_pos();
    lcd_tx_char( 'R' );

    while (1){
    motor2_f();
    motor3_f();
    }
*/

/*
    while ( 1 ) {
        if ( ( rf_in_port & ( 1 << rf_in) ) ) {
            rx_led_on();
        }
    }
*/
}

void motor3_f ()
{
    uint32_t i;

    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++);
}

void motor3_b ()
{
    uint32_t i;

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

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

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

void motor2_f ()
{
    uint32_t i;

    PORTC = ( 1 << orange_motor2 ) |
            ( 1 << yellow_motor2 );
    for( i=0;i<duration;i++);
     
    PORTC = ( 1 << orange_motor2 ) | 
            ( 1 << blue_motor2 );
    for( i=0;i<duration;i++);

    PORTC = ( 1 << pink_motor2 ) |
            ( 1 << blue_motor2 );
    for( i=0;i<duration;i++);

    PORTC = ( 1 << pink_motor2 ) |
            ( 1 << yellow_motor2 );
    for( i=0;i<duration;i++);
}

void motor2_b ()
{
    uint32_t i;

    PORTC = ( 1 << orange_motor2 ) |
            ( 1 << yellow_motor2 );
    for( i=0;i<duration;i++);
     
    PORTC = ( 1 << pink_motor2 ) |
            ( 1 << yellow_motor2 );
    for( i=0;i<duration;i++);

    PORTC = ( 1 << pink_motor2 ) |
            ( 1 << blue_motor2 );
    for( i=0;i<duration;i++);

    PORTC = ( 1 << orange_motor2 ) | 
            ( 1 << blue_motor2 );
    for( i=0;i<duration;i++);
}


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)) ){}

}

void init_lcd() 
{
    // initialization sequence
    delay();
    lcd_tx( ( 1 << DB5 ) | ( 1 << DB4 ) );    
    lcd_tx( ( 1 << DB5 ) | ( 1 << DB4 ) );    
    lcd_tx( ( 1 << DB5 ) | ( 1 << DB4 ) );    

    // set the 4-bit interface
    lcd_tx( 1 << DB5 );
    
    // set two lines and a 5x7 font
    lcd_tx( 1 << DB5 );
    lcd_tx( 1 << DB7 );

    // turn the display on/off
    lcd_tx( 0 );
    lcd_tx( ( 1 << DB7 ) | ( 1 << DB6 ) | ( 1 << DB5 ) );

    // entry mode set
    lcd_tx( 0 );
    lcd_tx( ( 1 << DB6 ) | ( 1 << DB5 ) );    

}

void lcd_tx( uint8_t a ) 
{
    lcd_port = ( a | ( 1 << on_pin ) | ( 1 << contact_pin ) );
    lcd_port |= ( 1 << E );
    _NOP();
    _NOP();
    _NOP();
    _NOP();
    _NOP();
    lcd_port &= ~( 1 << E );
    delay();

}

void lcd_tx_int( uint8_t num )
{
    uint8_t i;

    i = 0;
    while ( num > 99 )
    {
        num = num - 100 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );
 
    i = 0;
    while ( num > 9 )
    {
        num = num - 10 ;
        i++;   
    }
   lcd_tx_char ( i + '0' );
 
    i = 0;
    while ( num > 0 )
    {
        num = num - 1 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );
 
}

void lcd_tx_int16( uint16_t num )
{
    uint8_t i;

    i = 0;
    while ( num > 9999 )
    {
        num = num - 10000 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );

    i = 0;
    while ( num > 999 )
    {
        num = num - 1000 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );

    i = 0;
    while ( num > 99 )
    {
        num = num - 100 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );
 
    i = 0;
    while ( num > 9 )
    {
        num = num - 10 ;
        i++;   
    }
   lcd_tx_char ( i + '0' );
 
    i = 0;
    while ( num > 0 )
    {
        num = num - 1 ;
        i++;   
    }
    lcd_tx_char ( i + '0' );
 
}

void lcd_tx_char( uint8_t a )
{
    lcd_tx( ( a ) | ( 1 << RS) );
    lcd_tx( ( a << 4 ) | ( 1 << RS) );

}

void delay() 
{
    uint32_t i;
    for (i = 0; i < 100000; i++);

}

void lcd_clear()
{
    lcd_tx ( 0 );
    lcd_tx ( 0x10 );

}

void lcd_zero_pos()
{
    lcd_tx( 1 << DB7 );
    lcd_tx( 0 );

}

void lcd_2nd_line ()
{
    lcd_tx( ( ( 1 << DB7 ) + ( 1 << DB6) ) );
    lcd_tx( 0 );
   
}