'Trabajando con servos, PIC12F629
Define CONF_WORD = 0x3184
Define CLOCK_FREQUENCY = 4
'Define SIMULATION_WAITMS_VALUE = 1 'suprime los tiempos de espera Waitms, solo activar esta linea en simulacion
AllDigital
Symbol mass = GPIO.0
Symbol rojo = GPIO.1
Symbol verde = GPIO.2
Symbol menoss = GPIO.3
Symbol boton = GPIO.4
Symbol servos = GPIO.5
TRISIO = %011001
'define variables
Dim velocidad As Byte
velocidad = 3
Dim n As Byte
Dim posicion As Byte '1 centro 2-dch 3-izq posicion servo
Dim pulso_dch As Byte 'angulo dch servo
Dim pulso_izq As Byte 'angulo izq servo
Dim pulso_dch1 As Byte
Dim pulso_izq1 As Byte
Dim outon As Byte 'Tiempo de la señal de control del servo a On (duty cycle)
Dim outon1 As Byte
'Dim outoff As Word
'Dim periodo As Word
Gosub leer
rojo = 0
verde = 0
servos = 0
'###################################################################################################
'posiciona los servos como habían quedado
If posicion = 0xff Or posicion = 01 Then 'el servo está en el centro o se conecta la primera vez
'mueve el servo al centro e inicializa variables
posicion = 01
Write 01, posicion
pulso_dch = 155
Write 03, pulso_dch
pulso_izq = 155
Write 04, pulso_izq
Call servo_inicio(pulso_dch)
rojo = 1
verde = 1
Endif
If posicion = 02 Then
Call servo_inicio(pulso_dch)
rojo = 1
verde = 0
Endif
If posicion = 03 Then
Call servo_inicio(pulso_izq)
rojo = 0
verde = 1
Endif
'####################################################################################################
'inicio programa
main:
Gosub leer
If mass = 1 Then
WaitMs 200
Gosub mas
Endif
If menoss = 1 Then
WaitMs 200
Gosub menos
Endif
If boton = 1 Then
WaitMs 400
Gosub mover_servo
Endif
Goto main
End
'###################################################################################################
mas:
Gosub leer
Select Case posicion
Case 01
pulso_dch1 = pulso_dch + 1
If pulso_dch1 > 210 Then
pulso_dch = 155
pulso_dch1 = pulso_dch + 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo3(pulso_dch, pulso_dch1)
pulso_dch = pulso_dch1
Write 03, pulso_dch
Case 02
pulso_dch1 = pulso_dch + 1
If pulso_dch1 > 210 Then
pulso_dch = 155
pulso_dch1 = pulso_dch + 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo3(pulso_dch, pulso_dch1)
pulso_dch = pulso_dch1
Write 03, pulso_dch
Case 03
pulso_izq1 = pulso_izq - 1
If pulso_izq1 < 100 Then
pulso_izq = 155
pulso_izq1 = pulso_izq - 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo4(pulso_izq, pulso_izq1)
pulso_izq = pulso_izq1
Write 04, pulso_izq
EndSelect
Return
menos:
Gosub leer
Select Case posicion
Case 01
pulso_dch1 = pulso_dch - 1
If pulso_dch1 < 155 Then
pulso_dch = 155
pulso_dch1 = pulso_dch + 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo4(pulso_dch, pulso_dch1)
pulso_dch = pulso_dch1
Write 03, pulso_dch
Case 02
pulso_dch1 = pulso_dch - 1
If pulso_dch1 < 155 Then
pulso_dch = 155
pulso_dch1 = pulso_dch + 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo4(pulso_dch, pulso_dch1)
pulso_dch = pulso_dch1
Write 03, pulso_dch
Case 03
pulso_izq1 = pulso_izq + 1
If pulso_izq1 > 155 Then
pulso_izq = 155
pulso_izq1 = pulso_izq - 1
WaitMs 1000
verde = 1
rojo = 1
Endif
Call servo3(pulso_izq, pulso_izq1)
pulso_izq = pulso_izq1
Write 04, pulso_izq
EndSelect
Return
'####################################################################
'mueve el servo
mover_servo:
Gosub leer
If posicion = 01 Then
'el servo està en centro y tiene que pasar a dch
Call servo(155, pulso_dch, velocidad)
servos = 0
rojo = 0
verde = 0
WaitMs 200
rojo = 1
verde = 0
boton = 0
posicion = 02
Write 01, posicion
Return 'goto main 'NO SE PUEDE SALIR DE UN ETIQUETA/RETURN CON GOTO HAY QUE SALIR CON RETURN
Endif
If posicion = 02 Then
'el servo està en dch y tiene que pasar a izq
Call servo1(pulso_dch, pulso_izq, velocidad)
servos = 0
rojo = 0
verde = 0
WaitMs 200
rojo = 0
verde = 1
boton = 0
posicion = 03
Write 01, posicion
Return 'goto main 'NO SE PUEDE SALIR DE UN ETIQUETA/RETURN CON GOTO HAY QUE SALIR CON RETURN
Endif
If posicion = 03 Then
'el servo está en izq y tiene que pasar a dch
Call servo(pulso_izq, pulso_dch, velocidad)
servos = 0
rojo = 0
verde = 0
WaitMs 200
rojo = 1
verde = 0
boton = 0
Write 01, 02
Return 'goto main 'NO SE PUEDE SALIR DE UN ETIQUETA/RETURN CON GOTO HAY QUE SALIR CON RETURN
Endif
Return
leer:
Read 01, posicion
Read 03, pulso_dch
Read 04, pulso_izq
Return
'################################################################
'procedimiento para mover servo con control velocidad
Proc servo(x1 As Byte, y1 As Byte, velocidad As Byte)
Const periodo = 20000 'Periodo en uSeg. para servos de 50Hz
Dim outoff As Word 'Variable local
For outon = x1 To y1
outoff = (periodo - x1) * 10 'Calculo tiempo de la señal a Off en uSeg.
For n = 0 To velocidad 'Se repite durante 1500mSeg. para dar tiempo al servo a posicionarse
ServoOut servos, outon '1.6mSeg. (señal a On)
WaitUs outoff 'Tiempo de la señal a Off
Next n
Next outon
End Proc
'#################################################################
'procedimiento para mover servo con boton mas
Proc servo3(x2 As Byte, y2 As Byte)
For n = x2 To y2
ServoOut servos, n
WaitMs 4
Next n
End Proc
'##################################################################
'procedimiento para mover servo con boton menos
Proc servo4(x3 As Byte, y3 As Byte)
For n = x3 To y3 Step -1
ServoOut servos, n
WaitMs 4
Next n
End Proc
'################################################################
'procedimiento para mover servo con control velocidad en sentido negativo
Proc servo1(x4 As Byte, y4 As Byte, velocidad As Byte)
Const periodo1 = 20000 'Periodo en uSeg. para servos de 50Hz
Dim outoff1 As Word 'Variable local
For outon = x4 To y4 Step -1
outoff1 = (periodo1 - x4) * 10 'Calculo tiempo de la señal a Off en uSeg.
For n = 0 To velocidad 'Se repite durante 1500mSeg. para dar tiempo al servo a posicionarse
ServoOut servos, outon '1.6mSeg. (señal a On)
WaitUs outoff1 'Tiempo de la señal a Off
Next n
Next outon
End Proc
Proc servo_inicio(x5 As Byte)
Const periodo2 = 20000
Dim outoff2 As Word
outon = x5
outoff2 = (periodo2 - outon) * 10 'Calculo tiempo de la señal a Off en uSeg.
For n = 1 To 75 'Se repite durante 1500mSeg. para dar tiempo al servo a posicionarse
ServoOut servos, outon '1.6mSeg. (señal a On)
WaitUs outoff2 'Tiempo de la señal a Off
Next n
End Proc
'End