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

print("\n\rStarting...")
wait(10)
print("\n\r")

fmag=10000
frac=1000

pslope = ((5084-2067)*frac)/(150)
pinter = 5084*frac - pslope*150

pressuredefault= 100  '(PSI) running pressure in 0.1psi
updaterate=(10)		  '(100ms) time between updates
faultcurrent=(10)*10  '(amps) absolute maximum battery current in 0.1 Amps
rolloff=(6)*10        '(amps) where to start limiting pressure in 0.1 Amps
maxcurrent=(8)*10     '(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

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
LoopGain = 100     'proportional gain
maxfilter = 1000*frac

maxCombinedGain=2000000000/5000
maxErrorGain=MaxCombinedGain/b
ExcessGain=maxErrorGain/LoopGain
maxErrorGain=ExcessGain*LoopGain

setcommand(_VAR,2,LoopGain)
setcommand(_VAR,3,b)

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

 pressure = getvalue(_AI,1)

 bamp=getvalue(_BATAMPS,1)
 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/frac)-pressure
 
 error = error * MaxErrorGain

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

  output=filter/ExcessGain

  if output > 1000 then
    output = 1000
    filter=maxfilter
  else if output < 0 then
    if output<-1000 then
     filter=-maxfilter
    end if
    output = 0
  end if

 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,"\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