Define CONF_WORD = 0x3d32
Define CLOCK_FREQUENCY = 20
'***************** Explorer RX ***************************************
'NOMBRE: Explorer_Rx_Tx_18
'MICRO: PIC16f877A
'DESCRIPCION: COMUNICACION RF PARA CONTROLAR 2 MOTORES y 5 Servos, tipo Mando/Receptor
'tiempo de lectura entre byte y byte del puerto serie +-10mseg maximo y un +-milisegundo minimo
'Version 1.0
'Programa Bot_Miguelito_RF_10
'Se le añade una quinta tecla o maniobra
'Version 1.1
'Implementacion de hasta 11 byte en la trama de datos
'Version 1.2
'Implementacion de un servo_1 en el PROTC.2
'Implementacion de up/down cada 100ms para servo_1
'Version 1.3
'Mejora de la resolucion del servo_1
'Eliminacion de los 100ms de retrazo para servo_1
'Version 1.4
'Implementacion de cinco servos
'No funciona bien el PORTA.4 para manejar servos, asi que se cambia al PORTA.5
'Implementacion del control de los servos con el teclado de cinco teclas
'Version 1.5
'Se mejora la rutina del control de los servos para que actue sobre todo el recorrido de estos
'Nuevo calibrado del punto de reposo de los servos y punto maximo y minimo de cada articulacion
'Version 1.6
'Se cambia la rutina de las bases de tiempo
'Se refresca el display cada 100ms
'Se hace independiente el posicionamiento de los servos (del main) de la llamada a la lectura del bufer
'Se refresca la nueva posicion de los servos cada 100ms
'Version 1.7
'Se amplia el numero de timer
'Se refresca el display cada 50ms
'Se aumenta el paso de los codos a 200ms y el resto se disminulle a 50ms
'Se asocia la pinza/muñeca a la misma maniobra piza/codo, se mantiene tambien la anterior asociacion
'Se rectifica la rutina de los servos y pequeño calibrado de los puntos de trabajo
'No se ejecuta el codigo de la usart ubicado en el vector interrupciones, mientras funciona el codigo de los servos
'para evitar temblores en las articulaciones en reposo
'Version 1.8
'Incremeto de la velocidad de los comandos que manejan el display
'************************************************************************************************
'-------------------------------------Lo primero Programa-----------------------------------------
'-------------------------------------PUERTOS DEFINIDOS POR EL SISTEMA----------------------------
'-------------------------------------Puertos del LCD---------------------------------------------
Define LCD_BITS = 8
Define LCD_DREG = PORTB
Define LCD_DBIT = 0
Define LCD_RSREG = PORTC
Define LCD_RSBIT = 0
Define LCD_EREG = PORTC
Define LCD_EBIT = 4
Define LCD_RWREG = PORTC
Define LCD_RWBIT = 1
Define LCD_COMMANDUS = 2000 'Tiempo de espera despues de ejecutar un comand del lcd
Define LCD_DATAUS = 50 'Tiempo de espera despues de enviar un dato al lcd
Define LCD_INITMS = 50 'Tiempo de espera despues de inicializar el display, solo se ejecuta una vez
'--------------------------------------DEFINICION DE VARIABLES-------------------------------------
'-------------------------------------Variables sub_rutinas del puerto serie---------------------
DIM assi
AS Byte
'Contiene el valor de lectura del puerto serie DIM n_usart
AS Byte
'Numero de datos del USART y del bufer IMAGEN de control DIM usart
AS Byte
'Variable indice que apunta al ultimo dato adquirido por la USART DIM aux_usart
AS Byte
'Auxiliar de la variable indice usart del USART DIM c_serial
AS Byte
'Como USART pero para la lectura de control del bufer IMAGEN DIM bufer
AS Byte
'Indica si hay datos por leer en el bufer USART '-------------------------------------Variables del contador del TIMER1----------------------------
DIM aux_pause
AS Word
'variable auxiiar DIM pause
AS Word
'contiene el valor para simular el comando Waitms DIM basetimer_1ms
AS Byte
'base de tiempos raiz DIM timer_1ms
AS Word
'base de tiempos se incrementa cada 1mSeg DIM timer1_10ms
AS Byte
'base de tiempos se incrementa cada 10mSeg minimo DIM timer1
AS Byte
'fija el valor real de los timer '--------------------------------------Variables R/W de la Trama de Datos y Lectura Entradas --------
DIM arranque
AS Byte
'arranque, estabiliza el bit de bajada DIM grupo
AS Byte
'byte de sincronismo (grupo de trabajo) DIM checsum
AS Byte
'interviene en la deteccion de errores en las tramas de datos DIM w_posrx
AS Byte
'Tiempo entre lectura y lectura del bufer DIM rd_grupo
AS Byte
'Variables imagen (contienen los datos leidos del puerto serie) DIM entrada
AS Byte
'contiene el valor de las entradas DIM aux_ent0
AS Byte
'Contienen el estado de las entradas para enviar en la trama DIM e_0
AS Byte
'contiene los ultimos valores para aplicar leidos de la trama '---------------------------------------Variables del Control de los Servos ------------------------
DIM servos_control
AS Byte
'Controla que se ejecute la rutina de control de los servos DIM servos_contador
AS Byte
'Indica el tiempo de control transcurrido en la rutina de servos DIM servos_aux_contador
AS Byte
DIM servo_1
AS Byte
'Pinza/Pinza DIM servo_2
AS Byte
'Pinza/Muñeca DIM servo_3
AS Byte
'Pinza/Codo DIM servo_4
AS Byte
'Camara/Muñeca DIM servo_5
AS Byte
'Camara/Codo DIM mecanismo
AS Byte
'Selecciona el mecanismo a controlar, 0 = motores ruedas, 1 = pinza/pinza y pinza/muñeca '2 = pinza/muñeca y pinza/codo, 3 = camara/muñeca y camara/codo
'---------------------------------------Variables del Programa -------------------------------------
DIM n
AS Byte
'variable auxiliar para las rutinas del programa '------------------------------------VARIABLES tipo array programa----------------------------------
'------------------------------------VARIABLES tipo array sub_rutinas-------------------------------
'------------------------------------Variables tipo array puerto serie------------------------------
DIM usart_imagen
(31) AS Byte
'Componen el bufer IMAGEN para control del bufer USART DIM usart_bufer
(31) AS Byte
'Componen el bufer de la USART de bajo nivel '-----------------------------------------------ASIGNACIONES----------------------------------------
'-------------------------------------Asigana valores a las variables de la rutina de los servos----
servos_control = 0
servos_contador = 0
servos_aux_contador = 0
servo_1 = 63 'Pinza/Pinza
servo_2 = 46 'Pinza/Muñeca
servo_3 = 61 'Pinza/Codo
servo_4 = 47 'Camara/Muñeca
servo_5 = 50 'Camara/Codo
mecanismo = 1
'-------------------------------------Asigna valores a las variables de la rutina del puerto serie--
usart = 0
aux_usart = 0
c_serial = 0
bufer = 0
n_usart = 30
assi = 0
'-------------------------------------AsignA valores a las variables del TIMER1---------------------
aux_pause = 0
pause = 0
basetimer_1ms = 0
timer_1ms = 0
timer1_10ms = 0
timer2_10ms = 0
timer3_10ms = 0
timer4_10ms = 0
timer5_10ms = 0
timer6_10ms = 0
timer1 = 5 '50ms
timer2 = 35 '350ms
timer3 = 5 '50ms
timer4 = 20 '200ms
timer5 = 5 '50ms
timer6 = 20 '200ms
'--------------------------------------asignacion de valores generales-----------------------------
arranque = 0
grupo = 250
rd_ent0 = 1
rd_ent1 = 1
rd_ent2 = 1
rd_ent3 = 1
rd_ent4 = 1
rd_ent5 = 1
rd_ent6 = 1
rd_ent7 = 1
rd_ent8 = 1
rd_ent9 = 1
rd_ent10 = 1
aux_ent0 = 1
aux_ent1 = 1
aux_ent2 = 1
aux_ent3 = 1
aux_ent4 = 1
aux_ent5 = 1
aux_ent6 = 1
aux_ent7 = 1
aux_ent8 = 1
aux_ent9 = 1
aux_ent10 = 1
entrada = 0
entrada.0 = 1
entrada.1 = 1
entrada.2 = 1
entrada.3 = 1
entrada.4 = 1
e_0 = 1
e_1 = 1
e_2 = 1
e_3 = 1
e_4 = 1
e_5 = 1
e_6 = 1
e_7 = 1
e_8 = 1
e_9 = 1
e_10 = 1
w_posrx = 10 '10mSeg rf, se espera un maximo de tiempo antes de leer el bufer si no hay datos
'--------------------------------------DEFINICION DE PUERTOS------------------------------------------------
'--------------------------------ASIGNACION DE I/O y valores de inicion de las salidas----------------------
ADCON1 = 0x07
TRISA = 0xff
TRISE.0 = 1
TRISE.1 = 1
TRISE.2 = 1
TRISC.5 = 0
TRISD = 0x00
TRISD.0 = 1
TRISD.1 = 1
PORTC.5 = 0
PORTD.2 = 1
PORTD.3 = 1
PORTD.4 = 1
PORTD.5 = 1
PORTD.6 = 1
PORTD.7 = 1
IF RE1 = 0 THEN 'Si trabajo en modo receptor
TRISA = 0x0
PORTA.0 = 0
PORTA.1 = 0
PORTA.2 = 0
PORTA.3 = 0
PORTA.4 = 0
PORTA.5 = 0
Endif
'------------------------------------INICIO DEL PUERTO SERIE, TIMER E INTERRUPCONES------------------
Hseropen 1200 'Velocidad del puerto serie e inicializa los comandos de bajo nivel que controlan la USART
INTCON.PEIE = 1 'bit de habilitacion de interrupciones de perifericos(USART), para que funcione la implementacio P.serie
PIE1.RCIE = 1 'Activa la interrupcion de la USART en modo rx
T1CON.TMR1ON = 1 'Configuracion y habilitacion del TMR1
T1CON.TMR1CS = 0
T1CON.T1CKPS0 = 0
T1CON.T1CKPS1 = 0
TMR1H = 0xec 'Para 1ms
TMR1L = 0x78
PIE1.TMR1IE = 1
Enable 'INTCON.GIE habilita todas las interrupciones globales
'--------------------------------------Inicio Programa--------------------------------------------------
Lcdinit 'Inicializa el display
pause = 1000
GOSUB pause_ms
Lcdcmdout LcdClear
Lcdout "HOLA TOY LISTO:"
GOSUB stop_md 'para motores
GOSUB stop_mi
GOSUB error_usart 'Borra el BUFFER IMAGEN y por lo tanto tambien al USART, OBLIGATORIO POR LO MENOS una vez
pause = 500
GOSUB pause_ms
Lcdcmdout LcdClear
'--------------------------------------Main Programa----------------------------------------------------
main:
IF RCSTA.OERR = 1 THEN 'testea posible bloqueo del puerto fisico en modo Rx
GOSUB error_usart
Endif
IF RE1 = 1 THEN 'trabajo en modo emisor
GOSUB lectura_entradas 'lee las entradas en ent0...3, teclas
GOSUB envio_comando 'envia la trama
Lcdcmdout LcdHome 'muestra las teclas pulsadas
Lcdout "Tx" #aux_ent0, #aux_ent1, #aux_ent2, #aux_ent3, #aux_ent4, " ", #aux_ent5, " ", #aux_ent6, " "
Lcdcmdout LcdLine2Home
Lcdout #aux_ent7, " ", #aux_ent8, " ", #aux_ent9, " ", #aux_ent10, " "
Endif
IF RE1 = 0 THEN 'trabajo en modo receptor
IF timer1_10ms >= timer1 THEN 'muestra maniobras y posicion de los servos, cada 50ms
Lcdcmdout LcdHome
Lcdout "Rx" #e_0, #e_1, #e_2, #e_3, #e_4, " ", #servo_1, " ", #servo_2, " "
Lcdcmdout LcdLine2Home
Lcdout #servo_3, " ", #servo_4, " ", #servo_5, " ", #mecanismo, " "
timer1_10ms = 0
Endif
IF bufer > 0 THEN 'Rx, lee el puerto serie si hay datos y activo maniobras de los motores
GOSUB recivo_comando
Endif
IF e_4 = 0 THEN 'Selecciona el mecanismo a controlar
IF timer2_10ms >= timer2 THEN
mecanismo = mecanismo + 1
IF mecanismo >= 4 THEN mecanismo = 0
timer2_10ms = 0
Endif
Endif
IF timer3_10ms >= timer3 THEN
IF mecanismo = 1 THEN 'Control de la Pinza
IF e_3 = 0 THEN
IF servo_1 < 63 THEN servo_1 = servo_1 + 1 'Pinza/Pinza, superior
Endif
IF e_2 = 0 THEN
IF servo_1 > 30 THEN servo_1 = servo_1 - 1 'Pinza/Pinza, inferior
Endif
IF e_1 = 0 THEN
IF servo_2 < 75 THEN servo_2 = servo_2 + 1 'Pinza/Muñeca
Endif
IF e_0 = 0 THEN
IF servo_2 > 18 THEN servo_2 = servo_2 - 1
Endif
Endif
timer3_10ms = 0
Endif
IF mecanismo = 2 THEN
IF timer5_10ms >= timer5 THEN
IF e_1 = 0 THEN
IF servo_2 < 75 THEN servo_2 = servo_2 + 1 'Pinza/Muñeca
Endif
IF e_0 = 0 THEN
IF servo_2 > 18 THEN servo_2 = servo_2 - 1
Endif
timer5_10ms = 0
Endif
IF timer4_10ms >= timer4 THEN
IF e_3 = 0 THEN
IF servo_3 < 73 THEN servo_3 = servo_3 + 1 'Pinza/codo
Endif
IF e_2 = 0 THEN
IF servo_3 > 36 THEN servo_3 = servo_3 - 1
Endif
timer4_10ms = 0
Endif
Endif
IF mecanismo = 3 THEN 'Control de la Camara
IF timer5_10ms >= timer5 THEN
IF e_0 = 0 THEN
IF servo_4 < 75 THEN servo_4 = servo_4 + 1 'Camara/Muñeca
Endif
IF e_1 = 0 THEN
IF servo_4 > 19 THEN servo_4 = servo_4 - 1
Endif
timer5_10ms = 0
Endif
IF timer6_10ms >= timer6 THEN
IF e_3 = 0 THEN
IF servo_5 < 75 THEN servo_5 = servo_5 + 1 'Camara/codo
Endif
IF e_2 = 0 THEN
IF servo_5 > 29 THEN servo_5 = servo_5 - 1
Endif
timer6_10ms = 0
Endif
Endif
IF mecanismo = 0 THEN 'Control de los motores de avance
IF e_0 = 0 THEN
GOSUB fwd_md
GOSUB rev_mi
GOSUB on_md
GOSUB on_mi
Endif
IF e_1 = 0 THEN
GOSUB rev_md
GOSUB fwd_mi
GOSUB on_md
GOSUB on_mi
Endif
IF e_2 = 0 THEN
GOSUB fwd_md
GOSUB fwd_mi
GOSUB on_md
GOSUB on_mi
Endif
IF e_3 = 0 THEN
GOSUB rev_md
GOSUB rev_mi
GOSUB on_md
GOSUB on_mi
Endif
IF e_0
= 1 AND e_1
= 1 AND e_2
= 1 AND e_3
= 1 THEN GOSUB stop_md
GOSUB stop_mi
Endif
Endif
Endif
GOTO main
'--------------------------------------FIN DEL MAIN---------------------------------------------------
'--------------------------------------Sub_Rutinas Programa-----------------------------------------------
recivo_comando: 'leo el bufer del puerto serie
GOSUB readserial
IF assi = grupo THEN
rd_grupo = assi
ELSE
RETURN
Endif
GOSUB readserial
rd_ent0 = assi
GOSUB readserial
rd_ent1 = assi
GOSUB readserial
rd_ent2 = assi
GOSUB readserial
rd_ent3 = assi
GOSUB readserial
rd_ent4 = assi
GOSUB readserial
rd_ent5 = assi
GOSUB readserial
rd_ent6 = assi
GOSUB readserial
rd_ent7 = assi
GOSUB readserial
rd_ent8 = assi
GOSUB readserial
rd_ent9 = assi
GOSUB readserial
rd_ent10 = assi
GOSUB readserial
rd_checsum = rd_ent0 + rd_ent1 + rd_ent2 + rd_ent3 + rd_ent4 + rd_ent5
rd_checsum = rd_ent6 + rd_checsum + rd_ent7 + rd_ent8 + rd_ent9 + rd_ent10
IF assi = rd_checsum THEN 'renuevo los valores de las entradas con los valores leidos del bufer
e_0 = rd_ent0
e_1 = rd_ent1
e_2 = rd_ent2
e_3 = rd_ent3
e_4 = rd_ent4
e_5 = rd_ent5
e_6 = rd_ent6
e_7 = rd_ent7
e_8 = rd_ent8
e_9 = rd_ent9
e_10 = rd_ent10
Endif
RETURN
envio_comando: 'transmite la trama de datos, modo tx
pause = 5 'pausa antes de enviar la trama
GOSUB pause_ms
aux_ent0.0 = entrada.0
aux_ent1.0 = entrada.1
aux_ent2.0 = entrada.2
aux_ent3.0 = entrada.3
aux_ent4.0 = entrada.4
checsum = aux_ent0 + aux_ent1 + aux_ent2 + aux_ent3 + aux_ent4 + aux_ent5
checsum = checsum + aux_ent6 + aux_ent7 + aux_ent8 + aux_ent9 + aux_ent10
Hserout arranque, grupo, aux_ent0, aux_ent1, aux_ent2, aux_ent3, aux_ent4, aux_ent5, aux_ent6, aux_ent7, aux_ent8, aux_ent9, aux_ent10, checsum
pause = 1 'pausa despues de enviar la trama
GOSUB pause_ms
RETURN
lectura_entradas: 'lee las entradas o controles del bot, modo tx
entrada.0 = RA0
entrada.1 = RA1
entrada.2 = RA2
entrada.3 = RA3
entrada.4 = RA4
RETURN
pause_ms: 'rutina de espera, rutina tipo Waitms, pause establese el tiempo total en mSeg
aux_pause = 0 'variable auxiliar lleva la cuenta de los ms
timer_1ms = 0 'al borrar el contador se activa y comienza la cuenta de 1mSeg
WHILE aux_pause <= pause 'tiempo en mSeg maximos a contar
aux_pause = aux_pause + 1 'variable auxiliar lleva la cuenta de los ms
IF timer_1ms > 0 THEN timer_1ms = 0 'al borrar el contador se activa y cuenta 1mSeg
WHILE timer_1ms < 1 'espera un miliesegundo
WEND
WEND
RETURN
pause_ms_rx: 'rutina de espera antes de leer el bufer, rutina tipo Waitms, w_posrx establese el tiempo total en mSeg
aux_pause = 0 'variable auxiliar lleva la cuenta de los ms
timer_1ms = 0 'al borrar el contador se activa y comienza la cuenta de 1mSeg
WHILE aux_pause <= w_posrx 'tiempo en mSeg maximos a contar
IF bufer > 0 THEN aux_pause = w_posrx 'si entran datos en el bufer se termina el contaje
aux_pause = aux_pause + 1 'variable auxiliar lleva la cuenta de los ms
IF timer_1ms > 0 THEN timer_1ms = 0 'al borrar el contador se acitva y cuenta 1mSeg
WHILE timer_1ms < 1 'espera un miliesegundo
WEND
WEND
RETURN
on_md: 'activa el motor derecho
PORTD.7 = 1
RETURN
stop_md: 'para el motor derecho
PORTD.7 = 0
RETURN
on_mi: 'activa el motor izquierdo
PORTD.4 = 1
RETURN
stop_mi: 'para el motor izquierdo
PORTD.4 = 0
RETURN
fwd_md: 'sentido adelante motor derecho (no lo pone en marcha)
PORTD.5 = 0
PORTD.6 = 1
RETURN
rev_md: 'sentido atras derecho (no activa el motor)
PORTD.5 = 1
PORTD.6 = 0
RETURN
fwd_mi: 'sentido adelante izquierdo (no activa el motor)
PORTD.2 = 0
PORTD.3 = 1
RETURN
rev_mi: 'sentido atras izquierdo (no activa el motor)
PORTD.2 = 1
PORTD.3 = 0
RETURN