//Interfaz MiniZ
#include <avr/io.h>

#define F_CPU 16000000UL
#include <util/delay.h>
#include <avr/interrupt.h>

#define BAUD 9600
#define MYUBRR ((F_CPU/(BAUD*16UL))-1)

#define Bus_Datos PORTC
#define Bus_Control PORTA

#define RS  PORTA5               
#define RW  PORTA6
#define E   PORTA7

#define LED1 PORTL7  //Amarillo
#define LED2 PORTL5  //Rojo
#define LED3 PORTG1  //Verde

//Comandos para situar el cursor en inicio de líneas.
#define DDRA_LINEA1 0x80    //1000 0000
#define DDRA_LINEA2 0xC0    //1100 0000
#define DDRA_LINEA3 0x94    //1001 0100
#define DDRA_LINEA4 0xD4    //1101 0100

void inicializar_puertos(void);
void reseteo(void);
void escribir_valor_decimal(unsigned char registro);
void escribir_direccion(unsigned char valorl, unsigned char valorh);
void escribir_int_decimal(unsigned int registro);

//Funciones para escribir en el LCD
void lcd_inicializar();
void lcd_escribir_c(char caracter);
void lcd_escribir(char *cadena);
void posicionar_cursor(unsigned char x, unsigned char y);
void lcd_comando(char comando);
char lcd_invertir_bits(char caracter);
char escribir_angulo(unsigned char adcbajo, unsigned char adcalto);


//Funciones I2C
void i2c_ST(unsigned char dato);
unsigned char i2c_SR(void);
void i2c_error(void);

//Pasar datos al PC
void servo_a_pc(void);




//Variables
volatile unsigned char botones;
volatile unsigned char programa;
volatile unsigned char i2c_enviar;
volatile unsigned char i2c_recibido;
volatile unsigned char boton_libre;
volatile unsigned char comunicaciones;

volatile unsigned char posicion;   //i2c al miniz
volatile unsigned char velocidad;
volatile unsigned char r_estado;
volatile unsigned char baterial;
volatile unsigned char bateriah;
volatile unsigned char direccionl;
volatile unsigned char direccionh;
volatile unsigned char sensoresd;
volatile unsigned char sensoresi;
volatile unsigned char encoder1;
volatile unsigned char encoder0;
volatile unsigned char estadoleds;

volatile unsigned char ainicioalto;  //pantalla 4
volatile unsigned char ainiciobajo;
volatile unsigned char afinalalto;
volatile unsigned char afinalbajo;
volatile unsigned char capturar;

volatile unsigned char pc_adc_alto[250]; //datos para mandar al pc
volatile unsigned char pc_adc_bajo[250];

int main(void)
{

    static unsigned char copia_i2c_enviar=0;
    static unsigned char copia_i2c_recibido=0;
    static unsigned char pantalla=0; //static quitar
    unsigned char pantalla_old=0;

    unsigned int posicion_sec;
    unsigned int velocidad_sec;
    unsigned int bateria;
    unsigned char copia_baterial;
    unsigned char copia_bateriah;
    unsigned char copia_sensoresd;
    unsigned char copia_sensoresi;

    static unsigned char seleccion4 = 0x01;
    static unsigned char lanzar4;
    static unsigned char posicion_i_fija;
    static unsigned char posicion_f_fija;
    int i = 0; //limpiar vector de pc

    char linea1[]="Interfaz MiniZ";  
    char linea2[]="con Arduino MeGa";
    char linea3[]="www.jmnlab.com";


    inicializar_puertos();
    reseteo();
    lcd_inicializar();

    PORTL &= ~((1<<LED1)|(1<<LED2));
    PORTG &= ~(1<<LED3);

    lcd_comando(0x01);
    posicionar_cursor(1,4);  
    lcd_escribir(linea1);         
     posicionar_cursor(2,3);         
    lcd_escribir(linea2);
    posicionar_cursor(4,4);
    lcd_escribir(linea3);


    while(1)
    {
        switch(botones)
        {

        case 7:
        {    
            if(boton_libre==0)
            {
                boton_libre=1;
                
                if(pantalla==1)   
                    r_estado ^=0x02;    //Leds del coche
                else if(pantalla==4)
                    lanzar4 = 0x01;
            }
        }break;

        case 6:
        {
            if(boton_libre==0)
            {
                boton_libre=1;  
                
                if(pantalla==1)
                    r_estado ^=0x01;    //Leds del coche
                else if(pantalla==4)
                {
                    seleccion4--;
                    if(seleccion4<1)
                        seleccion4=1;
                }
            }
            
        }break;

        case 5:
        {
            if(boton_libre==0)
            {
                boton_libre=1;

                if(pantalla==1)   
                    r_estado ^=0x04;    //Leds del coche
                else if(pantalla==4)
                {
                    seleccion4++;
                    if(seleccion4>3)
                        seleccion4=3;
                }
            }
            
        }break;

        case 4:
        {
            pantalla=4;
        }break;

        case 3:
        {
            pantalla=3;   
        }break;

        case 2:
        {
            pantalla=2;
        }break;

        case 1:
        {
            pantalla=1;
        }break;

        default: break;
        }


        ADMUX  &= ~(1<<MUX0);            //Canal 0.
         _delay_us(600);                     
        posicion=ADCH;

 //       posicionar_cursor(4,10);        //Canal 1
        ADMUX  |= (1<<MUX0);    
        _delay_us(600);                     
        velocidad=ADCH;

        /************************************************/

        switch(pantalla)
        {

        case 0:
        {
        }break;


        case 1:
        {        
            if(pantalla_old != 1)
            {
                   PORTL &= ~(1<<LED2);
                PORTL &= ~(1<<LED1);
                PORTG |= (1<<LED3);    
                pantalla_old=1;
                lcd_comando(0x01);;
                lcd_escribir("Prueba conexion I2C.");
                 posicionar_cursor(2,1);
                   lcd_escribir("Led V I:     C:");
                posicionar_cursor(3,1);
                lcd_escribir("Led A I:     C:");
                posicionar_cursor(4,1);
                lcd_escribir("Led R I:     C:");
            }

            copia_i2c_enviar=r_estado;
            copia_i2c_recibido=estadoleds;
        
    
            if(((copia_i2c_enviar)&(0x01))!=0)
                {
                    posicionar_cursor(2,10);
                 lcd_escribir_c('O');
                 lcd_escribir_c('N');
                 lcd_escribir_c(' ');
                }
                else
            {
                posicionar_cursor(2,10);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
            }    

            copia_i2c_enviar=r_estado;
            if(((copia_i2c_enviar)&(0x02))!=0)
            {
                posicionar_cursor(3,10);
                lcd_escribir_c('O');
                lcd_escribir_c('N');
                lcd_escribir_c(' ');
            }
            else
            {
                posicionar_cursor(3,10);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
                }

            copia_i2c_enviar=r_estado;
            if(((copia_i2c_enviar)&(0x04))!=0)
            {
                posicionar_cursor(4,10);
                lcd_escribir_c('O');
                lcd_escribir_c('N');
                lcd_escribir_c(' ');
            }
            else
            {
                posicionar_cursor(4,10);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
            }

            if(((copia_i2c_recibido)&(0x01))!=0)
            {
                   posicionar_cursor(2,17);
                lcd_escribir_c('O');
                lcd_escribir_c('N');
                lcd_escribir_c(' ');
            }
            else
            {
                posicionar_cursor(2,17);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
            }    

            copia_i2c_recibido=estadoleds;
            if(((copia_i2c_recibido)&(0x02))!=0)
            {
                posicionar_cursor(3,17);
                lcd_escribir_c('O');
                lcd_escribir_c('N');
                lcd_escribir_c(' ');
            }
            else
            {
                posicionar_cursor(3,17);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
            }

            copia_i2c_recibido=estadoleds;
            if(((copia_i2c_recibido)&(0x04))!=0)
            {
                posicionar_cursor(4,17);
                lcd_escribir_c('O');
                lcd_escribir_c('N');
                lcd_escribir_c(' ');
            }
            else
            {
                posicionar_cursor(4,17);
                lcd_escribir_c('O');
                lcd_escribir_c('F');
                lcd_escribir_c('F');
            }
        }break;
        
        case 2:
        {
            if(pantalla_old != 2)
            {
                PORTL &= ~((1<<LED1)|(1<<LED2));
                PORTG &= ~(1<<LED3);
                PORTL |= (1<<LED1);
                pantalla_old=2;
                lcd_comando(0x01);
                lcd_escribir("Direccion:        s");
                posicionar_cursor(1,18);
                lcd_escribir_c(0b11100100);
                posicionar_cursor(2,1);
                lcd_escribir("Realimentacion:");
                posicionar_cursor(3,1);
                lcd_escribir("Velocidad:        s");
                posicionar_cursor(3,18);
                lcd_escribir_c(0b11100100);
                posicionar_cursor(4,1);
                lcd_escribir("EI:");
                posicionar_cursor(4,11);
                lcd_escribir("ED:");

            }
                posicionar_cursor(2,17);
                escribir_direccion(direccionl, direccionh);
                posicionar_cursor(4,5);
                escribir_valor_decimal(encoder1);
                posicionar_cursor(4,15);
                escribir_valor_decimal(encoder0);

                posicion_sec=posicion;
                velocidad_sec=velocidad;

                posicion_sec = 1000 + posicion_sec*4;
                velocidad_sec = 1000 + velocidad_sec*4;
                posicionar_cursor(1,12);
                escribir_int_decimal(posicion_sec);
                posicionar_cursor(3,12);
                escribir_int_decimal(velocidad_sec);
        }break;

        case 3:
        {
            if(pantalla_old != 3)
            {
                PORTL &= ~((1<<LED1)|(1<<LED2));
                PORTG &= ~(1<<LED3);
                PORTL |= (1<<LED2);
                pantalla_old=3;
                lcd_comando(0x01);
                lcd_escribir("Bateria:      mV");
                posicionar_cursor(3,7);
                lcd_escribir("Sensores");
            }

            copia_baterial = baterial;
            copia_bateriah = bateriah;
            bateria = ((int)bateriah * 256) + baterial;
            bateria=bateria*6;
            posicionar_cursor(1,10);
            escribir_int_decimal(bateria);

            posicionar_cursor(4,5);

            copia_sensoresd = sensoresi;
            copia_sensoresi= sensoresi;

            if(((copia_sensoresi)&(0x04)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x08)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x10)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x20)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x40)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x80)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');

            copia_sensoresd = sensoresd;
            copia_sensoresi= sensoresd;

//            posicionar_cursor(2,1);
//            escribir_valor_decimal(sensoresd);

            if(((copia_sensoresi)&(0x80)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x40)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x20)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x10)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x08)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');
            copia_sensoresi = copia_sensoresd;

            if(((copia_sensoresi)&(0x04)) != 0)
                lcd_escribir_c('0');
            else
                lcd_escribir_c('1');

        }break;

/*******************************************************************************************************/
        case 4:
        {
            if(pantalla_old != 4)
            {
                PORTL &= ~((1<<LED1)|(1<<LED2));
                PORTG &= ~(1<<LED3);
                PORTL |= (1<<LED1);
                PORTG |= (1<<LED3);
                pantalla_old=4;

                lcd_comando(0x01);
                lcd_escribir("Captura Servomotor.");
                posicionar_cursor(2,1);
                lcd_escribir(" A.I:     ADC:");
                posicionar_cursor(3,1);
                lcd_escribir(" A.F:     ADC:");
                posicionar_cursor(4,1);
                lcd_escribir(" Capturar y enviar.");
            }
//                posicionar_cursor(2,16);
//                escribir_direccion(direccionl, direccionh);

            switch(seleccion4)
            {
                case 1:
                {
                    posicionar_cursor(2,1);
                    lcd_escribir_c(0b01111110);
                    posicionar_cursor(3,1);
                    lcd_escribir_c(' ');
                    posicionar_cursor(4,1);
                    lcd_escribir_c(' ');

                    ainiciobajo = direccionl;
                    ainicioalto = direccionh;
                    posicion_i_fija = posicion;

                    posicionar_cursor(2,7);
                    escribir_angulo(ainiciobajo, ainicioalto);
                    posicionar_cursor(2,16);
                    escribir_direccion(ainiciobajo, ainicioalto);

                }break;

                case 2:
                {
                    posicionar_cursor(3,1);
                    lcd_escribir_c(0b01111110);
                    posicionar_cursor(2,1);
                    lcd_escribir_c(' ');
                    posicionar_cursor(4,1);
                    lcd_escribir_c(' ');

                    afinalbajo = direccionl;
                    afinalalto = direccionh;
                    posicion_f_fija = posicion;

                    posicionar_cursor(3,7);
                    escribir_angulo(afinalbajo, afinalalto);
                    posicionar_cursor(3,16);
                    escribir_direccion(afinalbajo, afinalalto);
                }break;

                case 3:
                {

                    posicionar_cursor(4,1);
                    lcd_escribir_c(0b01111110);
                    posicionar_cursor(3,1);
                    lcd_escribir_c(' ');
                    posicionar_cursor(2,1);
                    lcd_escribir_c(' ');

                    if(lanzar4 == 1)
                    {
                        posicion = posicion_i_fija;

                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);

                        for(i=0; i<500; i++)
                        {
                            pc_adc_alto[i] = 0;
                            pc_adc_bajo[i] = 0;                            
                        }

                        capturar=1;
                        posicion = posicion_f_fija;    //se manda el servo a la posicion final
                        _delay_ms(250);             //sampleamos en la interrupcion i2c
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);
                        _delay_ms(250);

                        capturar=0;
                        servo_a_pc();//llamada función que mande los datos al pc.
                        lanzar4 = 0;                
                    }

                }break;

                default:break;
            }


        }break;
/********************************************************************************************************/
        default:break;
    }
}
}

void inicializar_puertos(void)              //Configurar hardware del pic.
{
   //Configurar entradas y salidas.
   DDRC=0xFF;     //1111 1111
   PORTC=0x00;
      
   DDRA|=((1<<RS)|(1<<RW)|(1<<E));     //1110 0000
   PORTA &= ~(1<<E);  
    
   DDRL= 0xA0;      //1010 0000
   PORTL=0x00;

   DDRG =0x02;      //0000 0010
   PORTG=0x00;

   //Configurar interrupciones externas.
   PCICR |= (1<<PCINT2);
   PCMSK2 |= 0xFE;
   sei();

   //Configurar ADC
    ADMUX |=(1<<REFS0);                           //Tensión de referencia 5 voltios.
    ADMUX |=(1<<ADLAR);                           //8 bits de resolución.
                                                  //Canal ADC por defecto ADC0, no tenemos que cambiarlo.
    ADCSRA |=(1<<ADATE);                          //Habilitamos el Free Running Mode, por defecto seleccionado
    ADCSRA |=((1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0));  //Prescaler 128, Fadc 125 KHz @ 16MHz.
    DIDR0 |= ((1<<ADC0D)|(1<<ADC1D));             //Apagamos la parte digital del puerto.
    ADCSRA |= (1<<ADEN);                          //Encendemos ADC
    ADCSRA|= (1<<ADSC);                           //Comenzar conversiones.

    //Configurar I2C
    TWAR = 0xE2;                //Dirección de esclavo en el bus I2C.
    TWCR |= ((1<<TWEA)|(1<<TWEN)|(1<<TWIE));
}

void lcd_inicializar() //Se mandan los comandos de configuración según el datasheet.
{
    _delay_ms(100);                       
    lcd_comando(0x3C);    //Function Set RS0 RW0 0011NF** 0011 1100
    lcd_comando(0x3C);    //Function Set
    lcd_comando(0x0C);    //Display ON/OFF Control RS0 RW0 00001DCB 0000 1100
    lcd_comando(0x01);    //Display Clear RS0 RW0 00000001
    lcd_comando(0x06);    //Entry Mode Set RS0 RW0 000001I/DS 0000 0110
}

void lcd_escribir_c(char caracter)
{
    Bus_Control |= (1<<RS);        //RS1    Dato
    Bus_Control &= ~(1<<RW);  //RS0
    Bus_Datos = lcd_invertir_bits(caracter);       
    Bus_Control |= (1<<E);
    _delay_us(100);
    Bus_Control &= ~(1<<E);
    _delay_ms(1);
}

void lcd_comando(char comando)
{
    Bus_Control &= ~(1<<RS);    //RS0   Comando
    Bus_Control &= ~(1<<RW);   //RW0
    Bus_Datos  = lcd_invertir_bits(comando); //Comando
    Bus_Control |= (1<<E);
    _delay_us(100);
    Bus_Control &= ~(1<<E);
    _delay_ms(2);
}

void lcd_escribir(char *cadena)
{
    char *inicio = cadena;
    unsigned char i=0;

    for(i=0;((inicio[i]!='\0')&&(i<20));i++)
    {
        Bus_Control |= (1<<RS);    //RS0 RW0 caracter
        Bus_Control &= ~(1<<RW);
        Bus_Datos  = lcd_invertir_bits(inicio[i]);       
        Bus_Control |= (1<<E);
        _delay_us(100);
        Bus_Control &= ~(1<<E);
        _delay_ms(1);
    }
}

void posicionar_cursor(unsigned char x, unsigned char y) //línea x 1 a 4, carácter y de 1 a 20.
{                                                                                               
    unsigned char comando=DDRA_LINEA1;
    switch(x)
    {
        case 1:
        {
            comando=DDRA_LINEA1+y-1;
        }break;
        case 2:
        {
            comando=DDRA_LINEA2+y-1;
        }break;
        case 3:
        {
            comando=DDRA_LINEA3+y-1;
        }break;
        case 4:
        {
            comando=DDRA_LINEA4+y-1;
        }break;
        default:break;
    }
        lcd_comando(comando);
}

char lcd_invertir_bits(char caracter)
{
    char copia_caracter=caracter;
    char caracter_cambiado=0x00;

    caracter_cambiado |= (((copia_caracter)&(0x80))>>7);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x40))>>5);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x20))>>3);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x10))>>1);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x08))<<1);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x04))<<3);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x02))<<5);

    copia_caracter=caracter;
    caracter_cambiado |= (((copia_caracter)&(0x01))<<7);    
            
    return(caracter_cambiado);
}

void reseteo(void)
{

    PORTL |= ((1<<LED1)|(1<<LED2));
    PORTG |= (1<<LED3);
    _delay_ms(500);
    PORTL &= ~((1<<LED1)|(1<<LED2));
    PORTG &= ~(1<<LED3);
    _delay_ms(500);
    PORTL |= ((1<<LED1)|(1<<LED2));
    PORTG |= (1<<LED3);
    _delay_ms(500);
    PORTL &= ~((1<<LED1)|(1<<LED2));
    PORTG &= ~(1<<LED3);
    _delay_ms(500);
    PORTL |= ((1<<LED1)|(1<<LED2));
    PORTG |= (1<<LED3);
    _delay_ms(500);
    PORTL &= ~((1<<LED1)|(1<<LED2));
    PORTG &= ~(1<<LED3);
}

ISR(PCINT2_vect)
{

    if ((PINK&(1<<PINK1))==0)
        {
            botones=7;
            boton_libre=0;
        }

    else if ((PINK&(1<<PINK2))==0)
        {
            botones=6;
            boton_libre=0;
        }


    else if ((PINK&(1<<PINK3))==0)
        {
            botones=5;
            boton_libre=0;
        }


    else if ((PINK&(1<<PINK4))==0)
        {
            botones=4;
            boton_libre=0;
        }


    else if ((PINK&(1<<PINK5))==0)
        {
            botones=3;
            boton_libre=0;
        }


    else if ((PINK&(1<<PINK6))==0)
        {
            botones=2;
            boton_libre=0;
        }


    else if ((PINK&(1<<PINK7))==0)
        {
            botones=1;
            boton_libre=0;
        }

    PCIFR  |=(1<<PCIF2);
}

void escribir_valor_decimal(unsigned char registro) //Escribir binario a decimal.
{
    char centenas = 0;
    char decenas = 0;
    char unidades = 0;
    while(registro>=100)
    {
        registro= registro -100;
        centenas++;
    }
    while(registro>=10)
    {
        registro=registro -10;
        decenas++;
    }
    while(registro>0)
    {
        registro=registro-1;
        unidades++;
    }   
    lcd_escribir_c(centenas | 0b00110000);
    lcd_escribir_c(decenas | 0b00110000);
    lcd_escribir_c(unidades | 0b00110000);
}
/************************************************************************************/

ISR(TWI_vect)
{

    static unsigned char sincronizar;
    static unsigned char contador_byte;
    static unsigned int indice_pc;
    static unsigned char recibidos[9];
    unsigned char enviari2c[3];

    enviari2c[0] = posicion ;
    enviari2c[1] = velocidad;
    enviari2c[2] = r_estado;


    if((TWSR & 0xF8) == 0x60)
    {
        recibidos[contador_byte]=i2c_SR();
        contador_byte++;

        if(sincronizar<3)
            sincronizar=0;

    }
    else if((TWSR & 0xF8) == 0xA8)
    {
        i2c_ST(enviari2c[sincronizar]);
        sincronizar++;
        contador_byte=0;   
    }
    else
        i2c_error();

    if((sincronizar == 3)&&(contador_byte == 9))
    {
        baterial = recibidos[0];
        bateriah = recibidos[1];
        direccionl = recibidos[2];
        direccionh = recibidos[3];
        sensoresd = recibidos[4];
        sensoresi = recibidos[5];
        encoder1 = recibidos[6];
        encoder0 = recibidos[7];
        estadoleds = recibidos[8];
        sincronizar=0;
        
        if(capturar != 0)
        {
            pc_adc_alto[indice_pc] = direccionh;
            pc_adc_bajo[indice_pc] = direccionl;
            indice_pc = indice_pc +1;
            if(indice_pc > 250)
                indice_pc = 250;
        }
        else
            indice_pc = 0;
    }



    TWCR |= (1<<TWINT);
}

void i2c_ST(unsigned char dato)
{

    TWDR=dato;                        //Cargar el dato a enviar.
    TWCR &= ~((1<<TWSTO)|(1<<TWEA));
    TWCR |= (1<<TWINT);                //Enviar dato.

    while (!(TWCR & (1<<TWINT)))    //Esperar a TWINT
    {
    }

    if((TWSR & 0xF8) != 0xC8);        //Dato enviado, no esperamos ACK.
        i2c_error();

    TWCR &= ~((1<<TWSTO)|(1<<TWSTA));//Dejamos el módulo preparado.
    TWCR |= ((1<<TWINT)|(1<<TWEA));
}

unsigned char i2c_SR(void)
{
    unsigned char dato=0;

    TWCR &= ~(1<<TWSTO);                //Preparados para recibir dato y devolver ACK
    TWCR |= ((1<<TWINT)|(1<<TWEA));

    while (!(TWCR & (1<<TWINT)))        //Esperar a TWINT
    {
    }

    if((TWSR & 0xF8) != 0x80);            //Hemos recibido dato y generado ACK.
        i2c_error();
    dato=TWDR;                            //Almacenamos dato recibido.

    TWCR &= ~(1<<TWSTO);                //Preparados para recibir STOP
    TWCR |= ((1<<TWINT)|(1<<TWEA));

    while (!(TWCR & (1<<TWINT)))        //Esperar a TWINT
    {
    }

    if((TWSR & 0xF8) != 0xA0);            //STOP
        i2c_error();

    TWCR &= ~((1<<TWSTO)|(1<<TWSTA));    //Dejamos el módulo preparado.
    TWCR |= ((1<<TWINT)|(1<<TWEA));

    return(dato);
}

void i2c_error(void)
{
}

void escribir_direccion(unsigned char valorl, unsigned char valorh)
{

    unsigned int registro=0;
    registro= (int)valorh*256+valorl;
    char miles=0;
    char centenas = 0;
    char decenas = 0;
    char unidades = 0;


    while(registro>=1000)
    {
        registro= registro -1000;
        miles++;
    }

    while(registro>=100)
    {
        registro= registro -100;
        centenas++;
    }

    while(registro>=10)
    {
        registro= registro -10;
        decenas++;
    }

    while(registro>=1)
    {
        registro= registro -1;
        unidades++;
    }
 
    lcd_escribir_c(miles | 0b00110000);
    lcd_escribir_c(centenas | 0b00110000);
    lcd_escribir_c(decenas | 0b00110000);
    lcd_escribir_c(unidades | 0b00110000);
}

void escribir_int_decimal(unsigned int registro)
{
    char miles=0;
    char centenas = 0;
    char decenas = 0;
    char unidades = 0;

    while(registro>=1000)
    {
        registro= registro -1000;
        miles++;
    }

    while(registro>=100)
    {
        registro= registro -100;
        centenas++;
    }
    while(registro>=10)
    {
        registro=registro -10;
        decenas++;
    }
    while(registro>0)
    {
        registro=registro-1;
        unidades++;
    }   
    lcd_escribir_c(miles | 0b00110000);
    lcd_escribir_c(centenas | 0b00110000);
    lcd_escribir_c(decenas | 0b00110000);
    lcd_escribir_c(unidades | 0b00110000);
}

char escribir_angulo(unsigned char adcbajo, unsigned char adcalto)
{
    unsigned int registro=0;
    char angulo = 0;
    char decenas=0;
    char unidades=0;

    registro= (int)adcalto * 256 + adcbajo;

    if(registro >= 885)
        angulo=100;
    else if(registro >= 867)
        angulo = 22;
    else if(registro >= 866)
        angulo = 21;
    else if(registro >= 865)
        angulo = 20;
    else if(registro >= 863)
        angulo = 19;
    else if(registro >= 861)
        angulo = 18;
    else if(registro >= 860)
        angulo = 17;
    else if(registro >= 858)
        angulo = 16;
    else if(registro >= 857)
        angulo = 15;
    else if(registro >= 855)
        angulo = 14;
    else if(registro >= 853)
        angulo = 13;
    else if(registro >= 852)
        angulo = 12;
    else if(registro >= 850)
        angulo = 11;
    else if(registro >= 848)
        angulo = 10;
    else if(registro >= 846)
        angulo = 9;
    else if(registro >= 844)
        angulo = 8;
    else if(registro >= 842)
        angulo = 7;
    else if(registro >= 840)
        angulo = 6;
    else if(registro >= 838)
        angulo = 5;
    else if(registro >= 836)
        angulo = 4;
    else if(registro >= 834)
        angulo = 3;
    else if(registro >= 832)
        angulo = 2;
    else if(registro >= 830)
        angulo = 1;
    else if(registro >= 828)
        angulo = 0;
    else if(registro >= 825)
        angulo = -1;
    else if(registro >= 823)
        angulo = -2;
    else if(registro >= 821)
        angulo = -3;
    else if(registro >= 818)
        angulo = -4;
    else if(registro >= 816)
        angulo = -5;
    else if(registro >= 813)
        angulo = -6;
    else if(registro >= 810)
        angulo = -7;
    else if(registro >= 808)
        angulo = -8;
    else if(registro >= 805)
        angulo = -9;
    else if(registro >= 802)
        angulo = -10;
    else if(registro >= 799)
        angulo = -11;
    else if(registro >= 796)
        angulo = -12;
    else if(registro >= 793)
        angulo = -13;
    else if(registro >= 790)
        angulo = -14;
    else if(registro >= 786)
        angulo = -15;
    else if(registro >= 783)
        angulo = -16;
    else if(registro >= 780)
        angulo = -17;
    else if(registro >= 776)
        angulo = -18;
    else if(registro >= 772)
        angulo = -19;
    else if(registro >= 769)
        angulo = -20;
    else if(registro >= 765)
        angulo = -21;
    else if(registro >= 761)
        angulo = -22;
    else if(registro >= 740)
        angulo = -22;
    else
        angulo = 100;

    if(angulo == 100)
    {
        lcd_escribir("err");
        return(0);
    }

    if(angulo<0)
    {
        lcd_escribir("-");
        angulo = angulo * (-1);
    }
    else
        lcd_escribir("+");

    while(angulo>=10)
    {
        angulo= angulo -10;
        decenas++;
    }

    while(angulo>=1)
    {
        angulo= angulo -1;
        unidades++;
    }

    lcd_escribir_c(decenas | 0b00110000);
    lcd_escribir_c(unidades | 0b00110000);

    return(0);    
}

void servo_a_pc(void)
{
    unsigned int i = 0;
    unsigned char datoh=0;
    unsigned char datol=0;
    unsigned int registro=0;
    char miles=0;
    char centenas = 0;
    char decenas = 0;
    char unidades = 0;
    unsigned char dato=0;

    //Se configura la USART
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    // Habilitar emisor y receptor
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);
    // Configuración: 8data, 2stop bit
    UCSR0C = (1<<USBS0)|(3<<UCSZ00)|(1<<UCSZ01);

    cli();

    for(i=0;i<250;i++)
    {

        miles = 0;
        centenas = 0;
        decenas = 0;
        unidades = 0;

        datoh = pc_adc_alto[i];
        datol = pc_adc_bajo[i];
        registro= (int)datoh * 256 + datol;

        while(registro>=1000)
        {
            registro= registro -1000;
            miles++;
        }

        while(registro>=100)
        {
            registro= registro -100;
            centenas++;
        }
        while(registro>=10)
        {
            registro=registro -10;
            decenas++;
        }
        while(registro>0)
        {
            registro=registro-1;
            unidades++;
        }   

        dato = (miles | 0b00110000);
       // Esperar a que se transmita el dato
        while ( !( UCSR0A & (1<<UDRE0)) );
        // Dato a transmitir.
        UDR0 = dato;

        
        dato = (centenas | 0b00110000);
       // Esperar a que se transmita el dato
        while ( !( UCSR0A & (1<<UDRE0)) );
        // Dato a transmitir.
        UDR0 = dato;

        dato = (decenas | 0b00110000);
       // Esperar a que se transmita el dato
        while ( !( UCSR0A & (1<<UDRE0)) );
        // Dato a transmitir.
        UDR0 = dato;

        dato = (unidades | 0b00110000);
       // Esperar a que se transmita el dato
        while ( !( UCSR0A & (1<<UDRE0)) );
        // Dato a transmitir.
        UDR0 = dato;

        dato = 0x0a;
        while ( !( UCSR0A & (1<<UDRE0)) );
        // Dato a transmitir.
        UDR0 = dato;
    }

    UCSR0B &= ~((1<<RXEN0)|(1<<TXEN0));
    sei();
}