#define F_CPU 20000000UL    // Baby Orangutan frequency (20MHz)
#include <avr/io.h>
#include <util/delay.h>

//Control de motores.Salidas.
#define M1A PORTD5
#define M2A PORTD3
#define M1B PORTD6
#define M2B PORTB3
//Leds. Salidas.
#define LEDP PORTD1
#define    LEDV PORTB1
#define    LEDR PORTD0
//Interruptores. Entradas.
#define SW1    PORTB0
#define SW2 PORTB2
//Sensores. Entradas y salidas.
#define    S0 PORTD7
#define S1 PORTD4
#define S2 PORTD2
#define S3 PORTB5
#define S4 PORTB4
#define S5 PORTC3
#define S6 PORTC2
#define S7 PORTC1
#define LED_ON PORTC0

void inicializar_puertos(void);
void reset(void);

int main( void )
{
    unsigned char leer_pin;

    inicializar_puertos();
    reset();

    leer_pin = PINB & (1<<SW1);
    if(leer_pin)
    {
    PORTD &= ~(1<<LEDR);    //Diodos apagados.
    PORTB &= ~(1<<LEDV);
    PORTC &= ~(1<<LED_ON);
    }
    else            
    {
    PORTD |= (1<<LEDR);        //Diodos encendidos.
    PORTB |= (1<<LEDV);
    PORTC |= (1<<LED_ON);
    }

    while ( 1 )
    {    
        DDRD |= ((1<<S0)|(1<<S1)|(1<<S2)); //Se configuran los sensores como salida.
        DDRB |= ((1<<S3)|(1<<S4));
        DDRC |= ((1<<S5)|(1<<S6)|(1<<S7));

        PORTD |= ((1<<S0)|(1<<S1)|(1<<S2)); //Se pone a 1 la salida
        PORTB |= ((1<<S3)|(1<<S4));
        PORTC |= ((1<<S5)|(1<<S6)|(1<<S7));

        _delay_us(40); //Se descarga el condensador.

        DDRD &= ~((1<<S0)|(1<<S1)|(1<<S2)); //Se configuran los sensores como entrada.
        DDRB &= ~((1<<S3)|(1<<S4));
        DDRC &= ~((1<<S5)|(1<<S6)|(1<<S7));

        PORTD &= ~((1<<S0)|(1<<S1)|(1<<S2)); //Pull-up apagado, Hi-Z
        PORTB &= ~((1<<S3)|(1<<S4));
        PORTC &= ~((1<<S5)|(1<<S6)|(1<<S7));

        _delay_ms(4);
        
    }
    return 0;
}

void inicializar_puertos(void)
{
   DDRD=0x6A;     //0110 1011  0,1,3,5,6 Salidas
   PORTD=0x00;   
   DDRB=0x0A;     //0000 1010  1,3 Salidas
   PORTB=0x00;
   DDRC=0x01;     //0000 0001  0 Salida
   PORTC=0x00;  
}

void reset(void)
{
    PORTD |= (1<<LEDP); //Encendemos Led en Baby Orangutan.
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);//Apagamos Led en Baby Orangutan.
    _delay_ms(300);
    PORTD |= (1<<LEDP);
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);
    _delay_ms(300);
    PORTD |= (1<<LEDP);
    _delay_ms(300);
    PORTD &= ~(1<<LEDP);
}