'the free version has a 1k size limit
'to keep the size of this version under 1k a few things have been done:
'1) make some #define so that they are not done in code
'2) reduce the number of characters in every print statement
'3) combine halt functions into one

' Cascaded pressure/current controller with supply-current limiting
' AI1: 4-20 mA pressure sensor through 100 ohms
'      400 mV = 0 psi
'     2000 mV = 30 psi
' Motor 1: pump
'
' 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 = dry run startup in Seconds
' VAR12 = dry run during run in seconds
' VAR13 = wire resistance in milli-ohms
' VAR14 = fast back EMF value in deci-Volt
' VAR15 = slow back EMF value in deci-Volt
' VAR16 = back EMF time out in milli-seconds

#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

                     'calibration based upon a 30psi 4-20mA sensor across 100 ohm
                     'SDC2160 returns a value in millivolts not counts with a range of 0-5100mV
                     'thus 100 Ohm results in a reading range of 400-2000
#define CONST_pressDry 453 '10deciPSI to mV, this must be hand calculated




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 pwm as integer
dim pwmreq as integer
dim pwmcap as integer
dim onoff as integer
dim counter as integer
'dim pwr as integer
dim imot as integer
dim pterm as integer
dim isumax as integer
dim maxcmd as integer
dim ibat 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 errorCode as integer
dim errorValue as integer

setcommand(_MS,1)

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

' Default tuning values
setcommand(_VAR,1,50)    ' 5.0 psi
setcommand(_VAR,2,1)     ' pressure Kp = 1 Q8 count/mV
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)     ' supply-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)   ' at startup how long to run dry (Seconds)
setcommand(_VAR,12,1000)   ' how long to run dry after startup (Seconds)
setcommand(_VAR,13,180)   ' out and back resistance of pump wire in milli-ohms
setcommand(_VAR,14,100)   'slow back EMV in deci-Volt
setcommand(_VAR,15,120)   'fast back EMV in deci-Volt
setcommand(_VAR,16,30000)   'back EMF time out in milli-seconds


' Read initial parameters before entering 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 first time update
isum = 0
icmd = 0
pwm = 0
pwmreq = 0
pwmcap = 300             ' soft-start initial PWM ceiling
dryCount = 0
startup = 0

' Initialize filters from present inputs
pf = getvalue(_AI,3)
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

' Filter pressure at the 1 kHz fast-loop rate
'pi = getvalue(_AI,3)
pf = pf + ((getvalue(_AI,3) - pf) >> fs)

' Filter measured supply/battery current at the fast-loop rate
ibat = getvalue(_BATAMPS,1)
ibatf = ibatf + ((ibat - ibatf) >> ibfs)

counter++
if counter mod 10 = 0 then
    ' Pressure loop at 100 Hz
    sp = getvalue(_VAR,1)
    pk = getvalue(_VAR,2)
    ps = getvalue(_VAR,3)
    fs = getvalue(_VAR,4)
    ck = 100 'getvalue(_VAR,5)
    imax = getvalue(_VAR,6)
    ilim = getvalue(_VAR,7)
    ibfs = getvalue(_VAR,8)
    ibatk = getvalue(_VAR,9)
    ibrel = getvalue(_VAR,10)
'    pwr = getvalue(_MOTPWR,1)
    bvolt = getvalue(_V, 2)

	'telemetry runs at 1 Hz
    if counter >= 1000 then
        'read tubidity value
        turbidity = getvalue(_AI,2)
    	'pa = (pf-400)*3/16 'convert mV to deci-psi
        print(sp,",",(pf-400)*3/16,",",ibatf,",",getvalue(_MOTPWR,1),",",turbidity,"\n\r")
        counter = 0
	end if
	
    if bvolt < 100 Then 
        errorCode=UNDER_VOLTAGE_CODE
        errorValue=bvolt
        goto halt 'undervoltage is reported in 10*volt
    end if

	if pf < CONST_pressDry then 
        dryCount++
    else 
        dryCount=0
        startup=1
    end if
  
    if startup=0 then
        if dryCount > getvalue(_VAR,11) then 
	        errorCode=DRY_RUN_START_CODE
	        errorValue=pf
            goto halt
        end if
    else	 
        if dryCount > getvalue(_VAR,12) then
	        errorCode=DRY_RUN_RUN_CODE
	        errorValue=pf
            goto halt
        end if
    end if
  

    ' Convert 0.1 psi setpoint to sensor millivolts
    ' 0-300 deci-psi maps to 400-2000 mV
    sp = 400 + (sp * 16 / 3)

    ' Pressure error in millivolts
    e = sp - pf

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

    ' Conditional integration / anti-windup
    ' Always allow negative pressure error to unwind the integrator.
    ' Positive integration is allowed only when neither the current
    ' command, PWM output, nor supply-current limiter is saturated.
    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
' Reduce the PWM ceiling quickly above the current limit.
' Restore it slowly only after current is 0.3 A below the limit.
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

' The lower of requested PWM and supply-limited PWM is applied
pwm = pwmreq
if pwm > pwmcap then pwm = pwmcap

if pwm = 0 then
    backEMF=0
else
    backEMF=bvolt*pwm/1000-ibat*getValue(_VAR,13)/pwm '180 is 0.18 ohms of wire resistance * 1000  1000 is the error goes from 0-1000
end if

if backEMF > getvalue(_VAR,14) then
    if backEMF > getvalue(_VAR,15) then
        EMFcount+=10
    else
        EMFcount++
    end if
end if

if EMFcount > getvalue(_VAR,16) then
    errorCode=OVER_BACK_EMF_CODE
    errorValue=backEMF
    goto halt
end if

setcommand(_GO,1,pwm)

wait(1)
goto top

halt:                  'hard fault, estop
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
haltloop:  
  print("ERROR:",errorCode,":",errorValue,"\n\r")
  wait(1000)
  goto haltloop