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 temp as integer
dim schedule as integer
dim pressuredefault as integer
dim updaterate as integer
dim faultcurrent as integer
dim rolloff as integer
dim maxcurrent as integer
dim rollscale 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 pressDry as integer
dim dryCounts as integer
dim startup as boolean
dim drystarttime as integer
dim dryruntime as integer
dim backEMF as integer
dim EMFcount as integer

setcommand(_MS,1)
 
print("\n\rStarting (R0-4)")
for temp=10 andwhile temp >= 0 evaluate temp -= 1
 wait(1000)
 print(temp," ")
next temp
print("\n\r")

fmag=10000
frac=1000

'pallisades calibration (100 ohm)
'Guam as well
pslope = ((1571-814)*frac)/(109-40)
pinter = 1571*frac - pslope*109

'Scott Creek Calibration
'pslope = ((2900-1440)*frac)/(150)
'pinter = 2900*frac - pslope*150

pressuredefault=25   '(PSI) running pressure in 0.1psi
updaterate=(10)		  '(100ms) time between updates
faultcurrent=(96)  '(amps) absolute maximum battery current in 0.1 Amps
rolloff=(80)        '(amps) where to start limiting pressure in 0.1 Amps
maxcurrent=(90)     '(amps) maximum battery current at which pressure = 0 psi, in 0.1 Amps
rollscale=100/(maxcurrent-rolloff)
counter=updaterate  'this allows a quick update after pause
pressureset=pressuredefault
pressDry=pslope*10+pinter 'pressure threshold to begin countdown to dry pump, from 0.1psi to counts
dryCounts=0
startup=True
drystarttime=300
dryruntime=100
EMFcount=0

setcommand(_VAR,1,pressuredefault) 'save in VAR 1 in tenths of PSI

b=3				'decay rate of filter
 				'b=1-d
				'd=e^(-2pif)
filter = 0		'past value of filter
gain = 100     'proportional gain
maxfilter = 700*frac

setcommand(_VAR,2,gain)
setcommand(_VAR,3,b)
setcommand(_VAR,4,drystarttime)
setcommand(_VAR,5,dryruntime)
setcommand(_VAR,6,80)  'maximum back emf counts at 1x time
setcommand(_VAR,7,105)  'maximum back emf counts at 10x time
setcommand(_VAR,8,300)  'x100ms EMF time out
while 1
 gain = getvalue(_VAR,2)
 b = getvalue(_VAR,3)

 turbidity = getvalue(_AI,2)

 pressure = getvalue(_AI,1)*frac
 
 if pressure < pressDry then 
     dryCount++
 else 
     dryCount=0
	 startup=False
 end if
 
 if startup then
     if dryCount > getvalue(_VAR,4) then goto dryhalt
 else	 
     if dryCount > getvalue(_VAR,5) then goto dryhalt
 end if
 
 bamp=getvalue(_BATAMPS,1)
 bvolt = getvalue(_V, 2)
 
 'if bvolt < 100 Then goto volthalt 'undervoltage is reported in 10*volt
 
 if bamp > faultcurrent Then goto faulthalt  'over absolute maximum, shut it down
 bamperror=(maxcurrent-bamp)*rollscale   'what is the error

 pressureset=getvalue(_VAR,1)
 if (pressureset < 0) or (pressureset > 150) then pressureset=pressuredefault
 pressureset= pslope*pressureset+pinter 'convert to counts

 if bamperror <0 then                 'it is over maximum then 
    pressureset=0                         'set to 0 pressure
 elseif bamperror < 100 then          'is it at limiting threshold
     pressureset=(pressureset*bamperror)/100 'by how much to limit pressure
 end if
 
 error = pressureset-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
  end if
  
 backEMF=bvolt*output/1000-bamp*180/output

 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 goto EMFhalt
 
 
 setcommand(_GO,1,output)

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

 wait(100)
end while

faulthalt:                  'hard fault, estop and send error
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
  print("Over Current ",bamp,"\n\r")
  terminate

volthalt:                  'hard fault, estop and send error
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
  print("Under Voltage ",bvolt,"\n\r")
  terminate
  
dryhalt:                  'hard fault, estop and send error
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
  print("Dry run condition detected","\n\r")
  terminate

EMFhalt:                  'hard fault, estop and send error
  SetCommand(_ESTOP,1)
  setcommand(_GO,1,0)          
  print("Over back EMF","\n\r")
  terminate