PID-filter routine (2nd try).

[Ioannis]

Here is my first try on PID control. Obviously failed...

At the attached schematic is the part of the circuit, essentialy a Voltage stabilizer. The PWM from the PIC drives the Power stage while from the output a voltage sample is returned to the ADC.

Unfortunately there is no control at all. When Setpoint is 0 or 1023 the output is always high (pid_out is 511).

Also I noticed that Direction bit is high when Voltage sample is higher than Setpoint. Is this correct?

Test PIC is F887 driving the internal HPWM.

'**************************************************************************
'**************************************************************************
'**		
'**		PWM Controller with voltage level feedback
'**
'**     21.11.2010, v.1.0
'**
'**     PID Control by Henrik Olsson
'**************************************************************************
'**************************************************************************

DEFINE OSC 8

OSCCON = %01110001 'SET SYSTEM CLOCK SWITCH BIT

;----- Configuration bits ------------------------------------------------

@Line1 = _DEBUG_OFF & _LVP_OFF & _FCMEN_OFF & _IESO_OFF & _BOR_ON
@Line2 = _CP_OFF & _MCLRE_OFF & _PWRTE_ON & _WDT_ON & _INTRC_OSC_NOCLKOUT

@ __CONFIG _CONFIG1, Line1 & Line2

@ __CONFIG _CONFIG2, _WRT_HALF & _BOR40V

            	
PORTA=0:PORTB=0:PORTC=0:PORTD=0:PORTE=0

TRISA=%00101101
TRISB=%00000000
TRISC=%10000000
TRISD=%00000011
TRISE=%00000000

OPTION_REG.0=1		'PSA0 PRESCALER SELECT 1:1 TO 1:256
OPTION_REG.1=1		'PSA1
OPTION_REG.2=1		'PSA2
OPTION_REG.3=0		'PRESCALER TO: 1->WDT, 0->TMR0
OPTION_REG.4=0		'T0SE SOURCE EDGE 1->H TO L, 0->L TO H
OPTION_REG.5=0		'T0CS 1->FROM RA4, 0->FROM INT. CLOCK
OPTION_REG.6=0		'INT EDGE SELECT 0->H TO L, 1->L TO H
OPTION_REG.7=0		'PULL UP 1->DISABLE, 0->ENABLE

WPUB = %00111111

'DEFINE HSER_RCSTA 90h
'DEFINE HSER_TXSTA 24h
'DEFINE HSER_BAUD 19200
'DEFINE HSER_CLROERR 1 ' Clear overflow automatically

'************************* Define LCD parameters

ADCON0 = %11000001
ADCON1 = %10000000	'Set PORTA/E analog/digital, right justify result
ANSEL  = %00101101  'lower port A as Analog input
ANSELH = %00000000  'all others Digital inputs

DEFINE ADC_BITS 10  'Number of bits in Adcin result
DEFINE ADC_CLOCK 3  'ADC clock source (rc = 3)
DEFINE ADC_SAMPLEUS 50
                     
but_0           var portd.0
but_1           var portd.1


'**************************************************************************
'****
'****                            Initialization
'****
'**************************************************************************
clear

ADValue VAR WORD                     '<---This is your variable.
Setpoint VAR WORD                    '<---This is your variable.
Direction var PortB.7                '<---Direction bit to motor driver 

p_gain      var word
i_gain      var word
d_gain      var word

Setpoint=200

INCLUDE "incPIDv1.5.pbp"                 'Include the PID routine.

    'These variables are declared by the incPID routine but
    'the user needs to assign values to them.
'    pid_Kp = $0700                     'Set Kp to 7.0
'    pid_Ki = $0080                     'Set Ki to 0.5
'    pid_Kd = $0225                     'Set Kd to 2.14
    pid_Ti = 8                         'Update I-term every 8th call to PID
    pid_I_Clamp = 100                  'Clamp I-term to max ±100
    pid_Out_Clamp = 511                'Clamp the final output to ±511
     
  Start:
    Gosub analog                        'Get speed from tacho and setpoint pot - NOT SHOWN HERE.
    
    pid_Kp=p_gain:pid_Ki=i_gain:pid_Kd=d_gain
                                     
    pid_Error = Setpoint - value     'Calculate the error

    Gosub PID                          'Result returned in pid_Drive

    Direction = pid_Out.15             'Set direction pin accordning to sign
    pid_Out = ABS pid_Out              'Convert from two's comp. to absolute
    HPWM 1, pid_Out, 500             'Set PWM output
    
    'Just for monitoring
    serout2 portc.6,16468, [27,"[10;0H","P:",dec5 p_gain,"  I:",dec5 i_gain,"  D:",dec5 d_gain,"  PID out:",#pid_out,"    ",27,"[12;0H","Vout:",dec5 value,"  SP:",dec5 setPoint,"  pid_err:",dec5 pid_error, 27,"[14;0H","Direction: ",#direction]

Goto Start                           '...and do it again.

analog:
    adcin 0,value
    gosub average
    adcin 2,p_gain
    p_gain=p_gain<<2
    adcin 3,i_gain
    i_gain=i_gain>>2
    adcin 4,d_gain
    d_gain=d_gain<<2
    
    'Two digital inputs for setting Setpoint level
    if !but_0 then
        if Setpoint>0 then
            setpoint=setpoint-2
        endif
    endif
    if !but_1 then
        if Setpoint<1021 then
            setpoint=setpoint+2
        endif
    endif 
return

END

I am using 3 Analog inputs to set the P,I,D values as it is clear from the analog subroutine.

Whatever the values the pid_out stays at 511 and plays a litle at the point where Setpoint is almost equal to the Vout level.
Any inputs welcome.

Ioannis