'***************** 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 timer2_10ms As Byte
Dim timer3_10ms As Byte
Dim timer4_10ms As Byte
Dim timer5_10ms As Byte
Dim timer6_10ms As Byte
Dim timer1 As Byte 'fija el valor real de los timer
Dim timer2 As Byte
Dim timer3 As Byte
Dim timer4 As Byte
Dim timer5 As Byte
Dim timer6 As Byte
'--------------------------------------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 rd_checsum As Byte
Dim rd_ent0 As Byte
Dim rd_ent1 As Byte
Dim rd_ent2 As Byte
Dim rd_ent3 As Byte
Dim rd_ent4 As Byte
Dim rd_ent5 As Byte
Dim rd_ent6 As Byte
Dim rd_ent7 As Byte
Dim rd_ent8 As Byte
Dim rd_ent9 As Byte
Dim rd_ent10 As Byte
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 aux_ent1 As Byte
Dim aux_ent2 As Byte
Dim aux_ent3 As Byte
Dim aux_ent4 As Byte
Dim aux_ent5 As Byte
Dim aux_ent6 As Byte
Dim aux_ent7 As Byte
Dim aux_ent8 As Byte
Dim aux_ent9 As Byte
Dim aux_ent10 As Byte
Dim e_0 As Byte 'contiene los ultimos valores para aplicar leidos de la trama
Dim e_1 As Byte
Dim e_2 As Byte
Dim e_3 As Byte
Dim e_4 As Byte
Dim e_5 As Byte
Dim e_6 As Byte
Dim e_7 As Byte
Dim e_8 As Byte
Dim e_9 As Byte
Dim e_10 As Byte
'---------------------------------------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----------------------------------------
'-------------------------------------Asigna 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
End
'--------------------------------------FIN DEL MAIN---------------------------------------------------