'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

#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)

' 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 + ((getvalue(_BATAMPS,1) - 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)

	'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 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

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