Select Git revision
doubletank.jl 7.43 KiB
#export DoubleTankSimulator, DoubleTank
using LabConnections.Computer #for LabStream
const uppertankempty = 0.0
const lowertankempty = 0.0
const uppertankfull = 10.07
const lowertankfull = 9.39
const venturimin = 0.0
const venturimax = 14.0
using Parameters
#This should be changed in LabConnections in some reasonably manner
############################################################
function LabConnections.Computer.getwritecommand(stream::ComediStream, input::AnalogOutput10V, val::Float64)
abs(val) <= 10 || error("Volatage $val not in range [-10,10]")
return (Int32(0),Int32(3),input.i,val)
end
function LabConnections.Computer.getreadcommand(stream::ComediStream, input::AnalogInput10V)
return (Int32(0),Int32(2),input.i)
end
############################################################
# This allows calibration without having to toggle the pump controller
function LabConnections.Computer.send(n::Void, a::Number)
nothing
end
@with_kw mutable struct PID
b::Float64 = 1.0
K::Float64 = 1.0
h::Float64 = 0.05
Ti::Float64 = 1.0
Td::Float64 = 1.0
y_old::Float64 = 0.51
N::Int64 = 10
P::Float64 = 0
I::Float64 = 0
D::Float64 = 0
Tot::Float64 = 0
end
function PID(b, K, h, Ti, Td, y_old, N)
PID(b, K, h, Ti, Td, y_old, N, 0.0, 0.0, 0.0, 0.0)
end
function (p::PID)(r, y, onv=(1,1,1))
#The PID should operate entirely on normalized values
@unpack b, K, h, Ti, Td, N, y_old, P, I, D, Tot = p
ad = Td/(N*h+Td)
bd = N*K*Td/(N*h+Td)
Tot = 0.0
P = K*(b*r-y)
if onv[1]==1
Tot += P
end
if onv[3]==1
D = ad*D - bd*(y-y_old)
Tot += D
end
if onv[2]==1 && Ti !=0
Ichange = K/Ti*h*(r-y)
if (Tot + I < 1 && Ichange > 0)||
(Tot + I > 0 && Ichange < 0)
I += Ichange
end
Tot += I end
y_old = y
@pack p = P, I, D, Tot, y_old
Tot
end
@with_kw mutable struct DoubleTankSimulator <: SimulatedProcess
h::Float64 = 0.05
x::Vector{Float64} = [0.5, 0.5]
n::Int = 1
α::Float64 = 2.1e-5
A::Float64 = 4.9e-4
a1::Float64 = 3.1e-6
a2::Float64 = 3.1e-6
g::Float64 = 9.8
scale::Float64 = 0.16
σ::Float64 = 0.0 #in meters
end
mutable struct Pump <: PhysicalProcess
h::Float64
stream::LabStream
u::Float64
v::Float64
pid::PID
venturirange::Array{Float64, 1}
measure::AnalogInput10V
control::Union{AnalogOutput10V, Void}
end
function Pump(stream)
pid = PID()
pid.K = 0.21
pid.Ti = 0.06
pid.b = 0.0
pid.h = 0.01
Pump(pid.h, stream, 0.0, 0.0, pid, [venturimin, venturimax], AnalogInput10V(4), AnalogOutput10V(0))
end
outputrange(p::Pump) = [0.,10.]
inputrange(p::Pump) = [(0.,1.)]
function control(p::Pump, yref)
p.u = yref
end
function initialize(p::Pump)
@async while true
@periodically p.h begin
#normalized venturi
venturi = (read(p.measure)-p.venturirange[1])/(p.venturirange[2]-p.venturirange[1])
flow = sqrt(max(venturi,0.0))
p.v = p.pid(p.u, flow, (1,1,0))
send(p.control, 10*clamp(p.v, 0.0, 1.0))
end
end
end
measure(p::Pump) = read(p.measure)
struct DoubleTank <: PhysicalProcess
h::Float64
stream::LabStream
measure::Array{AnalogInput10V, 1}
uprange::Array{Float64, 1}
lowrange::Array{Float64, 1}
pump::Pump
end
function DoubleTank(;
h::Float64 = 0.05,
stream::LabStream = ComediStream(),
measure = [AnalogInput10V(0), AnalogInput10V(1)],
uprange = [uppertankempty, uppertankfull],
lowrange = [lowertankempty, lowertankfull],
pump::Pump = Pump(stream)) #Change
p = DoubleTank(Float64(h),stream,measure, uprange, lowrange, pump)
init_devices!(p.stream, p.measure..., pump.measure, pump.control)
initialize(pump)
p
end
function control(p::DoubleTank, u)
control(p.pump, u)
end
"""
Calibrates the tanks. Sets the pump to max for filltime seconds, during which the flow is measured calpts times. The tank voltage is then measured, when they are presumably full. The pump is then set to min for emptytime and the flow is again measured calpts time. The tank voltage is then again measured, when they're presumably empty. These values are then used to set the parameters for the DoubleTank measure function and the DoubleTank.pump controller.
"""
function calibrate(p::DoubleTank)
print("Calibrating...\n")
filltime = 40
emptytime = 30
calpts = 20
#Override pump control
c = p.pump.control
p.pump.control = nothing
measurements = zeros(1:calpts, Float64)
send(c, 10.0) #just a number > 10
for i in 1:calpts
sleep(filltime/calpts)
measurements[i] = measure(p.pump)
end
fulltanks = read.(p.measure)
venturimax = mean(measurements)
print("Upper Full Voltage: $(fulltanks[1])\n")
print("Lower Full Voltage: $(fulltanks[2])\n")
print("Venturi Avg max: $venturimax\n")
send(c, 0.0)
for i in 1:calpts
sleep(emptytime/calpts)
measurements[i] = measure(p.pump)
end
emptytanks = read.(p.measure)
venturimin = mean(measurements)
print("Upper Empty Voltage: $(emptytanks[1])\n")
print("Lower Empty Voltage: $(emptytanks[2])\n")
print("Venturi Avg min: $venturimin\n")
p.uprange[1] = emptytanks[1]
p.uprange[2] = fulltanks[1]
p.lowrange[1] = emptytanks[2]
p.lowrange[2] = fulltanks[2]
p.pump.venturirange = [venturimin, venturimax]
#Return pump control
p.pump.control = c
print("Finished calibration\n")
nothing
end
function calibrate(p::DoubleTankSimulator)
print("Simulator can't be calibrated!\n")
end
function measure(p::DoubleTank)
#This should give an array of two values in the range [0,1]
minv = [p.uprange[1], p.lowrange[1]]
maxv = [p.uprange[2], p.lowrange[2]]
clamp.((read.(p.measure)-minv)./(maxv-minv), 0.0, 1.0)
end
const AbstractDoubleTank = Union{DoubleTank, DoubleTankSimulator}
num_outputs(p::AbstractDoubleTank) = 2
num_inputs(p::AbstractDoubleTank) = 1
outputrange(p::AbstractDoubleTank) = [(0.0,1.0),(0.0,1.0)]
inputrange(p::AbstractDoubleTank) = [(0.,1.)]
isasstable(p::AbstractDoubleTank) = true
sampletime(p::AbstractDoubleTank) = p.h
bias(p::DoubleTankSimulator) = 0
function qu(p, u)
u*p.α
end
function control(p::DoubleTankSimulator, u::Number)
@unpack a1, a2, g, x, A, h, scale = p
x *= scale
#The simulator should operate on physical values but u should be in [0,1]
u = clamp(u, inputrange(p)[1]...)
qut = a1*sqrt(2*g*x[1]) #m^3/s
dA1 = qu(p,u) - qut #m^3/s
if -dA1*h>x[1]*A
qut = x[1]*A/h-qu(p, u)
dA1 = qu(p, u) - qut #m^3/s
elseif x[1] + (dA1*h)/A > scale*outputrange(p)[1][2]
dA1 = (scale*outputrange(p)[1][2] - x[1])*A/h
end
dA2 = qut - a2*sqrt(2*g*x[2]) #m^3/s
if(-dA2*h>x[2]*A)
dA2 = -x[2]*A/h
end
x[1] += (dA1*h)/A
x[2] += (dA2*h)/A
x /= scale
@pack p = a1, a2, g, x, A, h
end
measure(p::DoubleTankSimulator) = p.x + p.σ*p.scale*randn(2)
LabProcesses.initialize(p::DoubleTankSimulator) = nothing
LabProcesses.finalize(p::DoubleTankSimulator) = nothing
LabProcesses.initialize(p::DoubleTank) = nothing
LabProcesses.finalize(p::DoubleTank) = nothing