Declare Warnings = OFF
Device 18F4431
Xtal = 20
'include "modedefs.bas"
'All_Digital = True
Float_Display_Type = Fast ' Use the fast floating point display library
Optimiser_Level = 2 ' 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
Dim NUM[9] As Byte
Dim I As Byte
Dim INDICE As Byte
Dim DATOE As Byte
'Dim DATOACUM1 As Word
Dim DATOACUM As Dword
Dim DATOS As Word
Dim mot_pwr As Dword
Dim recu_pos As Dword
Dim Upos As Word
Dim vel As Word
Dim pot_val As Byte
Dim Posicion As Word
Dim Posicion2 As Word
Dim posH As Byte
Dim posL As Byte
Dim posHtem As Byte
Dim FreMot As Word
Dim a As Float
Dim b As Float
Dim c As Float
Dim rt As Float
Dim eT As Float
Dim iT As Float
Dim dT2 As Float
Dim yT As Dword
Dim uT As Float
Dim iT0 As Float
Dim eT0 As Float
Dim vmax As Byte
Dim vmin As Byte
Dim Pantalla As Byte
Dim sentido2 As Byte
Dim Activo As Byte
'--------------------
ANSEL0 = %00000001
ANSEL1 = %00000000
TRISA = %00011111
LATA = %00000000
TRISB = %00000000
TRISC = %10010000
TRISD = %00000000
QEICON = %10011000
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 2000 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
recu_pos=40000
'se le asigna una posicion de inicio.
Pantalla = 1
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
posL = POSCNTL
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
'sentido2 =0
ElseIf sentido2 =2 Then
Posicion2 =Posicion2 - 1
'sentido2 =0
EndIf
EndIf
yT= 65536 * Posicion2 + Posicion
Return
'*************************************************************************************
'****************************************************************************************
calvel: 'CALCULO DEL PWM CON PID
eT = Abs(mot_pwr - yT) 'calculo del error
eT = eT * (360/1200) '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
'Activo = 1
INDICE=0
'HSerOut[activo]
DelayMS 1
ElseIf DATOE=="x" Or DATOE=="z" Then
'Activo = 0
INDICE=0
recu_pos = mot_pwr
EndIf
DATO[INDICE]=DATOE
INDICE=INDICE+1
If DATOE== "*" Then ' cuando del PC llaga un "*" interpreto el numero que recibi
If DATO[0]=="y" Then
INDICE=0
For I=2 To 7
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"
mot_pwr=DATOACUM
If mot_pwr > yT Then
sentido2 = 1
Else
sentido2 = 2
EndIf
Case "V"
If DATOACUM > vmin Then
If DATOACUM > 255 Then
vmax = 255
Else
vmax = DATOACUM
End If
EndIf
Case "v"
If DATOACUM < vmax Then
If DATOACUM < 20 Then
vmin = 20
ElseIf DATOACUM > 200 Then
vmin =200
Else
vmin = DATOACUM
End If
EndIf
Case "P"
a = DATOACUM / 10000
Case "I"
b = DATOACUM / 10000
Case "D"
c = DATOACUM / 10000
Case "q"
If DATO[2]="1" Then
Pantalla=1
ElseIf DATO[2]="2" Then
Pantalla=2
ElseIf DATO[2]="3" Then
Pantalla=3
EndIf
Case "x"
HSerOut[Dec6 yT]
DelayMS 10
End Select
'HSerOut[DATOE] 'envio lo que recibo....nomas...
'Activo = 0
EndIf
If DATO[0]=="x" Then
RCSTA.4=0
RCSTA.4=1
INDICE=0
mot_pwr=recu_pos
EndIf
EndIf
If DATO[0]=="y" Then
HSerOut[DATOE] 'envio lo que recibo....nomas...
'Else
' RCSTA.4=0
' RCSTA.4=1
EndIf
INTCON = %11000000
Context Restore