//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();
}