Ahora el PID
2 participantes
Página 1 de 1.
Ahora el PID
Necesito una ayuda de quien ha construido un control motores multiples.el bus de comunicaciòn entre pic funciona,màs los motores no giran perfectamente sincronisados.he pensado,mejor dicho ya construi,de lectores de giro de las ruedas con unos CNY70 (leo perfectamente los impulsos a la salida).màs no se como gestirlos en interrupt con GCBASIC y TMR0
Alguien tiene algun ejemplo de dos o mas motores???
Alguien tiene algun ejemplo de dos o mas motores???
alejandro- Participante Activo
- Mensajes : 87
Fecha de inscripción : 12/02/2009
Re: Ahora el PID
Hola Alejandro.
Por empezar por algo: una idea general de cómo lo haría yo (creo):
-Configurar interrupciones por puerto B y conectas el encoder a dos pines del puerto B, en cada interrupcion compruebas qué pin ha cambiado y llevas la cuenta en dos contadores (tics1 y tics2):
pin_cambiado = PBanterior xor PORTB 'el pin cambiado se pone a 1
if pin_cambiado = 1 then tics1 += 1 'encoder1 conectado a PORTB.0
if pin_cambiado = 2 then tics2 += 1 'encoder2 conectado a PORTB.1
if pin_cambiado = 3 then tic21 +=1 : tics2 += 1 'los dos han cambiado
PBanterior = PORTB
Para encoders conectados a PORTB.3 y PORTB.6 (por ejemplo):
if pin_cambiado = 8 then tics1 += 1 'encoder1 conectado a PORTB.3
if pin_cambiado = 64 then tics2 += 1 'encoder2 conectado a PORTB.6
if pin_cambiado = 72 then tic21 +=1 : tics2 += 1 'los dos han cambiado
- Configurar Timer0 para un periodo que te venga bién... por ejemplo 100 ms (o 500 ms o lo que tu veas). Si no te llega a 100 ms, configuras a 10 ms y pones un contador x10.
Cada vez que llegue a 100 ms llamas al PID.
Pones tics1 y tics2 a 0.
_____________________________________________________________________________________________
Para el PID:
Creo que en la variable: Comando tienes las velocidades "objetivo" de los motores solo deberías pasarlas a dos variables (una para cada motor). Por ejemplo vel1 y vel2
tics1 y tics2 son las velocidades reales de los motores; solo tienes que adaptar las unidades de vel y tics... esto quizás es lo más complicado de averiguar.
El periodo sabes que es 100 ms.
Ya solo necesitas buscar unos valores adecuados para las constantes y ya tienes todos los datos para el PID.
Un posible problema es cómo afectan las interrupciones a las comunicaciones. El PID debería ser lo más sencillo posible.
Saludos.
Por empezar por algo: una idea general de cómo lo haría yo (creo):
-Configurar interrupciones por puerto B y conectas el encoder a dos pines del puerto B, en cada interrupcion compruebas qué pin ha cambiado y llevas la cuenta en dos contadores (tics1 y tics2):
pin_cambiado = PBanterior xor PORTB 'el pin cambiado se pone a 1
if pin_cambiado = 1 then tics1 += 1 'encoder1 conectado a PORTB.0
if pin_cambiado = 2 then tics2 += 1 'encoder2 conectado a PORTB.1
if pin_cambiado = 3 then tic21 +=1 : tics2 += 1 'los dos han cambiado
PBanterior = PORTB
Para encoders conectados a PORTB.3 y PORTB.6 (por ejemplo):
if pin_cambiado = 8 then tics1 += 1 'encoder1 conectado a PORTB.3
if pin_cambiado = 64 then tics2 += 1 'encoder2 conectado a PORTB.6
if pin_cambiado = 72 then tic21 +=1 : tics2 += 1 'los dos han cambiado
- Configurar Timer0 para un periodo que te venga bién... por ejemplo 100 ms (o 500 ms o lo que tu veas). Si no te llega a 100 ms, configuras a 10 ms y pones un contador x10.
Cada vez que llegue a 100 ms llamas al PID.
Pones tics1 y tics2 a 0.
_____________________________________________________________________________________________
Para el PID:
Creo que en la variable: Comando tienes las velocidades "objetivo" de los motores solo deberías pasarlas a dos variables (una para cada motor). Por ejemplo vel1 y vel2
tics1 y tics2 son las velocidades reales de los motores; solo tienes que adaptar las unidades de vel y tics... esto quizás es lo más complicado de averiguar.
El periodo sabes que es 100 ms.
Ya solo necesitas buscar unos valores adecuados para las constantes y ya tienes todos los datos para el PID.
Un posible problema es cómo afectan las interrupciones a las comunicaciones. El PID debería ser lo más sencillo posible.
Saludos.
Re: Ahora el PID
genial........y simple.
fijate si es + o - asi
PD:estàn un poco apretados en la sub del interrupt,no????
fijate si es + o - asi
- Código:
''''''''''''''' CONFIGURO EL PIC '''''''''''''''''''''''''''''''''''''''''''''''''
#chip 16F876A,20
#config OSC=HS,WDT_OFF,PWRTE_ON,CP_OFF,DEBUG_OFF,WRT_OFF,BODEN_OFF,CPD_OFF,LVP_OFF
''''''''''''''' VARIABLES ''''''''''''''''''''''''''''''''''''''''''''''''''''''''
DIM Odom_km,Impulso_Odom AS WORD
''''''''''''''' DECLARA EL INTERRUPT '''''''''''''''''''''''''''''''''''''''''''''
On Interrupt Timer0Overflow Call Control_Interrupt()
On Interrupt PORTBChange Call Control_Interrupt()
On Interrupt SSP1Ready Call Control_Interrupt()
''''''''''''''' SUB PROCEDURAS '''''''''''''''''''''''''''''''''''''''''''''''''''
SUB Control_Interrupt()
DIM Impulso_A,Impulso_B,Clear_Buffer AS BYTE
IF PIR1.SSPIF=1 THEN 'I2C slave subroutine
PIR1.SSPIF=0 'Limpia el flag del interrupt
IF SSPSTAT.D_A=1 THEN 'Dato,no ID_Address
Comando=SSPBUF 'Lee el dato del master
SSPCON.CKP=1 'Libera el clock
END IF
Clear_Buffer=SSPBUF 'Limpia el buffer del byte address
END IF
IF INTCON.TMR0IF=1 THEN
Inc(Contador_PID)
'el cristal de 20mhz no me permite lecturas superior a 13,107 ms
'el TMR0 es impostado para obtener un interrupt a 10 ms
IF Contador_PID=50 Then
PID(Impulso_A,Impulso_B)
Impulso_A=0
Impulso_B=0
END IF
TMR0=0x3F
INTCON.TMR0IF=0
END IF
IF INTCON.RBIF=1 THEN
IF PORTB.0=1 THEN Impulso_A += 1 'encoder1 conectado a PORTB.0
IF PORTB.1=1 THEN Impulso_B += 1 'encoder2 conectado a PORTB.1
IF PORTB.0=1 OR PORTB.1=1 THEN Impulso_Odom +=1
IF Impulso_Odom=64800 THEN Odom_km +=1 : Impulso_Odom=0
INTCON.RBIF=0
END IF
END SUB
SUB PID(VReal_1 AS BYTE,VReal_2 AS BYTE)
''''''''''''''''''''''''''''''
''Sigue el algoritmo del PID''
''''''''''''''''''''''''''''''
END SUB
''''''''''''''' INICIA ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Main:
''''''''''''''' PARAMETROS ''''''''''''''''''''''''''''''''''''''''''''''''''''''''
ADCON1=b'10000010' 'Puerto A 0/1/2/3/5 son analogicos
TRISA=b'000000' 'Puerto A todos out
TRISB=b'00110000' 'Puerto B 4 y 5 son input,las demas output
TRISC=b'00011000' 'Puerto C 3 y 4 son input,las demas output
PORTA=b'00000' 'Puerto A son bajas
PORTB=b'00000000' 'Puerto B son bajas
PORTC=b'00000000' 'Puerto C son bajas
SSPADD=0xA2 'Setaje ID_Address = 0xA2
SSPSTAT=b'11000000' 'Slew rate control deshabilitado para freq. 100Khz,controla el rango de voltaje utilizado(CKE)por el microcontrolador PICmicro cuando la recepción de señales I2C.
SSPCON=b'00110110' 'Setaje como slave,con 7 bit address
SSPCON2=b'0000000' 'Setaje SSPCON2
PIE1=b'00001000' 'Habilita los interrupts de SSP(SSPEN)
OPTION=0xC7 '11000111
INTCON=0xE8 '11101000
TMR0=0x3F 'Preload=63
DO
'Routine de los motores'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
LOOP
PD:estàn un poco apretados en la sub del interrupt,no????
alejandro- Participante Activo
- Mensajes : 87
Fecha de inscripción : 12/02/2009
Re: Ahora el PID
No he mirado el programa a fondo, pero sí hay una cosa:
LLamas a la misma función para todas las interrupciones (Control_Interrupt) y luego dentro de esa función pones IF para cada interrupción.
Esto no es necesario, GcBasic ya lo hace, entonces se repiten las comprobaciones de interrupción.
Puedes hacer esto:
GcBasic te genera una subrutina de interrupciones con IFs, algo así (pero en asm):
También hay otra cosa:
Si cambia PORTB.0 de 0 a 1, pero PORTB.1 estaba en 1 y sigue en 1 te vá a subir el contador "Impulso_B" cuando no debería... por eso pensé en usar "xor":
pin_cambiado = PBanterior xor PORTB
En la variable "pin_cambiado" todas las entradas que cambian ponen el bit a 1, si no cambian el bit está a 0 aunque me faltó poner una máscara para aislar solo los bits a los que están conectados los encoders. O comprobar sólo el bit que quieras, si está a 1 entonces sube contador.
También este sistema te cuenta el doble: cuando cambia de 0 a 1 y cuando cambia de 1 a 0, entonces tiene el doble de resolución.
Pero seguro que hay otras formas...
Saludos.
LLamas a la misma función para todas las interrupciones (Control_Interrupt) y luego dentro de esa función pones IF para cada interrupción.
Esto no es necesario, GcBasic ya lo hace, entonces se repiten las comprobaciones de interrupción.
Puedes hacer esto:
- Código:
On Interrupt Timer0Overflow Call Timer0_Interrupt()
On Interrupt PORTBChange Call PORTB_Interrupt()
On Interrupt SSP1Ready Call SSP1_Interrupt()
GcBasic te genera una subrutina de interrupciones con IFs, algo así (pero en asm):
- Código:
Interrupt
Guardar contexto
IF INTCON.TMR0IF=1 THEN Call Timer0_Interrupt()
IF INTCON.RBIF=1 THEN Call PORTB_Interrupt()
IF PIR1.SSPIF=1 THEN Call SSP1_Interrupt()
Recuperar contexto
End Interrupt
También hay otra cosa:
- Código:
IF PORTB.0=1 THEN Impulso_A += 1 'encoder1 conectado a PORTB.0
IF PORTB.1=1 THEN Impulso_B += 1 'encoder2 conectado a PORTB.1
IF PORTB.0=1 OR PORTB.1=1 THEN Impulso_Odom +=1
Si cambia PORTB.0 de 0 a 1, pero PORTB.1 estaba en 1 y sigue en 1 te vá a subir el contador "Impulso_B" cuando no debería... por eso pensé en usar "xor":
pin_cambiado = PBanterior xor PORTB
En la variable "pin_cambiado" todas las entradas que cambian ponen el bit a 1, si no cambian el bit está a 0 aunque me faltó poner una máscara para aislar solo los bits a los que están conectados los encoders. O comprobar sólo el bit que quieras, si está a 1 entonces sube contador.
También este sistema te cuenta el doble: cuando cambia de 0 a 1 y cuando cambia de 1 a 0, entonces tiene el doble de resolución.
Pero seguro que hay otras formas...
Saludos.
Re: Ahora el PID
tienes razòn,no me di cuenta
Si cambia PORTB.0 de 0 a 1, pero PORTB.1 estaba en 1 y sigue en 1
te vá a subir el contador "Impulso_B" cuando no debería... por eso
pensé en usar "xor":
era una prueva.la sub unica para los interrupts sirve tener en un solo lugar todas las llamadas.las sub de interrupts multiples es màs elegante y permite individuar màs facilmente las routines que se usan.
LLamas a la misma función para todas las interrupciones
(Control_Interrupt) y luego dentro de esa función pones IF para cada
interrupción.
Esto no es necesario, GcBasic ya lo hace, entonces se repiten las comprobaciones de interrupción.
y sub multiples seràn...........
alejandro- Participante Activo
- Mensajes : 87
Fecha de inscripción : 12/02/2009
Re: Ahora el PID
la modifica que tratare de provar lo antes posible:
- Código:
''''''''''''''' VARIABLES ''''''''''''''''''''''''''''''''''''''''''''''''''''''''
DIM Odom_km,Impulso_Odom AS WORD
DIM Impulso_A,Impulso_B AS BYTE
''''''''''''''' DECLARA EL INTERRUPT '''''''''''''''''''''''''''''''''''''''''''''
On Interrupt Timer0Overflow Call TMR0_Interrupt()
On Interrupt PORTBChange Call PORTB_Interrupt()
On Interrupt SSP1Ready Call I2C_Interrupt()
''''''''''''''' SUB PROCEDURAS '''''''''''''''''''''''''''''''''''''''''''''''''''
SUB TMR0_Interrupt()
IF INTCON.TMR0IF=1 THEN
Inc(Contador_PID)
'el cristal de 20mhz no me permite lecturas superior a 13,107 ms
'el TMR0 es impostado para obtener un interrupt a 10 ms
IF Contador_PID=50 Then
PID(Impulso_A,Impulso_B)
Impulso_A=0
Impulso_B=0
END IF
TMR0=0x3F
INTCON.TMR0IF=0
END IF
END SUB
SUB PORTB_Interrupt()
IF INTCON.RBIF=1 THEN
PORTB_New=PORTB_Old xor PORTB 'El pin cambiado se pone a 1
IF PORTB_New=16 THEN Impulso_A += 1 'Encoder_A conectado a PORTB.4
IF PORTB_New=32 THEN Impulso_B += 1 'Encoder_B conectado a PORTB.5
if PORTB_New=48 THEN Impulso_A += 1 : Impulso_B += 1 'Los dos han cambiado
IF PORTB_New=16 OR PORTB.1=32 THEN Impulso_Odom +=1
IF Impulso_Odom=64800 THEN Odom_km +=1 : Impulso_Odom=0
INTCON.RBIF=0
END IF
END SUB
SUB I2C_Interrupt()
DIM Clear_Buffer AS BYTE
IF PIR1.SSPIF=1 THEN 'I2C slave subroutine
PIR1.SSPIF=0 'Limpia el flag del interrupt
IF SSPSTAT.D_A=1 THEN 'Dato,no ID_Address
Comando=SSPBUF 'Lee el dato del master
SSPCON.CKP=1 'Libera el clock
END IF
Clear_Buffer=SSPBUF 'Limpia el buffer del byte address
END IF
END SUB
SUB PID(VReal_1 AS BYTE,VReal_2 AS BYTE)
''''''''''''''''''''''''''''''
''Sigue el algoritmo del PID''
''''''''''''''''''''''''''''''
END SUB
''''''''''''''' INICIA ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Main:
''''''''''''''' PARAMETROS ''''''''''''''''''''''''''''''''''''''''''''''''''''''''
ADCON1=b'10000010' 'Puerto A 0/1/2/3/5 son analogicos
TRISA=b'000000' 'Puerto A todos out
TRISB=b'00110000' 'Puerto B 4 y 5 son input,las demas output
TRISC=b'00011000' 'Puerto C 3 y 4 son input,las demas output
PORTA=b'00000' 'Puerto A son bajas
PORTB=b'00000000' 'Puerto B son bajas
PORTC=b'00000000' 'Puerto C son bajas
SSPADD=0xA2 'Setaje ID_Address = 0xA2
SSPSTAT=b'11000000' 'Slew rate control deshabilitado para freq. 100Khz,controla el rango de voltaje utilizado(CKE)por el microcontrolador PICmicro cuando la recepción de señales I2C.
SSPCON=b'00110110' 'Setaje como slave,con 7 bit address
SSPCON2=b'0000000' 'Setaje SSPCON2
PIE1=b'00001000' 'Habilita los interrupts de SSP(SSPEN)
OPTION=0xC7 '11000111
INTCON=0xE8 '11101000
TMR0=0x3F 'Preload=63
DO
'Routine de los motores'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
LOOP
alejandro- Participante Activo
- Mensajes : 87
Fecha de inscripción : 12/02/2009
Re: Ahora el PID
Te comento algunos detalles:
Esto no es necesario en las rutinas que atienden interruciones:
IF INTCON.TMR0IF=1 THEN
...
INTCON.TMR0IF=0
Porque eso ya lo hace GcBasic, si declaras las interruciones de eta manera:
On interrupt... Call....
GcBasic se encarga de todo, crea subrutina de interrupciones, guarda contexto, manda a subrutina según interrupción y borra flags; solo tienes que poner en las subrutinas el código que atiende la interrupción.
Por lo mismo, ten cuidado con esto:
PIE1=b'00001000' 'Habilita los interrupts de SSP(SSPEN)
INTCON=0xE8 '11101000
Estás sobreescribiendo directamente los registros de interrupción.
No necesitas habilitar interrupciones SSP, porque ya lo has hecho:
On Interrupt SSP1Ready Call I2C_Interrupt()
Al hacer esto GcBasic hace todo para que la interrupción funcione.
Saludos y suerte con ese proyecto.
Esto no es necesario en las rutinas que atienden interruciones:
IF INTCON.TMR0IF=1 THEN
...
INTCON.TMR0IF=0
Porque eso ya lo hace GcBasic, si declaras las interruciones de eta manera:
On interrupt... Call....
GcBasic se encarga de todo, crea subrutina de interrupciones, guarda contexto, manda a subrutina según interrupción y borra flags; solo tienes que poner en las subrutinas el código que atiende la interrupción.
Por lo mismo, ten cuidado con esto:
PIE1=b'00001000' 'Habilita los interrupts de SSP(SSPEN)
INTCON=0xE8 '11101000
Estás sobreescribiendo directamente los registros de interrupción.
No necesitas habilitar interrupciones SSP, porque ya lo has hecho:
On Interrupt SSP1Ready Call I2C_Interrupt()
Al hacer esto GcBasic hace todo para que la interrupción funcione.
Saludos y suerte con ese proyecto.
Re: Ahora el PID
y yo no hago nada,me quedo mirando...........................???
ok,grazias de los consejos.no lo sabia que lo hacia directamente con la llamada de interrupt.
ya montè los motores y funcionan bien sin el PID.voy a montar los CNY70 y les hago saber.
ok,grazias de los consejos.no lo sabia que lo hacia directamente con la llamada de interrupt.
ya montè los motores y funcionan bien sin el PID.voy a montar los CNY70 y les hago saber.
alejandro- Participante Activo
- Mensajes : 87
Fecha de inscripción : 12/02/2009
Temas similares
» Gedit_Pic-IDE: plugin para convertir Gedit en una IDE para PIC
» De nuevo GAMBAS o lo que gusten, pero ahora con USB CDC
» De nuevo GAMBAS o lo que gusten, pero ahora con USB CDC
Página 1 de 1.
Permisos de este foro:
No puedes responder a temas en este foro.
|
|