Declare Warnings = OFF
Device 18F4431
Xtal = 20
'All_Digital = True
Float_Display_Type = Fast ' Use the fast floating point display library
Optimiser_Level = 3 ' Optimise the code
Config_Start
OSC = HS ; HS oscillator
PWRTEN = OFF ; PWRT disabled
BOREN = OFF ; Brown-out Reset disabled in hardware and software
WDTEN = OFF ; WDT disabled (control is placed on the SWDTEN bit)
MCLRE = OFF ; RE3 input pin enabled; MCLR disabled
LVP = OFF ; Disabled
Debug = OFF ; Background debugger disabled, RB6 and RB7 configured as general purpose I/O pins
Config_End
Declare Hserial_Baud = 9600 ' Set baud rate for USART 1
Declare Hserial_RCSTA = %10010000 ' Enable serial port and continuous receive
Declare Hserial_TXSTA = %00100100 ' Enable transmit and asynchronous mode
Declare Hserial_Clear = On
'
Symbol GIE = INTCON.7
Symbol CambioCW = PIR3.3
Symbol Desbordamiento = PIR3.2
Symbol Sentido = QEICON.5
INTCON = %11000000 ' activamos las interrupciones y las de periféricos
On_Interrupt GoTo serie
PIE1.5 = 1 ' activamos la interrupción de recepción de la USART
Sentido=0
Desbordamiento=0
TRISD = %00000000
Declare LCD_Type 0 ' Type of LCD Used is Alpha
Declare LCD_DTPin PORTD.4 ' The control bits B4,B5,B6,B7
Declare LCD_RSPin PORTE.0 ' RS pin on B2
Declare LCD_ENPin PORTE.1 ' E pin on B3
Declare LCD_Interface 4 ' Interface method is 4 bit
Declare CCP1_Pin PORTC.2
Declare CCP2_Pin PORTC.1
'------------Variables-------------
Dim DATO[9] As Byte 'arreglo para recibir caracteres del puerto serie
Dim NUM[9] As Byte 'arreglo para interpretar numeros del RS232
Dim I As Byte
Dim INDICE As Byte 'variable para el index de posicion de datos
Dim DATOE As Byte 'variable para caracter recibido en rs232
Dim DATOACUM As Dword 'V_ para numero recibido por el RS232
Dim DATOS As Word
Dim mot_pwr As Dword ' set point de posicion
Dim recu_pos As Dword 'para recuperar posicion
Dim Upos As Word ' ultima posicion
Dim vel As Word 'velocidad del motor en pwm
Dim pot_val As Byte
Dim Posicion As Word 'posicion real en 16 bits
Dim Posicion2 As Word 'posicion real en 32 bits
Dim posH As Byte
Dim posL As Byte
Dim posHtem As Byte
Dim FreMot As Word 'frecuencia para el pwm "motor"
Dim a As Float 'parametro proporcional del PID
Dim b As Float 'parametro integral del PID
Dim c As Float 'parametro derivativo del PID
Dim rt As Float
Dim eT As Float
Dim iT As Float
Dim dT2 As Float
Dim yT As Dword
Dim uT As Float 'salida del PID en PWM
Dim iT0 As Float
Dim eT0 As Float
Dim vmax As Byte 'velocidad maxima del motor en PWM
Dim vmin As Byte 'velocidad minima del motor en PWM
Dim Pantalla As Byte
Dim sentido2 As Byte
Dim Activo As Byte
'--------------------
ANSEL0 = %00000001 'configuracion de E/S analogicas
ANSEL1 = %00000000
TRISA = %00011111 'configuracion de entradas digitales para el encoder
LATA = %00000000
TRISB = %00000000
TRISC = %10010000
TRISD = %00000000
QEICON = %10011000 'conteo X4, se resetea en 65535 (desborda), sin INDEX
PORTC.0 = 0 '1
PORTC.1 = 0
PORTD.0 =1
Print Cls
Print At 1,1," CARGANDO"
Print At 2,1," PARAMETROS"
DelayMS 10
PORTD.0=0
FreMot = 16000 'frecuencia para el hpwm
PORTC.0 = 0
PORTC.3 = 0
Upos=1
Posicion= 0 'variable posicion real
Posicion2=0
POSCNTH = 156 'para tener un 40000 en posicion
POSCNTL = 64
vmin= 75 'pwm minimo para el motor
vmax= 180 'pwm maximo para el motor
iT0= 0 'variable para calulo de termino derivarivo
eT0= 0 'variable para calculo del error
a = 0.50 'parametro proporcional
b = 0.0009 'parametro integral
c = 0.050 'parametro derivativo
INDICE=0
DATOE=0
DATOACUM=0
mot_pwr = 40000 'se le asigna una posicion de inicio.
recu_pos=40000
Pantalla = 1 'pantalla de inicio de LCD
sentido2= 0
Activo=0
'*********************************************************************
'********************PROGRAMA PRINCIPAL********************************
'*********************************************************************
lop:
GoSub CalPos 'ir a calcular la posicion actual
GoSub calvel 'calcular PWM para el motor
If mot_pwr = yT Then 'se borra el acumulado del error
iT0=0
eT0=0
sentido2 =0
HPWM 1, 0, FreMot 'no sale PWM por CCP1
HPWM 2, 0, FreMot 'por CCP2 no sale nada
EndIf
If mot_pwr > yT Then 'si set point es mayor que posicion actual
HPWM 1, vel, FreMot 'sale PWM por CCP1
HPWM 2, 0, FreMot 'por CCP2 no sale nada
ElseIf mot_pwr < yT Then 'en caso contrario posicion actual > set point
HPWM 2, vel, FreMot 'no sale nada por CCP1
HPWM 1, 0, FreMot 'sale pwm por CCP2
EndIf
' GoSub lcd 'muestra informacion en LCD
GoTo lop
End
' ***********************************************************************************
'*****************FIN PROGRAMA PRINCIPAL*********************************************
'************************************************************************************
lcd: 'sub que muestra en lcd
If Pantalla = 1 Then
Print At 1,1, "p=",Dec5 mot_pwr," v=", Dec3 vel'Dec3 vel
Print At 2,1, "R=",Dec5 Posicion ," e=", Dec1 eT
'HSerOut["pos=", dec yt, "*",13]
'delayms 10
ElseIf Pantalla = 2 Then
Print At 1,1, "P",Dec4 a," I", Dec4 b 'Dec3 vel
Print At 2,1, "D",Dec4 c ," e=", Dec1 eT
ElseIf Pantalla = 3 Then
Print At 1,1, "p=",Dec5 mot_pwr," P", Dec3 a'Dec3 vel
Print At 2,1, "R=",Dec5 Posicion ," I", Dec4 b
EndIf
Return
'**********************************************************************************
CalPos: 'sub para calcular posicion
posH = POSCNTH 'registros de posicion del modulo
posL = POSCNTL 'QEI del micro
posHtem = POSCNTH
If posH - posHtem = 0 Then GoTo Listo
posH = POSCNTH
posL = POSCNTL
Listo:
Posicion = 256*posH + posL 'se convierte en 16 bit la pos
If Desbordamiento = 1 Then
Desbordamiento= 0
If sentido2 = 1 Then
Posicion2 =Posicion2 + 1 'SE CONVIERTE EN 32 BITS
'sentido2 =0
ElseIf sentido2 =2 Then
Posicion2 =Posicion2 - 1
'sentido2 =0
EndIf
EndIf
yT= 65536 * Posicion2 + Posicion
Return
'*******************************************************************************
'****************************P*I*D**********************************************
calvel: 'CALCULO DEL PWM CON PID
eT = Abs(mot_pwr - yT)'calculo del error
eT = eT * (360/2000) 'ESCALAMOS: 360 grados es a 2000 pulsos del encoder
iT = b*eT + iT0 'calculo de valor integral (magnitud del error)
dT2 = c * (eT - eT0) 'calculo del valor derivativo (tiempo de respuesta)
uT = iT + a * eT
uT = uT + dT2 'valor del PID
If uT> vmax Then 'si la salida del PID es mayor que el valor de PWM
uT = vmax 'que puedo mandar asignale el valor 255
Else
If uT< vmin Then uT=vmin 'PWM minimo que quiero enviar
EndIf
vel=uT 'velocidad del motor en PWM
iT0=iT
eT0=eT
Return
'*********************interrupcion PUERTO SERIE ********************************
serie:
Context Save
HSerIn [DATOE] ' recibo el caracter de la pc por el rs232
RCSTA.4=0
RCSTA.4=1
If DATOE=="y" Then ' lo que llega por el RS232 es para este micro
INDICE=0 'EN UN MICRO AQUI LE PONGO "X" Y EN OTRO LE PONGO "Y"
DelayMS 1
ElseIf DATOE=="x" Or DATOE=="z" Then 'lo que llega por el RS232 no es
INDICE=0 'para este micro
recu_pos = mot_pwr 'almaceno la posicion actual
EndIf
DATO[INDICE]=DATOE 'se almacena el dato en el arreglo
INDICE=INDICE+1 'para el siguiente dato incremento el indice
If DATOE== "*" Then ' cuando del PC llaga un "*" interpreto el numero que recibi
If DATO[0]=="y" Then 'TRABAJA ESTE MICRO y no otros en la red
INDICE=0
For I=2 To 7 ' guardo los datos en un arreglo
Select Case DATO[I] 'dato que recibi
Case 48 ' es un cero?
NUM[I]=0
Case 49 ' es un uno?
NUM[I]=1
Case 50 ' es un dos?
NUM[I]=2
Case 51 'creo que ya entendieron....
NUM[I]=3
Case 52
NUM[I]=4
Case 53
NUM[I]=5
Case 54
NUM[I]=6
Case 55
NUM[I]=7
Case 56
NUM[I]=8
Case 57 ' es un nueve?
NUM[I]=9
Case Else 'CUALQUIER OTRO CARACTER LO TOMA COMO CERO "O"
NUM[I]=0
End Select
Next I
'aqui determino que numero me enviaron por el PC
'DATOACUM1=NUM[7] + NUM[6]*10 + NUM[5]*100 + NUM[4]*1000 + NUM[3]*10000
DATOACUM = NUM[7] + NUM[6]*10
DATOACUM=DATOACUM + NUM[5]*100
DATOACUM =DATOACUM + NUM[4]*1000
DATOACUM=DATOACUM + NUM[3]*10000
DATOACUM=DATOACUM + NUM[2]*100000
Select Case DATO[1]
Case "p" ' recibio un cambio de posicion
mot_pwr=DATOACUM
If mot_pwr > yT Then
sentido2 = 1
Else
sentido2 = 2
EndIf
Case "V" 'recibio un cambio de velocidad maxima
If DATOACUM > vmin Then
If DATOACUM > 255 Then
vmax = 255
Else
vmax = DATOACUM
End If
EndIf
Case "v" 'recibio un cambio de velocidad minima
If DATOACUM < vmax Then
If DATOACUM < 20 Then
vmin = 20
ElseIf DATOACUM > 200 Then
vmin =200
Else
vmin = DATOACUM
End If
EndIf 'recibio un cambio de parametro proporcional
Case "P"
a = DATOACUM / 10000
Case "I" 'recibio un cambio de parametro integral
b = DATOACUM / 10000
Case "D" 'recibio un cambio de parametro derivativo
c = DATOACUM / 10000
Case "q" 'recibio un cambio de pantalla a mostrar en LCD
If DATO[2]="1" Then
Pantalla=1
ElseIf DATO[2]="2" Then
Pantalla=2
ElseIf DATO[2]="3" Then
Pantalla=3
EndIf
Case "x" 'envia la posicion actual por rs232
HSerOut[Dec6 yT]
DelayMS 10
End Select
EndIf
If DATO[0]=="x" Then 'NO TRABAJA ESTE MICRO EN LA RED
RCSTA.4=0
RCSTA.4=1
INDICE=0
mot_pwr=recu_pos ' para que mantenga la posicion
EndIf
EndIf
If DATO[0]=="y" Then
HSerOut[DATOE] 'envio lo que recibo....nomas...
EndIf
INTCON = %11000000
Context Restore
justo en estos dias anduve pensando en hacer una practica con un motor DC y un encoder hecho apartir de un muse de pc de esos de bolita para ver si de una ves por todas puedo entenderlo... hice el intento varias veces de hacer un servo apartir de un motor dc con reduccion y un potenciometro pero nunca me salio ya que no estaba implementando el sistema P.I.D por no entender como funciona... saludosss 
