'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

#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

'*** CAUTION  *************
'if you change any of these values you must recalculate the dependent values
#define faultcurrent (96)  '(amps) absolute maximum battery current in 0.1 Amps
#define rolloff (82)  '(amps) where to start limiting pressure in 0.1 Amps
#define CONST_maxcurrent 92 
#define CONST_rolloffmag 100
'*****************************************
                     '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 rollscale 10 'CONST_rolloffmag/(CONST_maxcurrent-rolloff)  this must be hand calculated
#define CONST_pslope  5333   ' (2000-400)*CONST_frac/(300-0) this must be hand calculated
#define CONST_pinter  400000 ' 2000*CONST_frac - pslope*300 this must be hand calculated
#define CONST_pressDry 453330 'pslope*10+pinter   this must be hand calculated
#define CONST_drystarttime 300
#define CONST_dryruntime 100
#define CONST_updaterate 10  '(100ms) time between updates
#define CONST_pressuremax 300  '(PSI) running pressure in 0.1psi
#define CONST_pressuredefault 25   '(PSI) running pressure in 0.1psi
#define CONST_slowEMFlevel 100 '0.1volts maximum back emf counts at 1x time
#define CONST_fastEMFlevel 120 '0.1volts maximum back emf counts at 10x time
#define CONST_EMFtimeout 300  'x100ms EMF time out

dim filter as integer
dim error as integer
dim pressure as integer
dim setpoint as integer
dim gain as integer
dim b as integer
dim output as integer
dim maxint as integer
dim minint as integer
dim fmag as integer
dim maxfilter as integer
dim counter as integer
dim pressureset  as integer
dim pslope as integer
dim pinter as integer
dim bamp as integer
dim bamperror as integer
dim dryCount as integer
dim startup as boolean
dim backEMF as integer
dim EMFcount as integer
dim psetfilter as integer
dim errorCode as integer
dim errorValue as integer

setcommand(_MS,1)

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

fmag=10000
frac=1000

pslope = CONST_pslope 
pinter = CONST_pinter 

counter=CONST_updaterate 'this allows a quick update after pause
pressureset=CONST_pressuredefault
dryCount=0
startup=True
EMFcount=0
errorValue=-1

b=3				'decay rate of filter
 				'b=1-d
				'd=e^(-2pif)
filter = 0		'past value of filter
gain = 100     'proportional gain
maxfilter = 1000000
psetfilter = pinter 'initialize to 0 psi in counts

setcommand(_VAR,1,CONST_pressuredefault) 'save in VAR 1 in tenths of PSI
setcommand(_VAR,2,gain)
setcommand(_VAR,3,b)
setcommand(_VAR,4,CONST_drystarttime)
setcommand(_VAR,5,CONST_dryruntime)
setcommand(_VAR,6,CONST_slowEMFlevel)  
setcommand(_VAR,7,CONST_fastEMFlevel) 
setcommand(_VAR,8,CONST_EMFtimeout)  

while 1
  gain = getvalue(_VAR,2)
  b = getvalue(_VAR,3)

  turbidity = getvalue(_AI,2)

  pressure = getvalue(_AI,1)*frac
  

  if pressure < CONST_pressDry then 
    dryCount++
  else 
    dryCount=0
    startup=False
  end if
  
  if startup then
    if dryCount > getvalue(_VAR,4) then 
	  errorCode=DRY_RUN_START_CODE
	  errorValue=pressure
      goto halt
    end if
  else	 
    if dryCount > getvalue(_VAR,5) then
	  errorCode=DRY_RUN_RUN_CODE
	  errorValue=pressure
      goto halt
    end if
  end if
  
  bamp=getvalue(_BATAMPS,1)
  bvolt = getvalue(_V, 2)
  
  if bvolt < 100 Then 
    errorCode=UNDER_VOLTAGE_CODE
	errorValue=bvolt
    goto halt 'undervoltage is reported in 10*volt
  end if

  
  if bamp > faultcurrent Then
    errorCode=OVER_CURRENT_CODE
	errorValue=bamp
    goto halt
  end if

  bamperror=(CONST_maxcurrent-bamp)*rollscale   'what is the error

  pressureset=getvalue(_VAR,1)

  if (pressureset < 0) then pressureset=0
  if (pressureset > CONST_pressuremax) then pressureset=CONST_pressuremax
  
  if bamperror <0 then                 'it is over maximum then 
    pressureset=0                         'set to 0 pressure
  elseif bamperror < CONST_rolloffmag then          'is it at limiting threshold
    pressureset=(pressureset*bamperror)/CONST_rolloffmag 'by how much to limit pressure
  end if
  
  pressureset= pslope*pressureset+pinter 'convert to counts

  psetfilter += (50*(pressureset-psetfilter))/100
  
  error = psetfilter-pressure
  
  error = error * gain

  filter += (b*(error-filter))/fmag

  output = filter/frac

  if output > 1000 then
    output = 1000
    filter=maxfilter
  end if
    
  if output < -1000 then 
    output = -1000
    filter=-maxfilter
  end if
    
  if output < 0 then
    output=0
    backEMF=0
  else
    backEMF=bvolt*output/1000-bamp*180/output '180 is 0.18 ohms of wire resistance * 1000  1000 is the error goes from 0-1000
  end if

  if backEMF > getvalue(_VAR,6) then
    if backEMF > getvalue(_VAR,7) then
      EMFcount+=10
    else
      EMFcount++
    end if
  end if
  
  if EMFcount > getvalue(_VAR,8) then
    errorCode=OVER_BACK_EMF_CODE
	errorValue=backEMF
    goto halt
  end if
  
  setcommand(_GO,1,output)

  counter++
  if counter >= CONST_updaterate then  'time to send an update
    pwr=getvalue(_MOTPWR,1)
    pressure=(pressure-pinter)/pslope
    pressureset=(psetfilter-pinter)/pslope
    print(pressureset,",",pressure,",",bamp,",",pwr/10,",",turbidity,"\n\r")
    counter=0
  end if

  wait(100)
end while

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