DEVICE 16F876A
XTAL4
OPTION_REG = %00000111
INTCON = %11000000
PIE1=%00100000
SERIAL_BAUD = 2400
RSOUT_PIN = PORTC.6
RSOUT_MODE = TRUE
RSOUT_PACE = 1
RSIN_PIN = PORTC.7
RSIN_MODE = TRUE
HSERIAL_BAUD = 2400
HSERIAL_RCSTA = %10010000
HSERIAL_TXSTA = %00100100
HSERIAL_CLEAR = ON
Declare LCD_TYPE = 0.
Declare LCD_DTPINPORTB.4
Declare LCD_ENPINPORTB.2
Declare LCD_RSPINPORTB.3
Declare LCD_INTERFACE = 4
Declare LCD_LINES = 2
DEFINE HSER_RCSTA 90h
TRISA = 0
PORTA = 0
DIM Posicion_1 as WORD
DIM Posicion_2 as WORD
DIM Posicion_3 as WORD
DIM SELECTOR AS BYTE
DIM IN_SERVO AS BYTE
DIM N AS BYTE
DIM N1 AS BYTE
DIM GRADOS AS BYTE
DIM ANGULO AS BYTE
DIM GIROS AS BYTE
SYMBOL Pin_1 = PORTA.0
SYMBOL Pin_2 = PORTA.1
SYMBOL Pin_3 = PORTA.3
POSICION_1 = 0
POSICION_2 = 0
POSICION_3 = 0
IN_SERVO = 0
GRADOS = 0
ANGULO = 0
GIROS= 0
ON INTERRUPT GOTO INTER
INICIO:
SERVO PIN_1 , POSICION_1
SERVO PIN_2 , POSICION_2
SERVO PIN_3 , POSICION_3
ENABLE
GOTO INICIO
INTER:
HSERIN [DEC SELECTOR]
CLS
PRINT AT 1,1,"SERVO N.", AT 1,9, DEC SELECTOR
IF SELECTOR = 1 THEN GOTO INICIO_1
IF SELECTOR = 2 THEN GOTO INICIO_2
IF SELECTOR = 3 THEN GOTO INICIO_3
inicio_1:
HSERIN [DEC GRADOS]
CLS
PRINT AT 1,1,"GRADOS SERVO N.1", AT 2,6, DEC GRADOS
IF GRADOS > ANGULO THEN
GIROS = GRADOS - ANGULO
FOR N = 0 TO GIROS
Posicion_1 = Posicion_1 + 16
SERVO Pin_1 , Posicion_1
DELAYMS 50
NEXT
ENDIF
IF GRADOS < ANGULO THEN
GIROS = ANGULO - GRADOS
FOR N = 0 TO GIROS
Posicion_1 = Posicion_1 - 10
SERVO Pin_1 , Posicion_1
DELAYMS 50
NEXT
ENDIF
IF GRADOS = ANGULO THEN GIROS = GRADOS
DISABLE
GOTO inicio_1
inicio_2:
HSERIN [DEC GRADOS]
CLS
PRINT AT 1,1,"GRADOS SERVO N.2", AT 2,6, DEC GRADOS
IF GRADOS > ANGULO THEN
GIROS = GRADOS - ANGULO
FOR N = 0 TO GIROS
Posicion_2 = Posicion_2 + 16
SERVO Pin_2 , Posicion_2
DELAYMS 50
NEXT
ENDIF
IF GRADOS < ANGULO THEN
GIROS = ANGULO - GRADOS
FOR N = 0 TO GIROS
Posicion_2 = Posicion_2 - 10
SERVO Pin_2 , Posicion_2
DELAYMS 50
NEXT
ENDIF
IF GRADOS = ANGULO THEN GIROS = GRADOS
DISABLE
GOTO inicio_2
inicio_3:
HSERIN [DEC GRADOS]
CLS
PRINT AT 1,1,"GRADOS SERVO N.3", AT 2,6, DEC GRADOS
IF GRADOS > ANGULO THEN
GIROS = GRADOS - ANGULO
FOR N = 0 TO GIROS
Posicion_3 = Posicion_3 + 16
SERVO Pin_3 , Posicion_3
DELAYMS 50
NEXT
ENDIF
IF GRADOS < ANGULO THEN
GIROS = ANGULO - GRADOS
FOR N = 0 TO GIROS
Posicion_3 = Posicion_3 - 10
SERVO Pin_3 , Posicion_3
DELAYMS 50
NEXT
ENDIF
IF GRADOS = ANGULO THEN GIROS = GRADOS
DISABLE
GOTO inicio_3