' PumpController V2
'
' Cascaded pressure/current controller with supply-current limiting,
' pressure-control fault detection, undervoltage shutdown, and rough pump-speed
' monitoring using estimated motor back EMF.
'
' AI1: 4-20 mA pressure sensor through 100 ohms
'      400 mV = 0 psi
'     2000 mV = 30 psi
' AI2: turbidity input
' Motor 1: pump
'
' Controller rates:
'   Fast loop:       1 kHz
'   Pressure loop:   100 Hz
'   Telemetry:         1 Hz
'
' VAR1  = pressure setpoint, 0.1 psi
' VAR2  = pressure Kp, Q8 deciamp/mV
' VAR3  = pressure integral shift
' VAR4  = pressure filter shift
' VAR5  = motor-current Kp, PWM/deciamp
' VAR6  = maximum motor current, deciamps
' VAR7  = maximum supply current, deciamps
' VAR8  = supply-current filter shift
' VAR9  = supply-limit attack, PWM/deciamp error/ms
' VAR10 = supply-limit release, PWM/ms
' VAR11 = startup pressure-fault timeout, 10 ms ticks
' VAR12 = running pressure-fault timeout, 10 ms ticks
' VAR13 = round-trip pump-wire resistance, milliohms
' VAR14 = slow/high back-EMF threshold, decivolts
' VAR15 = fast/high back-EMF threshold, decivolts
' VAR16 = back-EMF trip accumulator limit
' VAR17 = allowable pressure error band, 0.1 psi
' VAR18 = pressure-sensor low-fault threshold, millivolts

#define NO_ERROR 0
#define UNDER_VOLTAGE_CODE 1
#define OVER_CURRENT_CODE 2
#define OVER_BACK_EMF_CODE 3
#define PRESSURE_LOW_CONTROL_CODE 4
#define PRESSURE_HIGH_CONTROL_CODE 5
#define PRESSURE_SUPPLY_CURRENT_LIMIT_CODE 6
#define PRESSURE_SENSOR_CODE 7

dim sp as integer
dim pf as integer
dim e as integer
dim pk as integer
dim ps as integer
dim fs as integer

dim ck as integer
dim imax as integer
dim isum as integer
dim icmd as integer
dim ie as integer
dim imot as integer
dim pterm as integer
dim isumax as integer
dim maxcmd as integer

dim pwm as integer
dim pwmreq as integer
dim pwmcap as integer

dim ibatf as integer
dim ilim as integer
dim iberr as integer
dim ibfs as integer
dim ibatk as integer
dim ibrel as integer

dim ef as integer
dim errorBandQ8 as integer
dim pressureFaultCount as integer
dim pressureFaultLimit as integer
dim pressureFaultDirection as integer
dim startup as integer
dim pressureSensorLowMv as integer

' Shared pressure-conversion arguments and results
dim convertDpsi as integer
dim convertMv as integer
dim convertQ8Mv as integer
dim setpointMv as integer
dim measuredDpsi as integer

dim bvolt as integer
dim wireResistance as integer
dim backEMF as integer
dim emfSlow as integer
dim emfFast as integer
dim emfCount as integer
dim emfLimit as integer

dim turbidity as integer
dim onoff as integer
dim counter as integer
dim errorCode as integer
dim errorValue as integer

setcommand(_MS,1)

print("\n\rStarting V2")
for counter=10 andwhile counter >= 0 evaluate counter -= 1
    wait(1000)
    print(" ",counter)
next counter
print("\n\r")

' Default tuning and protection values
setcommand(_VAR,1,50)       ' 5.0 psi
setcommand(_VAR,2,1)        ' pressure Kp
setcommand(_VAR,3,2)        ' pressure integral shift
setcommand(_VAR,4,3)        ' pressure filter alpha = 1/8
setcommand(_VAR,5,100)      ' motor-current Kp
setcommand(_VAR,6,200)      ' 20.0 A maximum motor current
setcommand(_VAR,7,85)       ' 8.5 A supply-current limit
setcommand(_VAR,8,2)        ' battery-current filter alpha = 1/4
setcommand(_VAR,9,4)        ' fast supply-limit attack
setcommand(_VAR,10,1)       ' slow supply-limit release
setcommand(_VAR,11,6000)    ' 60 s startup pressure-fault timeout at 100 Hz
setcommand(_VAR,12,1000)    ' 10 s running pressure-fault timeout at 100 Hz
setcommand(_VAR,13,180)     ' 0.180 ohm round-trip wire resistance
setcommand(_VAR,14,140)     ' 10.0 V slow/high back-EMF threshold
setcommand(_VAR,15,150)     ' 12.0 V fast/high back-EMF threshold
setcommand(_VAR,16,30000)   ' trip accumulator limit
setcommand(_VAR,17,10)      ' +/-1.0 psi allowable pressure error
setcommand(_VAR,18,300)     ' pressure-sensor fault below 300 mV

' Initial parameters required before entering the fast loop
sp = getvalue(_VAR,1)
pk = getvalue(_VAR,2)
ps = getvalue(_VAR,3)
fs = getvalue(_VAR,4)
ck = getvalue(_VAR,5)
imax = getvalue(_VAR,6)
ilim = getvalue(_VAR,7)
ibfs = getvalue(_VAR,8)
ibatk = getvalue(_VAR,9)
ibrel = getvalue(_VAR,10)

onoff = 0
counter = 9                 ' force parameter update on first pass
isum = 0
icmd = 0
pwm = 0
pwmreq = 0
pwmcap = 300                ' soft-start initial PWM ceiling
pressureFaultCount = 0
pressureFaultDirection = 0
startup = 0
emfCount = 0
backEMF = 0

' Initialize filters from present inputs
pf = getvalue(_AI,3)
ibatf = getvalue(_BATAMPS,1)

' Initialize signed Q8 pressure error from the present setpoint and pressure.
' This prevents the startup timeout from being cleared by an initial ef value of zero.
convertDpsi = sp
gosub pressureToMv
setpointMv = convertMv
e = setpointMv - pf
ef = e << 8

top:

' Toggle output once per 1 ms loop: 500 Hz square wave
if onoff = 0 then
    setcommand(_D0,1)
    onoff = 1
else
    setcommand(_D1,1)
    onoff = 0
end if

' Fast-loop filtering at 1 kHz
pf = pf + ((getvalue(_AI,3) - pf) >> fs)
ibatf = ibatf + ((getvalue(_BATAMPS,1) - ibatf) >> ibfs)

counter++

if counter mod 10 = 0 then

    ' Refresh tunable parameters and run pressure loop at 100 Hz
    sp = getvalue(_VAR,1)
    pk = getvalue(_VAR,2)
    ps = getvalue(_VAR,3)
    fs = getvalue(_VAR,4)
    ck = getvalue(_VAR,5)
    imax = getvalue(_VAR,6)
    ilim = getvalue(_VAR,7)
    ibfs = getvalue(_VAR,8)
    ibatk = getvalue(_VAR,9)
    ibrel = getvalue(_VAR,10)

    if startup = 0 then
        pressureFaultLimit = getvalue(_VAR,11)
    else
        pressureFaultLimit = getvalue(_VAR,12)
    end if
    wireResistance = getvalue(_VAR,13)
    emfSlow = getvalue(_VAR,14)
    emfFast = getvalue(_VAR,15)
    emfLimit = getvalue(_VAR,16)

    ' VAR17 is entered in deci-psi. Convert the pressure difference
    ' to signed-Q8 sensor millivolts; no 400 mV offset applies.
    convertDpsi = getvalue(_VAR,17)
    gosub pressureDifferenceToQ8Mv
    errorBandQ8 = convertQ8Mv
    pressureSensorLowMv = getvalue(_VAR,18)

    bvolt = getvalue(_V,2)

    ' Telemetry at 1 Hz:
    ' setpoint, pressure, battery current, motor power, turbidity, back EMF
    if counter >= 1000 then
        turbidity = getvalue(_AI,2)

        ' Check the filtered pressure signal for a failed 4-20 mA loop.
        if pf < pressureSensorLowMv then
            errorCode = PRESSURE_SENSOR_CODE
            errorValue = pf
            goto halt
        end if

        convertMv = pf
        gosub mvToPressure
        measuredDpsi = convertDpsi
        print(sp,",",measuredDpsi,",",ibatf,",",getvalue(_MOTPWR,1),",",turbidity,"\n\r")
        counter = 0
    end if

    ' Undervoltage is reported in decivolts
    if bvolt < 100 then
        errorCode = UNDER_VOLTAGE_CODE
        errorValue = bvolt
        goto halt
    end if

    ' Convert the deci-psi setpoint to absolute sensor millivolts.
    convertDpsi = sp
    gosub pressureToMv
    setpointMv = convertMv

    ' Signed pressure error in millivolts.
    ' Positive: pressure below setpoint. Negative: pressure above setpoint.
    e = setpointMv - pf

    ' Signed Q8 pressure-error filter. At 100 Hz, shift 9 gives a
    ' time constant of approximately 5.12 seconds.
    ef = ef + (((e << 8) - ef) >> 9)

    ' Pressure-control fault detection using one symmetric error band and
    ' one counter. Reset the counter when the error returns inside the band
    ' or changes direction. Latch supply-current limiting only during a
    ' positive-error interval.
    if ef > errorBandQ8 then
        if pressureFaultDirection = 1 then
            pressureFaultCount++
        else
            pressureFaultDirection = 1
            pressureFaultCount = 1
        end if

    else
        if ef < -errorBandQ8 then
            if pressureFaultDirection = -1 then
                pressureFaultCount++
            else
                pressureFaultDirection = -1
                pressureFaultCount = 1
            end if
        else
            pressureFaultCount = 0
            pressureFaultDirection = 0
            startup = 1
        end if
    end if

    if pressureFaultCount > pressureFaultLimit then
        if pressureFaultDirection > 0 then
            if pwmcap < pwmreq then
                errorCode = PRESSURE_SUPPLY_CURRENT_LIMIT_CODE
            else
                errorCode = PRESSURE_LOW_CONTROL_CODE
            end if
        else
            errorCode = PRESSURE_HIGH_CONTROL_CODE
        end if

        errorValue = ef >> 8
        goto halt
    end if

    ' Outer pressure PI controller; current terms are Q8 deciamps
    maxcmd = imax << 8
    isumax = maxcmd << ps
    pterm = e * pk

    ' Conditional integration / anti-windup
    if e < 0 then
        isum = isum + e
    else
        if pterm + (isum >> ps) < maxcmd then
            if pwmreq < 1000 then
                if pwmreq <= pwmcap then
                    isum = isum + e
                end if
            end if
        end if
    end if

    if isum < 0 then isum = 0
    if isum > isumax then isum = isumax

    icmd = pterm + (isum >> ps)
    if icmd < 0 then icmd = 0
    if icmd > maxcmd then icmd = maxcmd

end if

' Inner motor-current P controller at 1 kHz
imot = getvalue(_MOTAMPS,1)
ie = icmd - (imot << 8)

if ie > 0 then
    pwmreq = (ie * ck) >> 8
else
    pwmreq = 0
end if

if pwmreq > 1000 then pwmreq = 1000

' Supply-current limiter at 1 kHz
iberr = ibatf - ilim

if iberr > 0 then
    pwmcap = pwmcap - (iberr * ibatk)
else
    if ibatf < ilim - 3 then
        pwmcap = pwmcap + ibrel
    end if
end if

if pwmcap < 0 then pwmcap = 0
if pwmcap > 1000 then pwmcap = 1000

' Apply the lower of requested PWM and supply-current-limited PWM
pwm = pwmreq
if pwm > pwmcap then pwm = pwmcap

' Rough motor back-EMF estimate, in decivolts.
'
' Average applied motor voltage:
'     bvolt * pwm / 1000
'
' Estimated motor current from battery current:
'     ibatf * 1000 / pwm
'
' Wire drop in decivolts:
'     ibatf * wireResistance / pwm
'
' Do not divide when PWM is zero. At very low PWM, this estimate is noisy
' and should not be treated as an accurate speed measurement.
if pwm > 0 then
    backEMF = (bvolt * pwm / 1000) - (ibatf * wireResistance / pwm)
else
    backEMF = 0
end if

' High back-EMF persistence accumulator.
' Above the fast threshold, time accumulates ten times faster.
' Below the slow threshold, the accumulator resets.
if backEMF > emfSlow then
    if backEMF > emfFast then
        emfCount = emfCount + 10
    else
        emfCount++
    end if
else
    emfCount = 0
end if

if emfCount > emfLimit then
    errorCode = OVER_BACK_EMF_CODE
    errorValue = backEMF
    goto halt
end if

setcommand(_GO,1,pwm)

wait(1)
goto top

halt:
    ' Hard fault: stop motor and latch fault until controller reset
    setcommand(_ESTOP,1)
    setcommand(_GO,1,0)

haltloop:
    print("ERROR:",errorCode,":",errorValue,"\n\r")
    wait(1000)
    goto haltloop

' Pressure conversion subroutines
'
' Sensor calibration:
'     400-2000 mV = 0-300 deci-psi
'
' Shared arguments/results:
'     pressureToMv:
'         input  convertDpsi    absolute pressure, deci-psi
'         output convertMv      sensor signal, mV
'
'     mvToPressure:
'         input  convertMv      sensor signal, mV
'         output convertDpsi    absolute pressure, deci-psi
'
'     pressureDifferenceToQ8Mv:
'         input  convertDpsi    pressure difference, deci-psi
'         output convertQ8Mv    sensor-signal difference, Q8 mV
'
' The pressure-difference conversion does not include the 400 mV offset.

pressureToMv:
    convertMv = 400 + (convertDpsi * 16 / 3)
    return

mvToPressure:
    convertDpsi = (convertMv - 400) * 3 / 16
    return

pressureDifferenceToQ8Mv:
    convertQ8Mv = convertDpsi * 4096 / 3
    return