Está en la página 1de 11

robot_ultraso_v3_2modos

/*
Microrrobot Ultrasonido v1
*/
#include <16f876a.h>
#use delay(clock=20000000)
#byte OPTION_REG
#byte TRISA
#byte TRISB
#byte TRISC
#byte PORTA
#byte PORTB
#byte PORTC
#byte ADCON1
#byte TMR0
#byte INTCON
#bit T0IF
#byte RCREG
#byte RCSTA
#byte TXSTA
#byte SPBRG
#byte PIE1
#bit RCIF
#bit RCIE
#bit T0CS
#bit PSA
#bit PS2
#bit PS1
#bit PS0

=0x81
=0x85
=0x86
=0x87
=0x05
=0x06
=0x07
=0x9F
=0x01
= 0x0B
= 0x0B.2
=0x1A
=0x18
=0x98
=0x99
=0x8C
=0x0C.5
=0x8C.5
=0x81.5
=0x81.3
=0x81.2
=0x81.1
=0x81.0

#define TRISB_MOTOR_IZQUIERDO 0x00000110b


#define TRISB_MOTOR_DERECHO 0x00011000b
/* DEFINICIONES DE MOTORES Y SENSORES INFRARROJO */
//-- Definiciones para los motores. Estos valores define
//-- los movimientos del robot
#define
AVANZA
0x1C
#define
ATRAS
0x16
#define
IZQUIERDA 0x1E
#define
DERECHA
0x14
#define
STOP
0x00
#define LED 0x02
#define
#define
#define
#define

SENSOR1
SENSOR2
SENSOR3
SENSOR4

0x01
0x20
0x40
0x80

//-- Constantes de acceso a los infrarrojos


#define NEGRO 1
#define BLANCO 0
//-- Constantes de acceso a los bumpers
#define APRETADO 1
#define SIN_APRETAR 0
//-- Constantes de acceso al pulsador
#define PULSADO 1
#define SIN_PULSAR 0
//-- Constantes de acceso al led
#define ENCENDIDO 1
#define APAGADO 0
Pgina 1

robot_ultraso_v3_2modos
#define VERDADERO 1
#define FALSO 0
/* DEFINICIONES DE MODULOS ULTRASONIDO */
//-- Definiciones modulo ultrasonido
#define TRISA_SENSORES 0x05
//-- Delay definitions
#define INPUT_TRIGGER_DELAY 10 /* In microseconds */
//-- Delay between end of echo pulse and trigger pulse is 10 ms
#define DELAY_BETWEEN_ECHO_PULSE 10
//-- Maximum elapsed time for an echo pulse is approx. 36 ms
#define MAX_DELAY_ECHO_PULSE 44000 /* Number of timer increments
for 35 ms using prescale of 4 */
//-- Para cristal 4MHz
//-- #define MAX_DELAY_ECHO_PULSE 35000
/* Number of timer increments
for 35 ms using prescale of 1 */
/* #ifdef 20_MHZ_CLOCK */
/* Every Timer1 increment takes 0.8 us with prescale of 4 */
/* t us / 1000000 us/s * (347 m/s) * (100 cm/m) / 2 */
/* = # increments * 0.8 / 5000 * 347 or roughly t / 72 */
#define GET_DISTANCE(t) (t / 72)
/* #else */ /* 4 MHz clock */
/* Every increment takes 1 us */
/* t us / 1000000 us/s * (347 m/s) * (100 cm/m) / 2 */
/* = t / 5000 * 347 or roughly t / 58 */
/* #define GET_DISTANCE(t) (t / 58) */
/* #endif */
//-- Hardware pin definition
#define ULTRASONIC_TX1 PIN_A1
#define ULTRASONIC_RX1 PIN_A0
#define ULTRASONIC_TX2 PIN_A3
#define ULTRASONIC_RX2 PIN_A2

//PIN_C3
//PIN_C4

#define DISTANCE_INFINITE (0xfff0)


#define DISTANCE_OBJECT (0x0014) //20 cm
//-- MODO es el modo de funcionamiento (tiene 4 posibles, y se selecciona
//-- mediante las entradas PIN_C1, PIN_C2 ==> 00:Modo 0; 01:Modo 1; 10:Modo 2;
11:Modo 3
int MODO = 0;
//-- Type definitions
typedef int uint8_t;
typedef long uint16_t;
//-- Variable definitions
uint16_t echo_delay;
//-- Global variable definitions
uint16_t distance1 = DISTANCE_INFINITE; /* This is the distance of the closest
object ahead of the ultrasonic
module (in cm). If set to INFINITE,
there are no objects in sight. */
uint16_t distance2 = DISTANCE_INFINITE; /* This is the distance of the closest
object ahead of the ultrasonic
module (in cm). If set to INFINITE,
Pgina 2

robot_ultraso_v3_2modos
there are no objects in sight. */
void ConfigurarMicrorrobot();
void monitor_distance_sensor1();
void monitor_distance_sensor2();
void PausaTiempo(unsigned int segundos,unsigned int centesimas);
void Avanzar();
void GirarIzq();
void GirarDer();
void Retroceder();
void Parar();
void LeerModo();
unsigned char SensorIR_Izq_Lee_Negro();
unsigned char SensorIR_Der_Lee_Negro();
unsigned char SensorIR_IzqDetras_Lee_Negro();
unsigned char SensorIR_DerDetras_Lee_Negro();
unsigned char SensorIR_Izq_Lee_Blanco();
unsigned char SensorIR_Der_Lee_Blanco();
unsigned char LeerInfrarrojos();
unsigned char LeerSensor1();
unsigned char LeerSensor2();
unsigned char LeerSensor3();
unsigned char LeerSensor4();
unsigned char Bumper_Izq_esta_apretado();
unsigned char Bumper_Der_esta_apretado();
unsigned char LeerBumperIzquierdo();
unsigned char LeerBumperDerecho();
unsigned char LeerPuertoALinea1();
unsigned char LeerPuertoALinea2();
unsigned char LeerPuertoALinea3();
unsigned char LeerPuertoALinea4();
void EncenderLed();
void ApagarLed();
unsigned char Led_esta_encendido();
unsigned char Led_esta_apagado();
unsigned char EstadoLed();
void CambiarLed();
unsigned char LeerPulsador();
unsigned char Pulsador_esta_pulsado();
unsigned char Pulsador_no_esta_pulsado();
//---------------------------//- Comienzo del programa
//---------------------------void main(void)
{
ConfigurarMicrorrobot();
for ( ; ; ) {
if (MODO == 1){
monitor_distance_sensor1();
monitor_distance_sensor2();
if ((distance1 <= DISTANCE_OBJECT) || (distance2 <= DISTANCE_OBJECT)){
if (((distance1 + 3) >= distance2) && ((distance1 - 3) <= distance2)) {
//Si las dos distancias son iguales -proximasGirarDer();
delay_ms(300);
}else if (((distance1 + 3) >= distance2)){ //Si hay un objeto mas cerca
de la derecha (sensor2) que de la izquierda (sensor1)
GirarIzq();
delay_ms(300);
}else{ //Si hay un objeto mas cerca de la izquierda (sensor1) que de la
derecha (sensor2)
GirarDer();
delay_ms(300);
}
}else{
Pgina 3

robot_ultraso_v3_2modos
Avanzar();
delay_ms(300);
}
}else{
monitor_distance_sensor1();
monitor_distance_sensor2();
if ((distance1 <= 50) || (distance2 <= 50)){
if (((distance1 + 3) >= distance2) && ((distance1 - 3) <= distance2)) {
//Si las dos distancias son iguales -proximasif (distance1 > (DISTANCE_OBJECT + 3)){ //Microrrobot lejos. Se tiene
que acercar
Avanzar();
delay_ms(100);
}else if (distance1 < (DISTANCE_OBJECT - 3)){ //Microrrobot cerca. Se
tiene que alejar
Retroceder();
delay_ms(100);
}else{
Parar();
delay_ms(100);
}
}else if (((distance1 + 3) >= distance2)){ //Si hay un objeto mas cerca
de la derecha (sensor2) que de la izquierda (sensor1)
GirarDer();
delay_ms(100);
}else{ //Si hay un objeto mas cerca de la izquierda (sensor1) que de la
derecha (sensor2)
GirarIzq();
delay_ms(100);
}
}else{
GirarDer();
delay_ms(100);
}
}
//printf("echo delay %lu\r\n", echo_delay);
//printf("distance %lu\r\n", distance);
//if (distance < 10){
// PORTC = 0b00000001;
//}else if (distance < 20){
// PORTC = 0b00000011;
//}else if (distance < 30){
// PORTC = 0b00000111;
//}else if (distance < 40){
// PORTC = 0b00001111;
//}else if (distance < 50){
// PORTC = 0b00011111;
//}else if (distance < 100){
// PORTC = 0b00111111;
//}else if (distance < 200){
// PORTC = 0b01111111;
//}else{
// PORTC = 0b11111111;
//}
//PORTC = distance;
}
//for
//{
//
//
//
//
//
//
//
//

(;;)
if (SensorIR_IzqDetras_Lee_Negro() && SensorIR_DerDetras_Lee_Negro())
{
Avanzar();
delay_ms(300);
}
else if (SensorIR_Izq_Lee_Negro() && SensorIR_Der_Lee_Negro())
{
Retroceder();
Pgina 4

robot_ultraso_v3_2modos
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//}

delay_ms(300);
}
else if (SensorIR_Izq_Lee_Negro())
{
Retroceder();
delay_ms(300);
GirarDer();
delay_ms(500);
Avanzar();
delay_ms(100);
}
else if (SensorIR_Der_Lee_Negro())
{
Retroceder();
delay_ms(300);
GirarIzq();
delay_ms(500);
Avanzar();
delay_ms(100);
}
else if (SensorIR_IzqDetras_Lee_Negro())
{
Avanzar();
delay_ms(300);
GirarDer();
delay_ms(500);
Avanzar();
delay_ms(100);
}
else if (SensorIR_DerDetras_Lee_Negro())
{
Avanzar();
delay_ms(300);
GirarIzq();
delay_ms(500);
Avanzar();
delay_ms(100);
}
else
{
Avanzar(); //GirarDer();
delay_ms(50);
}

}
void ConfigurarMicrorrobot()
{
//-- Configurar el puerto B para trabajar con el Skybot
//-- RB0, RB5, RB6 y RB7 como entradas
//-- RB1, RB2, RB3 y RB4 como salidas
TRISB=0xE1;
//-- Configurar el puerto A para trabajar con el Skybot
//-- RA0, RA2 como entradas (RA0:Lee sensor ultrasonido 1; RA1:Lee sensor
ultrasonido 2)
//-- RA1, RA3, RA4 como salidas (RA1:Escribe sensor ultrasonido 1;
RA3:Escribe sensor ultrasonido 2)
TRISA=0x05;
//-- Configurar el puerto A para leer los bumpers del Skybot
ADCON1 = 0x06; //0x0E;
//-- Configurar puerto C como salidas todo excepto RC2 i RC1: entradas
TRISC=0x06;
PORTC = 0x00;
Pgina 5

robot_ultraso_v3_2modos
//Avanzar();
//delay_ms(500);
//-- Set initial condition for ultrasonic transmitter
output_low(ULTRASONIC_TX1);
output_low(ULTRASONIC_TX2);
//-- Set up Timer1
//-- Following settings will allow for 1 cm precision
/* #ifdef 20_MHZ_CLOCK */
setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);
/* #else */ // 4 MHz
/* setup_timer_1(T1_INTERNAL | T1_DIV_BY_1); */
/* #endif */
LeerModo();
}
void monitor_distance_sensor1(void) {
/* First, toggle the ULTRASONIC_TX pin to generate echo signal. */
/* Pull ultrasonic TX pin high */
output_high(ULTRASONIC_TX1);
/* Get the sensor the chance to see this */
delay_us(INPUT_TRIGGER_DELAY);
output_low(ULTRASONIC_TX1);
/* Wait for pulse on ULTRASONIC_RX pin. Distance is measured by
using time it takes for ultrasound to echo, which is
calculated according to length of echo pulse on ULTRASONIC_RX
pin. */
/* Wait for low to high transition on ULTRASONIC_RX pin. */
for ( ; !input(ULTRASONIC_RX1); )
;
/* Detected a low to high transition */
/* Reset timer. */
set_timer1(0);
/* Wait for high to low transition on ULTRASONIC_RX pin. */
for ( ; input(ULTRASONIC_RX1); )
;
/* Detected a high to low transition */
echo_delay = get_timer1();
/*
if
/*
/*
/*

#ifdef 20_MHZ_CLOCK */
(echo_delay >= 44000) {
#else */
if (echo_delay >= 35000) { */
#endif */
distance1 = DISTANCE_INFINITE;

}
else {
distance1 = GET_DISTANCE(echo_delay);
}
/* Make sure there is an appropriate delay between end of echo and
trigger pulse. If the delay between calls to monitor_distance_sensor
is greater than 10 ms, then this delay can be removed. */
delay_ms(DELAY_BETWEEN_ECHO_PULSE);
} /* monitor_distance_sensor1 */
void monitor_distance_sensor2(void) {
Pgina 6

robot_ultraso_v3_2modos
/* First, toggle the ULTRASONIC_TX pin to generate echo signal. */
/* Pull ultrasonic TX pin high */
output_high(ULTRASONIC_TX2);
/* Get the sensor the chance to see this */
delay_us(INPUT_TRIGGER_DELAY);
output_low(ULTRASONIC_TX2);
/* Wait for pulse on ULTRASONIC_RX pin. Distance is measured by
using time it takes for ultrasound to echo, which is
calculated according to length of echo pulse on ULTRASONIC_RX
pin. */
/* Wait for low to high transition on ULTRASONIC_RX pin. */
for ( ; !input(ULTRASONIC_RX2); )
;
/* Detected a low to high transition */
/* Reset timer. */
set_timer1(0);
/* Wait for high to low transition on ULTRASONIC_RX pin. */
for ( ; input(ULTRASONIC_RX2); )
;
/* Detected a high to low transition */
echo_delay = get_timer1();
/*
if
/*
/*
/*

#ifdef 20_MHZ_CLOCK */
(echo_delay >= 44000) {
#else */
if (echo_delay >= 35000) { */
#endif */
distance2 = DISTANCE_INFINITE;

}
else {
distance2 = GET_DISTANCE(echo_delay);
}
/* Make sure there is an appropriate delay between end of echo and
trigger pulse. If the delay between calls to monitor_distance_sensor
is greater than 10 ms, then this delay can be removed. */
delay_ms(DELAY_BETWEEN_ECHO_PULSE);
} /* monitor_distance_sensor1 */
void PausaTiempo(unsigned int segundos,unsigned int centesimas)
{
// Centesimas:
while (centesimas > 25)
{
delay_ms(250);
centesimas-=25;
}
if (centesimas > 0){
delay_ms(centesimas);
}
while (segundos > 2)
{
delay_ms(2000);
segundos-=2;
}
if (segundos==2) {
delay_ms(2000);
Pgina 7

robot_ultraso_v3_2modos
}
else if (segundos==1) {
delay_ms(1000);
}
}
void Avanzar()
{
// TRISB &= ~(TRISB_MOTOR_IZQUIERDO | TRISB_MOTOR_DERECHO);
PORTB=AVANZA;
return;
}
void GirarIzq()
{
// TRISB&=~(TRISB_MOTOR_IZQUIERDO | TRISB_MOTOR_DERECHO);
PORTB=IZQUIERDA;
return;
}
void GirarDer()
{
// TRISB&=~(TRISB_MOTOR_IZQUIERDO | TRISB_MOTOR_DERECHO);
PORTB=DERECHA;
}
void Retroceder()
{
// TRISB&=~(TRISB_MOTOR_IZQUIERDO | TRISB_MOTOR_DERECHO);
PORTB=ATRAS;
}
void Parar()
{
// TRISB&=~(TRISB_MOTOR_IZQUIERDO | TRISB_MOTOR_DERECHO);
PORTB=STOP;
}
void LeerModo()
{
if ((PORTC & 0x06)
MODO = 0;
}else if ((PORTC &
MODO = 1;
}else if ((PORTC &
MODO = 2;
}else if ((PORTC &
MODO = 3;
}else{
MODO = 0;
}
}

== 0){
0x06) == 4){
0x06) == 2){
0x06) == 6){

unsigned char SensorIR_Izq_Lee_Negro()


{
if (LeerSensor1() == NEGRO) return VERDADERO;
return FALSO;
}
unsigned char SensorIR_Der_Lee_Negro()
{
if (LeerSensor2() == NEGRO) return VERDADERO;
return FALSO;
}
unsigned char SensorIR_IzqDetras_Lee_Negro()
{
Pgina 8

robot_ultraso_v3_2modos
if (LeerSensor3() == NEGRO) return VERDADERO;
return FALSO;
}
unsigned char SensorIR_DerDetras_Lee_Negro()
{
if (LeerSensor4() == NEGRO) return VERDADERO;
return FALSO;
}
unsigned char SensorIR_Izq_Lee_Blanco()
{
if (LeerSensor1() == BLANCO) return VERDADERO;
return FALSO;
}
unsigned char SensorIR_Der_Lee_Blanco()
{
if (LeerSensor2() == BLANCO) return VERDADERO;
return FALSO;
}
unsigned char LeerInfrarrojos()
{
unsigned char sI=0,sD=0;
if (SensorIR_Izq_Lee_Negro()) sI=1;
if (SensorIR_Der_Lee_Negro()) sD=1;
if (sI && sD) return 3;
else if (sD) return 2;
return 1;
}
unsigned char LeerSensor1()
{
if ((PORTB & SENSOR1) != 0) return NEGRO;
return BLANCO;
}
unsigned char LeerSensor2()
{
if ((PORTB & SENSOR2) != 0) return NEGRO;
return BLANCO;
}
unsigned char LeerSensor3()
{
if ((PORTB & SENSOR3) != 0) return NEGRO;
return BLANCO;
}
unsigned char LeerSensor4()
{
if ((PORTB & SENSOR4) != 0) return NEGRO;
return BLANCO;
}
unsigned char Bumper_Izq_esta_apretado()
{
if (LeerBumperIzquierdo() == APRETADO) return VERDADERO;
return FALSO;
}
unsigned char Bumper_Der_esta_apretado()
{
if (LeerBumperDerecho() == APRETADO) return VERDADERO;
Pgina 9

robot_ultraso_v3_2modos
return FALSO;
}
unsigned char LeerBumperIzquierdo()
{
if (LeerPuertoALinea3() == 1) return APRETADO;
return SIN_APRETAR;
}
unsigned char LeerBumperDerecho()
{
if (LeerPuertoALinea2() == 1) return APRETADO;
return SIN_APRETAR;
}
unsigned char LeerPuertoALinea1()
{
if ((PORTA & 0x01) != 0) return 1;
return 0;
}
unsigned char LeerPuertoALinea2()
{
if ((PORTA & 0x02) != 0) return 1;
return 0;
}
unsigned char LeerPuertoALinea3()
{
if ((PORTA & 0x04) != 0) return 1;
return 0;
}
unsigned char LeerPuertoALinea4()
{
if ((PORTA & 0x08) != 0) return 1;
return 0;
}
void EncenderLed()
{
PORTB |= LED;
}
void ApagarLed()
{
PORTB &= ~LED;
}
unsigned char Led_esta_encendido()
{
if (EstadoLed() == ENCENDIDO) return VERDADERO;
return APAGADO;
}
unsigned char Led_esta_apagado()
{
if (EstadoLed() == APAGADO) return VERDADERO;
return APAGADO;
}
unsigned char EstadoLed()
{
if ( (PORTB&0x02)==0 ) return APAGADO;
return ENCENDIDO;
}
void CambiarLed()
Pgina 10

robot_ultraso_v3_2modos
{
if (EstadoLed()==0) {
EncenderLed();
}
else {
ApagarLed();
}
}
unsigned char LeerPulsador()
{
if ((PORTB & SENSOR1) != 0) return SIN_PULSAR;
return PULSADO;
}
unsigned char Pulsador_esta_pulsado()
{
if (LeerPulsador() == PULSADO) return VERDADERO;
return FALSO;
}
unsigned char Pulsador_no_esta_pulsado()
{
if (LeerPulsador() == SIN_PULSAR) return VERDADERO;
return FALSO;
}

Pgina 11