# When you want to target a pressure and feed it with multiple volume
# pumps instead of a regulator.

alias sensor d0

alias error r15
alias value r14
alias integral r13
alias on r11
alias previousError r10
alias output r9
alias derivative r8

define SETPOINT 150
define CUTOFF 200
define Kp 0.06 # because Kp*150 ~ 10
#define Ki 0.4
#define Kd 0.2
define Ki 0.05
define Kd 0.05

move previousError 0
move integral 0

start:
yield

l value sensor Pressure

jal eval
s db Setting output
sb HASH("StructureVolumePump") Setting output
sb HASH("StructureVolumePump") On on

move previousError error

j start

eval:

bgt value CUTOFF turnOff
move on 1

sub error SETPOINT value
mul output Kp error # P_out = Kp*error
add integral integral error # integral += error
mul r1 Ki integral # I_out = Ki * integral
add output output r1 # output += I_out
sub derivative error previousError # derivative = (error - self.previousError) / dt
mul r1 Kd derivative # D_out = self.Kd * derivative
add output output r1 # output += D_out

j ra

turnOff:
move on 0
j ra