Estoy montando un inversor de servos y los servos conectados al PIC no se mueven con precision, van moviendose pequeños tirones, comparados con un servo conectado a la entrada.
No se si es que el comando servoin no ofrece buena lectura para pequeñas variaciones de la señal WFM.
Os pego el codigo por si creeis que se puede mejorar.
'***********************12F683****************************
'cuenta con tres salidas GP1 directa, GP2 invertida, GP5 programable.
'pulsado el sw de programacion al encender memorizamos valores con la siguiente secuencia.
'GP5, GP3, GP1, en cada una centro, valor min y valor max.
'al salir del bucle de programacion, pulsando el sw de prog, invertimos o dejamos con directo GP5
Define CONF_WORD = 0x33c4
Define CLOCK_FREQUENCY = 8
AllDigital
Dim length1 As Byte 'variable donde guardo los valores de servo1
Dim length2 As Byte 'variable donde guardo los valores de servo2
Dim length3 As Byte 'variable donde guardo los valores de servo3
Dim entrada As Byte
'**************************************
Dim t1 As Byte
Dim t2 As Byte
Dim x1 As Byte
Dim x2 As Byte
Dim valor_min1 As Byte
Dim valor_max1 As Byte
Dim valor_min2 As Byte
Dim valor_max2 As Byte
Dim valor_min3 As Byte
Dim valor_max3 As Byte
Dim contador As Byte 'variable para guardar en eprom el modo de funcionamiento DE LA SALIDA PROGRAMABLE
Dim centro1 As Byte
Dim centro2 As Byte
Dim centro3 As Byte
Dim a As Byte 'variable para ir secuenciando los pasos de programacion
Symbol led = GP4
'***************************************
Read 1, contador
Read 10, centro1
Read 11, valor_min1
Read 12, valor_max1
Read 13, centro2
Read 14, valor_min2
Read 15, valor_max2
Read 16, centro3
Read 17, valor_min3
Read 18, valor_max3
'***************************************
Config GPIO.0 = Input
Config GPIO.1 = Output
Config GPIO.2 = Output
Config GPIO.3 = Input 'sw momeria
Config GPIO.4 = Output 'led
Config GPIO.5 = Output
'valor_min = 94 '100
'valor_max = 210 '200
a = 0
led = 0
If GP3 Then Gosub prog 'envio a subrrutina de programacion
loop:
ServoIn GP0, entrada
length1 = entrada
If length1 < valor_min1 Then length1 = valor_min1 'Limito el valor mínimo
If length1 > valor_max1 Then length1 = valor_max1 'Limito el valor máximo
t1 = length1 - valor_min1
x1 = valor_max1 - t1
'***************************************
length2 = entrada
If length2 < valor_min2 Then length2 = valor_min2 'Limito el valor mínimo
If length2 > valor_max2 Then length2 = valor_max2 'Limito el valor máximo
t2 = length2 - valor_min2
x2 = valor_max2 - t2
length3 = entrada
If length3 < valor_min3 Then length3 = valor_min3 'Limito el valor mínimo
If length3 > valor_max3 Then length3 = valor_max3 'Limito el valor máximo
'configuro el modo de funcionamiento de GP5
ServoOut GP1, length3 'Salida DUPLICADA
ServoOut GP2, x2 'SALIDA INVERTIDA
If contador = 0 Then ServoOut GP5, x1 'SALIDA PROGRAMABLE INVERTIDA
If contador = 1 Then ServoOut GP5, length1
If GP3 Then
WaitMs 250
led = 1
WaitMs 250
led = 0
contador = contador + 1
If contador > 1 Then contador = 0
Write 1, contador
Endif
Goto loop
End
prog:
led = 1
WaitMs 1000
led = 0
loop1:
If GP3 Then a = a + 1
ServoIn GP0, entrada
ServoOut GP5, entrada 'Salida DUPLICADA
'MEMORIZO EL CENTRO Y LOS MAX Y MIN DE LA SALIDA GP5
If a = 1 Then
'led = 1
WaitMs 500
centro1 = entrada
'led = 0
Gosub led_lento
a = 2
Endif
If a = 3 Then
'led = 1
WaitMs 500
If entrada < centro1 Then
valor_min1 = entrada
Else
valor_max1 = entrada
Endif
'led = 0
Gosub led_lento
a = 4
Endif
If a = 5 Then
'led = 1
WaitMs 500
If entrada < centro1 Then
valor_min1 = entrada
Else
valor_max1 = entrada
Endif
'led = 0
Gosub led_lento
a = 6
Endif
''MEMORIZO EL CENTRO Y LOS MAX Y MIN DE LA SALIDA GP5
If a <> 6 Then Goto loop1
Gosub led_rapido
loop2:
If GP3 Then a = a + 1
ServoIn GP0, entrada
ServoOut GP2, entrada 'Salida DUPLICADA
If a = 7 Then
'led = 1
WaitMs 500
centro2 = entrada
'led = 0
Gosub led_lento
a = 8
Endif
If a = 9 Then
'led = 1
WaitMs 500
If entrada < centro2 Then
valor_min2 = entrada
Else
valor_max2 = entrada
Endif
'led = 0
Gosub led_lento
a = 10
Endif
If a = 11 Then
'led = 1
WaitMs 500
If entrada < centro2 Then
valor_min2 = entrada
Else
valor_max2 = entrada
Endif
'led = 0
Gosub led_lento
a = 12
Endif
If a <> 12 Then Goto loop2
'***********************HASTA AQUI PARA LA SALIDA DE SERVO GP2
'GRABO LOS VALORES MAX Y MIN DE LA SALIDA GP1. ESTA SALIDA NO INVIERTE
Gosub led_rapido
loop3:
If GP3 Then a = a + 1
ServoIn GP0, entrada
ServoOut GP1, entrada 'Salida DUPLICADA
If a = 13 Then
'led = 1
WaitMs 500
centro3 = entrada
'led = 0
Gosub led_lento
a = 14
Endif
If a = 15 Then
'led = 1
WaitMs 500
If entrada < centro3 Then
valor_min3 = entrada
Else
valor_max3 = entrada
Endif
'led = 0
Gosub led_lento
a = 16
Endif
If a = 17 Then
'led = 1
WaitMs 500
If entrada < centro3 Then
valor_min3 = entrada
Else
valor_max3 = entrada
Endif
'led = 0
Gosub led_lento
a = 18
Endif
If a <> 18 Then Goto loop3
Gosub led_rapido
'SALGO DEL BUCLE DE PROGRAMACION Y GRABO TODOS LOS VALORES EN EPROM
If a = 18 Then
led = 1
WaitMs 250
led = 0
WaitMs 250
led = 1
WaitMs 250
led = 0
Write 10, centro1
Write 11, valor_min1
Write 12, valor_max1
Write 13, centro2
Write 14, valor_min2
Write 15, valor_max2
Write 16, centro3
Write 17, valor_min3
Write 18, valor_max3
Gosub led_rapido
Endif
Return
led_rapido:
led = 1
WaitMs 100
led = 0
WaitMs 100
led = 1
WaitMs 100
led = 0
Return
led_lento:
led = 1
WaitMs 250
led = 0
Return
