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

setcommand(_MS,1)
 
print("\n\rStarting (R0-1)")
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= 100  '(PSI) running pressure in 0.1psi
updaterate=(10)		  '(100ms) time between updates
faultcurrent=(11)*10  '(amps) absolute maximum battery current in 0.1 Amps
rolloff=(8)*10        '(amps) where to start limiting pressure in 0.1 Amps
maxcurrent=(10)*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
gain = 100     'proportional gain
maxfilter = 1000*frac

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

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

 pressure = getvalue(_AI,1)*frac
 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-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
  
 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