'****************************************************************
'* Name : servodriver.BAS *
'* Date : 21/11/2010 *
'****************************************************************
define osc 8 'declaracion del oscilador
Trisd = 255 'puerto D como entradas
Trisb = 0 'puerto B como salidas
Posicion var word 'Posicion a la que tiene que ir el servo
PosMenos var word 'Histeresis abajo
PosMas var word 'Histeresis arriba
Contador var word ' Contador de pulsos del encoder
A var bit 'canal A del encoder 500 pulsos/r
B var bit 'canal B del encoder 500 pulsos/r
C var Bit 'canal C del encoder 500 pulsos/r
uA var bit 'variable para saber si cambio canal A
uB var bit 'variable para saber si cambio canal B
uC var bit 'variable para saber si cambio canal C
Giro var bit 'sentido de giro del servo
contador = 0
posicion=5000 'una posicion arbitraria
ua=0
ub=0
Cambio: 'cambiar sentido de giro
posmenos=posicion-15
posmas=posicion+15
portb=0
Inicio:
; si se presiona un pulsador se cambia la
;posicion a la que se debe de mover el servomotor
if portd.1 =1 then goto soltar
if portd.0=1 then ' LEE CANAL A del encoder
A=1 'activado
else
A=0 'desactivado
endif
if portd.2=1 then ' LEE CANAL B del encoder
B=1 'activado
else
B=0 'desactivado
endif
;si el canal A mando un flanco de subida
;y el canal B esta activo giro=1
IF UA=0 and A=1 and B=1 then giro=1
;si el canal A mando un flanco de subida
;y el canal B esta desactivado giro=0
IF UA=0 and A=1 and B=0 then giro=0
;si cambia alguno de los canales del encoder
;es una posicion diferente y por lo tanto
;el contador se debe de incrementar
;dos canales de 500 pulsos con dos estados posibles
;me dan 2000 pulsos por revolucion (4 combinaciones)
if A<>uA or B<>uB then
if giro=1 then contador =contador+1 'incremento
if giro=0 then contador =contador-1 'decremento
endif
uA=A 'ultimo estado del canal A
uB=B 'ultimo estado del canal B
if contador =posicion then ' si llego a la posicion apago el motor
portb.1 =0
portb.2=0
endif
if contador<posmenos then ' giro en sentido =1
portb.1=1 'se activa la salida para que gire el motor giro=1
endif
;despues cambiare esto por PWM
if Contador>posmas then 'giro en sentido =0
portb.2=1 'se activa la salida para que gire el motor giro=0
endif
goto inicio
Soltar:
if portd.1=1 then goto Soltar
;cambio la posicion para que gire en sentido contrario
if posicion=1000 then
posicion=5000
else
posicion=1000
endif
goto Cambio
end
.
