' PumpController V2
'
' Cascaded pressure/current controller with supply-current limiting,
' dry-run protection, 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 dry-run timeout, 10 ms ticks
' VAR12 = running dry-run 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

#define NO_ERROR 0
#define UNDER_VOLTAGE_CODE 1
#define OVER_CURRENT_CODE 2
#define OVER_BACK_EMF_CODE 3
#define DRY_RUN_START_CODE 4
#define DRY_RUN_RUN_CODE 5
#define PRESSURE_SENSOR_CODE 6

' Pressure corresponding to the dry-run threshold.
' Calibration: 30 psi, 4-20 mA sensor, 100-ohm burden.
#define CONST_pressDry 453
#define CONST_pressureSensorMin 300

dim sp as integer                 ' pressure setpoint, deci-psi
dim setpointMv as integer         ' converted pressure setpoint, mV
dim pressureInputMv as integer    ' raw pressure-sensor input, mV
dim pf as integer                 ' filtered pressure-sensor input, mV
dim measuredDpsi as integer       ' converted filtered pressure, deci-psi
dim convertMv as integer          ' shared conversion input/output, mV
dim convertDpsi as integer        ' shared conversion input/output, deci-psi
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 dryCount as integer
dim startup as integer
dim dryStartLimit as integer
dim dryRunLimit 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 at 100 Hz
setcommand(_VAR,12,1000)    ' 10 s at 100 Hz
setcommand(_VAR,13,180)     ' 0.180 ohm round-trip wire resistance
setcommand(_VAR,14,100)     ' 10.0 V slow/high back-EMF threshold
setcommand(_VAR,15,120)     ' 12.0 V fast/high back-EMF threshold
setcommand(_VAR,16,30000)   ' trip accumulator limit

' 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
dryCount = 0
startup = 0
emfCount = 0
backEMF = 0

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

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
pressureInputMv = getvalue(_AI,3)
pf = pf + ((pressureInputMv - 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)

    dryStartLimit = getvalue(_VAR,11)
    dryRunLimit = getvalue(_VAR,12)
    wireResistance = getvalue(_VAR,13)
    emfSlow = getvalue(_VAR,14)
    emfFast = getvalue(_VAR,15)
    emfLimit = getvalue(_VAR,16)

    bvolt = getvalue(_V,2)

    ' Telemetry and slow diagnostic checks at 1 Hz:
    ' setpoint, pressure, battery current, motor power, turbidity, back EMF
    if counter >= 1000 then

        ' A 4-20 mA sensor across 100 ohms should normally produce
        ' approximately 400-2000 mV. Check the filtered signal once
        ' per second so brief input noise does not cause a shutdown.
        if pf < CONST_pressureSensorMin then
            errorCode = PRESSURE_SENSOR_CODE
            errorValue = pf
            goto halt
        end if

        turbidity = getvalue(_AI,2)
        convertMv = pf
        gosub mvToPressure
        measuredDpsi = convertDpsi
        print(sp,",",measuredDpsi,",",ibatf,",",getvalue(_MOTPWR,1),",",turbidity,",",backEMF,"\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

    ' Dry-run detection. dryCount increments every 10 ms.
    if pf < CONST_pressDry then
        dryCount++
    else
        dryCount = 0
        startup = 1
    end if

    if startup = 0 then
        if dryCount > dryStartLimit then
            errorCode = DRY_RUN_START_CODE
            errorValue = pf
            goto halt
        end if
    else
        if dryCount > dryRunLimit then
            errorCode = DRY_RUN_RUN_CODE
            errorValue = pf
            goto halt
        end if
    end if

    ' Convert the deci-psi setpoint to the sensor's millivolt scale.
    convertDpsi = sp
    gosub pressureToMv
    setpointMv = convertMv

    ' Outer pressure PI controller; current terms are Q8 deciamps
    e = setpointMv - pf
    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:
'     mvToPressure:
'         input  convertMv
'         output convertDpsi
'
'     pressureToMv:
'         input  convertDpsi
'         output convertMv

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

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