Este robot es una combinación del robot con sensor TPA81 y del robot explorador. Además de guiarse de una forma autónoma es capaz de distinguir cuando el obstáculo con el que se encuentra se trata de una persona y reacciona de forma diferente.
En el primero de los casos, se ha añadido un sensor TPA81 más. Uno para detectar los pixeles de temperatura verticales y el otro para los pixeles horizontales. Empleando dos servos combinados mecanicamente conseguimos el giro de la cabeza en todas las direcciones en el espacio y con cada servo controlado por su correspondiente TPA81. En el caso segundo, se ha sustituido el control de los servos mediante código por el control por la salida propia para esta función de los TPA81.
La primera acción del robot es comprobar si tiene presencia humana (o fuente de calor) en el frente. Si la detecta posiciona la cabeza en el espacio dirigiendola hacia el objetivo. A continuación se posicionará frente a la fuente a una distancia entre 0,8 m y 1,3 m . avanzando o retrocediendo.
Cuando la fuente o presencia humana desaparece, el robot comenzará el modo exploración avanzando y sorteando los obstáculos que encuentre en el camino hasta volver a encontrar una presencia humana o fuente de calor.
El sensor ultrasónico con el que detecta los obstáculos, tiene el inconveniente de que cuando el obstáculo esta muy ladeado, el rebote de las ondas ultrasónicas no pueden ser leidas. Para evitar la colisión en estos casos se empean dos sensores ópticos modelo Sharp-GP2Y0D810Z0F que detectan el obstáculo a una distancia inferior a 10cm y mediante el tramo de programa creado para este caso alinean al robot con el obstáculo o pared lateral.
pulsar en la imagen para ampliarla
////////////////////////////////////////////////////////////////////////////////
// //
// ROBOT //
// //
// Robot avanza sin chocar con obstáculos y reacciona frente a presencia //
// humana y fuentes de calor //
// //
// (c) RobotyPic 2010 //
// //
////////////////////////////////////////////////////////////////////////////////
#include <16F876A.h>
#fuses NOWDT, XT, NOPROTECT
#use delay(clock=4000000) //Frecuencia del cristal oscilador 4MHz
#use i2c(master, SCL=PIN_C3, SDA=PIN_C4) //Configuración comunicación I2C
#use i2c(master, SCL=PIN_C3, SDA=PIN_C4) //Configuración comunicación I2C
#byte PIR1=0x0C
#byte trisa=0x85
#byte porta=0x05
#byte trisb=0x86
#byte portb=0x06
#byte trisc=0x87
#byte portc=0x07
#byte porta=0x05
#byte trisb=0x86
#byte portb=0x06
#byte trisc=0x87
#byte portc=0x07
#define BIT_TEST_ON output_high(PIN_C0)
#define BIT_TEST_OFF output_low(PIN_C0)
#define BIT_TEST_OFF output_low(PIN_C0)
#define AVANCE output_high(PIN_B1),output_high(PIN_B3),output_low(PIN_B2),output_low(PIN_B4)
#define RETROCESO output_low(PIN_B1),output_low(PIN_B3),output_high(PIN_B2),output_high(PIN_B4)
#define PARO output_low(PIN_B1),output_low(PIN_B3),output_low(PIN_B2),output_low(PIN_B4)
#define GIRO_DERECHA output_high(PIN_B1),output_low(PIN_B3),output_low(PIN_B2),output_high(PIN_B4)
#define GIRO_IZQUIERDA output_low(PIN_B1),output_high(PIN_B3),output_high(PIN_B2),output_low(PIN_B4)
#define RETROCESO output_low(PIN_B1),output_low(PIN_B3),output_high(PIN_B2),output_high(PIN_B4)
#define PARO output_low(PIN_B1),output_low(PIN_B3),output_low(PIN_B2),output_low(PIN_B4)
#define GIRO_DERECHA output_high(PIN_B1),output_low(PIN_B3),output_low(PIN_B2),output_high(PIN_B4)
#define GIRO_IZQUIERDA output_low(PIN_B1),output_high(PIN_B3),output_high(PIN_B2),output_low(PIN_B4)
#define BUMPER_DER bit_test(porta,4)
#define BUMPER_IZQ bit_test(porta,5)
#define BUMPER_IZQ bit_test(porta,5)
#bit ojo_derecho = PORTB.7 //Bit 7 puerto B ojo derecho
#bit ojo_izquierdo = PORTB.6 //Bit 6 puerto B ojo izquierdo
#bit ojo_izquierdo = PORTB.6 //Bit 6 puerto B ojo izquierdo
/********************** Prototipos de las funciones ***************************/
void main (void); //función principal
void pulso_test (void); //da pulso y calcula distancia al obstáculo
void ccp2_int (void); //mide pulso de eco ultrasonidos
void navegacion (void); //avance y detección obstáculos por ultrasonidos
void bumper (void); //avance y detección obstáculos por bumpers
void servo_tpa81 (void); //establece posiciones de servos de los TPA81
void funcion_TPA81(void); //reacción frente a estímulos de los TPA81
void lectura_tpa81(void); //lectura valores tª de los TPA81
void movimiento_TPA81(void); //Avance o retroceso segun distancia al foco
void pulso_test (void); //da pulso y calcula distancia al obstáculo
void ccp2_int (void); //mide pulso de eco ultrasonidos
void navegacion (void); //avance y detección obstáculos por ultrasonidos
void bumper (void); //avance y detección obstáculos por bumpers
void servo_tpa81 (void); //establece posiciones de servos de los TPA81
void funcion_TPA81(void); //reacción frente a estímulos de los TPA81
void lectura_tpa81(void); //lectura valores tª de los TPA81
void movimiento_TPA81(void); //Avance o retroceso segun distancia al foco
/******************** Variables para lectura obstáculo ************************/
int1 nuevopulso=0; //Entra otro pulso de lectura
int16 TFB=0,TFS=0,TF=0; //Tiempo flancos
float AP=0.0; //Valor del pulso del obstáculo en microsegundos
int16 distancia=0; //Valor distancia al obstáculo
int16 distancia_derecha=0; //Valor distancia al obstáculo de la derecha
int16 distancia_izquierda=0; //Valor distancia al obstáculo de la izquierda
int1 cambio=0; //Control flanco subida o bajada del impulso leido
int16 TFB=0,TFS=0,TF=0; //Tiempo flancos
float AP=0.0; //Valor del pulso del obstáculo en microsegundos
int16 distancia=0; //Valor distancia al obstáculo
int16 distancia_derecha=0; //Valor distancia al obstáculo de la derecha
int16 distancia_izquierda=0; //Valor distancia al obstáculo de la izquierda
int1 cambio=0; //Control flanco subida o bajada del impulso leido
/********* Variables para lectura tª y control servos de los TPA81 ************/
int TPA81_h_ID = 0xD0; //Dirección I2C de acceso al TPA81
int TPA81_v_ID = 0xD2; //Dirección I2C de acceso al TPA81 vertical
int b[10]; //Buffer para datos lectura TPA81
int servo_h; //Posición del servo horizontal
int servo_v; //Posición del servo vertical
int i; //Posiciones para Buffer de datos temperatura
int media_a; //El valor medio de b[2] b[3] y b[4]
int media_b; //El valor medio de b[7] b[8] y b[9]
int condicion; //Condición para girar sensor térmico
short int flag_calor_h=0; //Con 1 se ha detectado fuente de calor
short int flag_calor_v=0; //Con 1 se ha detectado fuente de calor
short int flag_presencia_h=0; //Con 1 se ha detectado presencia
short int flag_presencia_v=0; //Con 1 se ha detectado presencia
int flag_h=0;
int flag_v=0;
int repeticiones; //Número de repeticiones de función
int TPA81_v_ID = 0xD2; //Dirección I2C de acceso al TPA81 vertical
int b[10]; //Buffer para datos lectura TPA81
int servo_h; //Posición del servo horizontal
int servo_v; //Posición del servo vertical
int i; //Posiciones para Buffer de datos temperatura
int media_a; //El valor medio de b[2] b[3] y b[4]
int media_b; //El valor medio de b[7] b[8] y b[9]
int condicion; //Condición para girar sensor térmico
short int flag_calor_h=0; //Con 1 se ha detectado fuente de calor
short int flag_calor_v=0; //Con 1 se ha detectado fuente de calor
short int flag_presencia_h=0; //Con 1 se ha detectado presencia
short int flag_presencia_v=0; //Con 1 se ha detectado presencia
int flag_h=0;
int flag_v=0;
int repeticiones; //Número de repeticiones de función
/******************************************************************************/
/********************* FUNCIÓN CONTROL DE LOS SERVOS **************************/
/*********** Control de las posiciones de los servos con tpa81 ****************/
/********************* FUNCIÓN CONTROL DE LOS SERVOS **************************/
/*********** Control de las posiciones de los servos con tpa81 ****************/
void servo_tpa81 ( byte slaveID, byte servo_pos ) {
i2c_start(); //Comienzo de la comunicación I2C ...
i2c_write(slaveID); //...con la dirección del TPA81...
i2c_write(0); //...apuntando a la posición 0 del registro del TPA81
i2c_write(servo_pos&0x1F); //escribe posición del servo
i2c_stop (); //Finalización de la transmisión
}
i2c_write(slaveID); //...con la dirección del TPA81...
i2c_write(0); //...apuntando a la posición 0 del registro del TPA81
i2c_write(servo_pos&0x1F); //escribe posición del servo
i2c_stop (); //Finalización de la transmisión
}
/******************************************************************************/
/******************* FUNCIÓN DE LECTURA DEL SENSOR TPA81 **********************/
/************ Carga valores de temperatura leídos en el buffer b[] ************/
/******************* FUNCIÓN DE LECTURA DEL SENSOR TPA81 **********************/
/************ Carga valores de temperatura leídos en el buffer b[] ************/
void lectura_tpa81( byte slaveID ) {
for ( i=0; i<10; i++) { //Contador de posiciones del buffer b[]
i2c_start(); //Comienzo de la comunicación I2C ...
i2c_write(slaveID); //...con la dirección del TPA81...
i2c_write(i); //...apuntando a la dirección (i) del registro
i2c_start(); //Reinicio
i2c_write(slaveID+1); //Cambio a función de lectura
b[i] = i2c_read(0); //Carga buffer b[] con datos leídos del TPA81
i2c_stop (); //Finalización de la transmisión
delay_ms(10);
}
}
i2c_write(slaveID); //...con la dirección del TPA81...
i2c_write(i); //...apuntando a la dirección (i) del registro
i2c_start(); //Reinicio
i2c_write(slaveID+1); //Cambio a función de lectura
b[i] = i2c_read(0); //Carga buffer b[] con datos leídos del TPA81
i2c_stop (); //Finalización de la transmisión
delay_ms(10);
}
}
/******************************************************************************/
/*********************** FUNCIÓN LECTURA DISTANCIA ****************************/
/** Da pulso test y dato obtenido por interrupción ccp2_int lo traduce a cms.**/
/*********************** FUNCIÓN LECTURA DISTANCIA ****************************/
/** Da pulso test y dato obtenido por interrupción ccp2_int lo traduce a cms.**/
void pulso_test(){
enable_interrupts(INT_CCP2); //Habilitación interrupción para medida de pulso eco
BIT_TEST_ON; //salida de pulso test durante 10us por Bit 0 puerto C
delay_us(10);
BIT_TEST_OFF;
while(nuevopulso==0) //Espera a finalizar pulso eco (interrupción ccp2_int)
{}
if(nuevopulso==1){ //Finalizado el pulso eco se calcula su valor.
TF=(TFB-TFS); //Valor entre pulso de subida y bajada.
AP=TF*1.0; //Valor pulso leido en us(obstáculo) de 100u a 25ms
AP = AP/58; //Distancia del obstáculo en cm
distancia =(int16)AP; //paso de flotante a entero largo
nuevopulso=0; //Listo para recibir nuevo pulso
}
disable_interrupts(INT_CCP2);
}
BIT_TEST_ON; //salida de pulso test durante 10us por Bit 0 puerto C
delay_us(10);
BIT_TEST_OFF;
while(nuevopulso==0) //Espera a finalizar pulso eco (interrupción ccp2_int)
{}
if(nuevopulso==1){ //Finalizado el pulso eco se calcula su valor.
TF=(TFB-TFS); //Valor entre pulso de subida y bajada.
AP=TF*1.0; //Valor pulso leido en us(obstáculo) de 100u a 25ms
AP = AP/58; //Distancia del obstáculo en cm
distancia =(int16)AP; //paso de flotante a entero largo
nuevopulso=0; //Listo para recibir nuevo pulso
}
disable_interrupts(INT_CCP2);
}
/******************************************************************************/
/************************ FUNCIÓN RESPUESTA SEGÚN TPA81 ***********************/
/************ Reacciona frente a los estímulos del sensor TPA81 ***************/
/************************ FUNCIÓN RESPUESTA SEGÚN TPA81 ***********************/
/************ Reacciona frente a los estímulos del sensor TPA81 ***************/
void funcion_TPA81() {
delay_ms(10);
lectura_tpa81( TPA81_h_ID ); //Lectura valores temperatura horizontales
condicion = ((b[1]+b[5]+b[6])/3);
//obtine la media de entre b[2], b[3], b[4] y Tª ambiente
media_a=(b[1]+b[2]+b[3]+b[4])/4;
//obtiene la media de entre b[7], b[8], b[9] y Tª ambiente
media_b=((b[1]+b[7]+b[8]+b[9])/4);
//Comprueba si debe girar a la izquierda
if (media_a > condicion){ //Si temp. a izquierda es mayor que central...
if (media_a > media_b){ //...y además es mayor que la derecha...
servo_h=servo_h+1; //... entonces gira el servo una posición
if (servo_h>=30) { //Si servo llega al final de su giro...
servo_h=16; //...lo deja en 0º ...
servo_tpa81 ( TPA81_h_ID, servo_h );
GIRO_IZQUIERDA; //... y gira el cuerpo a la izquierda
delay_ms(800);
PARO;
}
else servo_tpa81 ( TPA81_h_ID, servo_h ); //Sino sólo gira el servo
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si debe girar a la derecha
if (media_b > condicion){ //Si temp. a derecha es mayor que central...
if (media_b > media_a){
servo_h=servo_h-1; //... entonces gira el servo una posición
if (servo_h<=1){ //Si servo llega al final de su giro...
servo_h=16; //...lo deja en 0º ...
servo_tpa81 (TPA81_h_ID, servo_h);
GIRO_DERECHA; //... y gira el cuerpo a la derecha
delay_ms(800);
PARO;
}
else servo_tpa81 ( TPA81_h_ID, servo_h ); //Sino sólo gira el servo
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si algun pixel horizontal mide temperatura > 28ºC (presencia)
flag_presencia_h=0; //Reinicia el flag deteccion_h
delay_ms(10);
lectura_tpa81( TPA81_h_ID ); //Lectura valores temperatura horizontales
condicion = ((b[1]+b[5]+b[6])/3);
//obtine la media de entre b[2], b[3], b[4] y Tª ambiente
media_a=(b[1]+b[2]+b[3]+b[4])/4;
//obtiene la media de entre b[7], b[8], b[9] y Tª ambiente
media_b=((b[1]+b[7]+b[8]+b[9])/4);
//Comprueba si debe girar a la izquierda
if (media_a > condicion){ //Si temp. a izquierda es mayor que central...
if (media_a > media_b){ //...y además es mayor que la derecha...
servo_h=servo_h+1; //... entonces gira el servo una posición
if (servo_h>=30) { //Si servo llega al final de su giro...
servo_h=16; //...lo deja en 0º ...
servo_tpa81 ( TPA81_h_ID, servo_h );
GIRO_IZQUIERDA; //... y gira el cuerpo a la izquierda
delay_ms(800);
PARO;
}
else servo_tpa81 ( TPA81_h_ID, servo_h ); //Sino sólo gira el servo
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si debe girar a la derecha
if (media_b > condicion){ //Si temp. a derecha es mayor que central...
if (media_b > media_a){
servo_h=servo_h-1; //... entonces gira el servo una posición
if (servo_h<=1){ //Si servo llega al final de su giro...
servo_h=16; //...lo deja en 0º ...
servo_tpa81 (TPA81_h_ID, servo_h);
GIRO_DERECHA; //... y gira el cuerpo a la derecha
delay_ms(800);
PARO;
}
else servo_tpa81 ( TPA81_h_ID, servo_h ); //Sino sólo gira el servo
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si algun pixel horizontal mide temperatura > 28ºC (presencia)
flag_presencia_h=0; //Reinicia el flag deteccion_h
for (i=1; i<10; i++){
if (b[i]>28) flag_presencia_h=1;
}
if (condicion >= (media_b&&media_a)) flag_calor_h=1; //Flag calor_h=1 cuando detecta foco en el pixel central
else flag_calor_h=0;
lectura_tpa81( TPA81_v_ID ); //Lectura valores temperatura vertical
condicion = ((b[1]+b[6])/2);
//obtine la media de entre b[2], b[3], b[4] y Tª ambiente
media_a=(b[1]+b[2]+b[3]+b[4]+b[5])/5;
//obtiene la media de entre b[7], b[8], b[9] y Tª ambiente
media_b=((b[1]+b[7]+b[8]+b[9])/4);
//Comprueba si debe subir una posición
if (media_a > condicion){ //Si temp. a izquierda es mayor que central...
if (media_a > media_b){ //...y además es mayor que la derecha...
servo_v=servo_v-1; //... entonces gira el servo una posición
if (servo_v<=5) servo_v=5; //Si servo llega al final de su giro lo deja en misma posición
servo_tpa81 ( TPA81_v_ID, servo_v );
}
}
else {
flag_h=flag_v+1;
delay_ms(5);
}
//Comprueba si la cabeza debe bajar una posición
if (media_b > condicion){ //Si temp. a derecha es mayor que central...
if (media_b > media_a){
servo_v=servo_v+1; //... entonces gira el servo una posición
if (servo_v>=20) servo_v=20; //Si servo llega al final de su giro lo deja en misma posición
servo_tpa81 ( TPA81_v_ID, servo_v );
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si algun pixel vertical mide temperatura > 28ºC (presencia)d
flag_presencia_v=0; //Reinicia el flag deteccion_v
if (b[i]>28) flag_presencia_h=1;
}
if (condicion >= (media_b&&media_a)) flag_calor_h=1; //Flag calor_h=1 cuando detecta foco en el pixel central
else flag_calor_h=0;
lectura_tpa81( TPA81_v_ID ); //Lectura valores temperatura vertical
condicion = ((b[1]+b[6])/2);
//obtine la media de entre b[2], b[3], b[4] y Tª ambiente
media_a=(b[1]+b[2]+b[3]+b[4]+b[5])/5;
//obtiene la media de entre b[7], b[8], b[9] y Tª ambiente
media_b=((b[1]+b[7]+b[8]+b[9])/4);
//Comprueba si debe subir una posición
if (media_a > condicion){ //Si temp. a izquierda es mayor que central...
if (media_a > media_b){ //...y además es mayor que la derecha...
servo_v=servo_v-1; //... entonces gira el servo una posición
if (servo_v<=5) servo_v=5; //Si servo llega al final de su giro lo deja en misma posición
servo_tpa81 ( TPA81_v_ID, servo_v );
}
}
else {
flag_h=flag_v+1;
delay_ms(5);
}
//Comprueba si la cabeza debe bajar una posición
if (media_b > condicion){ //Si temp. a derecha es mayor que central...
if (media_b > media_a){
servo_v=servo_v+1; //... entonces gira el servo una posición
if (servo_v>=20) servo_v=20; //Si servo llega al final de su giro lo deja en misma posición
servo_tpa81 ( TPA81_v_ID, servo_v );
}
}
else {
flag_h=flag_h+1;
delay_ms(5);
}
//Comprueba si algun pixel vertical mide temperatura > 28ºC (presencia)d
flag_presencia_v=0; //Reinicia el flag deteccion_v
for (i=1; i<10; i++){
if (b[i]>26) flag_presencia_v=1;
}
if (condicion >= (media_b&&media_a)) flag_calor_v=1; //Flag calor_v=1 cuando detecta foco en el pixel central
else flag_calor_v=0;
}
if (b[i]>26) flag_presencia_v=1;
}
if (condicion >= (media_b&&media_a)) flag_calor_v=1; //Flag calor_v=1 cuando detecta foco en el pixel central
else flag_calor_v=0;
}
/******************************************************************************/
/************************ FUNCIÓN CALCULO DE LA DISTANCIA *********************/
#int_ccp2 //LLamada por interrupción flanco en RC2
void ccp2_int(){
/************************ FUNCIÓN CALCULO DE LA DISTANCIA *********************/
#int_ccp2 //LLamada por interrupción flanco en RC2
void ccp2_int(){
if(cambio==0){ //Si es flanco de subida...
TFS=CCP_2; //Carga en valor flanco subida valor registro ccpr1
setup_ccp2(CCP_CAPTURE_FE); //Configuración modo captura en flanco de bajada
cambio=1; //Próximo flanco debe ser de bajada
}
else{ //Si es flanco de bajada...
TFB=CCP_2; //Carga en valor flanco bajada valor registro ccpr1
setup_ccp2(CCP_CAPTURE_RE); //Configuración modo captura en flanco de subida
cambio=0; //Próximo flanco debe ser de subida
if(nuevopulso==0) //Fin de pulso...
nuevopulso=1; //Pulso finalizado.
}
}
TFS=CCP_2; //Carga en valor flanco subida valor registro ccpr1
setup_ccp2(CCP_CAPTURE_FE); //Configuración modo captura en flanco de bajada
cambio=1; //Próximo flanco debe ser de bajada
}
else{ //Si es flanco de bajada...
TFB=CCP_2; //Carga en valor flanco bajada valor registro ccpr1
setup_ccp2(CCP_CAPTURE_RE); //Configuración modo captura en flanco de subida
cambio=0; //Próximo flanco debe ser de subida
if(nuevopulso==0) //Fin de pulso...
nuevopulso=1; //Pulso finalizado.
}
}
/******************************************************************************/
/************************ FUNCIÓN LECTURA BUMPER ******************************/
/************************ FUNCIÓN LECTURA BUMPER ******************************/
void bumper(){
while (!BUMPER_DER) GIRO_IZQUIERDA; //Cuando detecte bumper derecho
while (!BUMPER_IZQ) GIRO_DERECHA; //Cuando detecte bumper izquierdo
AVANCE;
}
while (!BUMPER_IZQ) GIRO_DERECHA; //Cuando detecte bumper izquierdo
AVANCE;
}
/******************************************************************************/
/********************* FUNCIÓN AVANCE - DETECCIÓN OBSTÁCULO *******************/
/********************* FUNCIÓN AVANCE - DETECCIÓN OBSTÁCULO *******************/
void navegacion(){
delay_ms(500); //Tiempo entre lecturas de distancia
pulso_test();
if (distancia>30){ //Si distancia >30cm avanza
AVANCE;
Bumper(); //Comprueba obstáculos laterales en el avance
}
else{
PARO;
servo_h=1; //Posición servo -90º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
delay_ms(700); //Tiempo servo en -90º
pulso_test();
distancia_derecha=distancia;
servo_h=16; //Posición servo 0º
servo_tpa81 ( TPA81_h_ID, servo_h); //Manda posición del servo a TPA81
delay_ms (300); //Tiempo servo 0º
servo_h=30; //Posición servo +90º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
delay_ms(700); //Tiempo servo +90º
pulso_test();
distancia_izquierda=distancia;
servo_h=16; //Posición servo 0º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
if (distancia_derecha>distancia_izquierda){
GIRO_DERECHA;
delay_ms(500);
}
else{
GIRO_IZQUIERDA;
delay_ms(500);
}
}
}
pulso_test();
if (distancia>30){ //Si distancia >30cm avanza
AVANCE;
Bumper(); //Comprueba obstáculos laterales en el avance
}
else{
PARO;
servo_h=1; //Posición servo -90º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
delay_ms(700); //Tiempo servo en -90º
pulso_test();
distancia_derecha=distancia;
servo_h=16; //Posición servo 0º
servo_tpa81 ( TPA81_h_ID, servo_h); //Manda posición del servo a TPA81
delay_ms (300); //Tiempo servo 0º
servo_h=30; //Posición servo +90º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
delay_ms(700); //Tiempo servo +90º
pulso_test();
distancia_izquierda=distancia;
servo_h=16; //Posición servo 0º
servo_tpa81 ( TPA81_h_ID, servo_h ); //Manda posición del servo a TPA81
if (distancia_derecha>distancia_izquierda){
GIRO_DERECHA;
delay_ms(500);
}
else{
GIRO_IZQUIERDA;
delay_ms(500);
}
}
}
/******************************************************************************/
/*********************** FUNCIÓN MOVIMIENTO TPA81 *****************************/
/*********************** FUNCIÓN MOVIMIENTO TPA81 *****************************/
void movimiento_TPA81 () {
//Se mantendrá entre 1,5 m y 2,5 m de la fuente de calor
if ((flag_calor_v && flag_calor_h)==1){
pulso_test(); //Comprueba distancia a la fuente
delay_ms(300);
if (distancia>130){
//Antes de avanzar se posiciona frente a la fuente de calor tomando
//como referencia el ángulo de giro del servo del cuello horizontal
if (servo_h<14){
GIRO_DERECHA;
delay_ms(((16-servo_h)*60));
servo_h=16; //Deja el servo en posición 0º
servo_tpa81 ( TPA81_h_ID, servo_h );
}
if (servo_h>18){
GIRO_IZQUIERDA;
delay_ms(((servo_h-16)*60));
servo_h=16; //Deja el servo en posición 0º
servo_tpa81 ( TPA81_h_ID, servo_h );
}
AVANCE; //Con distancia a objeto > 1,3m avanza...
while (distancia>130) { //...hasta estar a 1,3m del objeto...
bumper(); //...usando bumpers
delay_ms(300);
pulso_test(); //Revisa distancia a la fuente
}
PARO; //Acercado ya hasta 1'3m, se para
}
if (distancia<80){ //Si distancia a objeto < 0,8m ...
RETROCESO; // ...retrocede...
while (distancia<80) { // ...hasta estar a 0,8m del objeto
pulso_test(); //Revisa distancia al objeto
delay_ms(300);
}
PARO; //Separado ya a 0'8m, se para
}
if (80<distancia<130) PARO; //Entre 0,8m y 1,3m se para.
}
}
if ((flag_calor_v && flag_calor_h)==1){
pulso_test(); //Comprueba distancia a la fuente
delay_ms(300);
if (distancia>130){
//Antes de avanzar se posiciona frente a la fuente de calor tomando
//como referencia el ángulo de giro del servo del cuello horizontal
if (servo_h<14){
GIRO_DERECHA;
delay_ms(((16-servo_h)*60));
servo_h=16; //Deja el servo en posición 0º
servo_tpa81 ( TPA81_h_ID, servo_h );
}
if (servo_h>18){
GIRO_IZQUIERDA;
delay_ms(((servo_h-16)*60));
servo_h=16; //Deja el servo en posición 0º
servo_tpa81 ( TPA81_h_ID, servo_h );
}
AVANCE; //Con distancia a objeto > 1,3m avanza...
while (distancia>130) { //...hasta estar a 1,3m del objeto...
bumper(); //...usando bumpers
delay_ms(300);
pulso_test(); //Revisa distancia a la fuente
}
PARO; //Acercado ya hasta 1'3m, se para
}
if (distancia<80){ //Si distancia a objeto < 0,8m ...
RETROCESO; // ...retrocede...
while (distancia<80) { // ...hasta estar a 0,8m del objeto
pulso_test(); //Revisa distancia al objeto
delay_ms(300);
}
PARO; //Separado ya a 0'8m, se para
}
if (80<distancia<130) PARO; //Entre 0,8m y 1,3m se para.
}
}
/******************************************************************************/
/*************************** FUNCIÓN PRINCIPAL ********************************/
/*************************** FUNCIÓN PRINCIPAL ********************************/
void main(){
trisa=0xff;
trisb=0x00; //Puerto B todo salidas
trisc=0b00000100; //Puerto C definición de entradas y salidas
servo_h=16; //Posición 0º del servo horizontal
servo_v=14; //Posición 0º del servo vertical
PARO; //Inicia con el robot parado
setup_timer_1(T1_INTERNAL); //Configuración timer1 para lectura obstáculo
setup_ccp2(CCP_CAPTURE_RE); //Configuración modo captura en flanco de subida
disable_interrupts(INT_TIMER1);
disable_interrupts(INT_CCP2); //Deshabilitación interrupción modo comparación
enable_interrupts (GLOBAL);
ojo_derecho=1;
ojo_izquierdo=1;
delay_ms(2000); //Estabilización en el arranque del sistema
trisb=0x00; //Puerto B todo salidas
trisc=0b00000100; //Puerto C definición de entradas y salidas
servo_h=16; //Posición 0º del servo horizontal
servo_v=14; //Posición 0º del servo vertical
PARO; //Inicia con el robot parado
setup_timer_1(T1_INTERNAL); //Configuración timer1 para lectura obstáculo
setup_ccp2(CCP_CAPTURE_RE); //Configuración modo captura en flanco de subida
disable_interrupts(INT_TIMER1);
disable_interrupts(INT_CCP2); //Deshabilitación interrupción modo comparación
enable_interrupts (GLOBAL);
ojo_derecho=1;
ojo_izquierdo=1;
delay_ms(2000); //Estabilización en el arranque del sistema
while (1){
funcion_TPA81(); //Salto a la función del TPA
if (flag_presencia_v==1||flag_presencia_h==1) { //Si detecta presencia...
PARO;
for(repeticiones=0;repeticiones<6;repeticiones++){//nºmovimientos para posicionar la cabeza
funcion_TPA81(); //Salto a la función del TPA
}
movimiento_TPA81(); //Avanzar o retroceder segun distancia
}
else {
servo_v=((servo_v+14)/2); //Posición servo v
servo_tpa81 ( TPA81_v_ID, servo_v); //Manda posición del servo a TPA81
delay_ms(10);
servo_h=((servo_h+16)/2);
servo_tpa81 ( TPA81_h_ID, servo_h);
navegacion(); //Salto a la función navegacion
}
}
}
En el siguiente enlace se pueden descargar todos los archivos del proyecto.
El proyecto contenido en el artículo ha sido diseñado con la versión v4.084 de CCS PIC C COMPILER
10 comentarios:
Hola que tal, espero estes bien, he leido tu blog y este Robot me llamo mucho la atencion, este sensor es nuevo?, no lo habia visto, donde lo compraste?, y muy buenos proyectos empezare a construir este y tratar de implementar algunos otros sensores como alguno de humedad o de gases y luego te comentare cuando arranque la construccion para ver si me puedes echar una mano en la programacion (no soy tan bueno como tu), un saludo que estes de lo mejor
otra pregunta mas jeje , en el diagrama manejas dos "bumpers", esos son sensores?? o que son y disculpa la ignorancia.
Hola Rel@x.
El sensor TPA81 es simplemente un sensor de temperatura a distancia con 8 píxeles. Por decirlo de otra manera, 8 sensores de medida de temperatura a distancia alineados. No es muy barato pero merece la pena experimentar con él. Además permite controlar un servo de una forma sencilla. En la red hay muchos distribuidores que lo venden Online.
Los “bumpers” que comentas son simplemente sensores de proximidad por infrarrojos. Aquí se han empleado los sharp-GP2Y0D810Z0F que también se pueden conseguir por Internet, aunque hay muchos otros modelos. No son necesarios pero le ayudan al robot a orientarse en casos específicos.
ah ok ya todo esta mas claro muchas gracias... un saludo
disculpa por que se usan dos tpa81 y no solamente uno
Julian. Los tpa81 tienen los pixeles de medida de temperatura en linea recta. Si se quiere detectar en vertical y en horizontal necesitas uno para cada dirección.
bueno muchas gracias
y sera que no podrias ayudar para que se pudiera hacer con un solo tpa81 pero a la vez el sensor de obstaculos
Solo tienes que eliminar la parte del programa del segundo TPA81
hola tu pagina es muy buena pero veo que algunos de los proyectos ya no se pueden descargar ya como este ..que estan subidos en depositfiles ....a ver si los resuben porque porfa por el bien de todos los interesados gracias ..espero respuesta...
me intereza hacer este proyecto pero no puedo ver los codigos ni hacer descarga quisiera ver si me ayudan con esto resubiendo gracias
Publicar un comentario