diff --git a/src/LabProcesses.jl b/src/LabProcesses.jl index b2f1a6883365f2159e13f0f8f7edcee54cc82fde..afd95d16f0ce1231fb1e7f433af2e2c93658728a 100644 --- a/src/LabProcesses.jl +++ b/src/LabProcesses.jl @@ -17,6 +17,7 @@ include("interface.jl") include("interface_documentation.jl") include("interface_implementations/ballandbeam.jl") include("interface_implementations/eth_helicopter.jl") +include("interface_implementations/furuta.jl") include("reference_generators.jl") include("controllers.jl") diff --git a/src/interface_implementations/define_furuta_system.jl b/src/interface_implementations/define_furuta_system.jl new file mode 100644 index 0000000000000000000000000000000000000000..da11954dd5f467f57c7ba5356c6b02dc2e37486c --- /dev/null +++ b/src/interface_implementations/define_furuta_system.jl @@ -0,0 +1,34 @@ +using ControlSystems + +""" + Furuta pendulum model +""" +function define_furuta_system() + l=0.413; + M=0.01; + Jp=0.0009; + r=0.235; + J=0.05; + m=0.02; + g=9.81; + x0=[0.1; 0; 0; 0]; + phidot_lin=0; + + + alfa=Jp+M*l^2; + beta=J+M*r^2+m*r^2; + gamma=M*r*l; + epsilon=l*g*(M+m/2); + + Afc=[0 1 0 0; + (beta*epsilon+alfa*beta*phidot_lin^2)/(alfa*beta-gamma^2) 0 0 0; + 0 0 0 1; + -(gamma*epsilon+alfa*gamma*phidot_lin^2)/(alfa*beta-gamma^2) 0 0 0]; + Bfc=[0 -gamma/(alfa*beta-gamma^2)*g 0 alfa/(alfa*beta-gamma^2)*g]'; + Cfc=zeros(4,4); + for i = 1:4 + Cfc[i,i] = 1; + end + Dfc=zeros(4,1); + ss(Afc,Bfc,Cfc,Dfc) +end diff --git a/src/interface_implementations/furuta.jl b/src/interface_implementations/furuta.jl new file mode 100644 index 0000000000000000000000000000000000000000..4feaa262e9454a1b09a17e333aedfcc1c823d91a --- /dev/null +++ b/src/interface_implementations/furuta.jl @@ -0,0 +1,71 @@ +# Interface implementation Furuta Pendulum ==================================================== + +export Furuta, FurutaSimulator, AbstractFuruta + +# @with_kw allows specification of default values for fields. If none is given, this value must be supplied by the user. replaces many constructors that would otherwise only supply default values. +# Call constructor like Furuta(bias=1.0) if you want a non-default value for bias + +# Furuta(;kwargs...) +#Physical Furuta process + +#Arguments (fields) +#- `h::Float64 = 0.01` +#- `bias::Float64 = 0.0` +#- `stream::LabStream = ComediStream()` +#- `measure::AnalogInput10V = AnalogInput10V(0)` +#- `control::AnalogOutput10V = AnalogOutput10V(1)` + +struct Furuta <: PhysicalProcess + h::Float64 + bias::Float64 + stream::LabStream + measure::AnalogInput10V + control::AnalogOutput10V +end +function Furuta(; + h::Float64 = 0.01, + bias::Float64 = 0., + stream::LabStream = ComediStream(), + measure::AnalogInput10V = AnalogInput10V(0), + control::AnalogOutput10V = AnalogOutput10V(1)) + p = Furuta(Float64(h),Float64(bias),stream,measure,control) + init_devices!(p.stream, p.measure, p.control) + p +end + +include("define_furuta_system.jl") +const furuta_system = define_furuta_system() + +struct FurutaSimulator <: SimulatedProcess + h::Float64 + s::SysFilter + FurutaSimulator(;h::Real = 0.01, bias=0) = new(Float64(h), SysFilter(furuta_system, h)) +end +#FurutaSimulator() = FurutaSimulator() + +const AbstractFuruta = Union{Furuta, FurutaSimulator} +num_outputs(p::AbstractFuruta) = 1 +num_inputs(p::AbstractFuruta) = 1 +outputrange(p::AbstractFuruta) = [(-10,10)] +inputrange(p::AbstractFuruta) = [(-10,10)] +isstable(p::AbstractFuruta) = false +isasstable(p::AbstractFuruta) = false +sampletime(p::AbstractFuruta) = p.h +bias(p::AbstractFuruta) = p.bias + +function control(p::AbstractFuruta, u::AbstractArray) + length(u) == 1 || error("Process $(typeof(p)) only accepts one control signal, tried to send u=$u.") + control(p,u[1]) +end + +control(p::AbstractFuruta, u::Number) = send(p.control,u) +control(p::FurutaSimulator, u::Number) = p.s(u) + + +measure(p::Furuta) = read(p.measure) +measure(p::FurutaSimulator) = p.s.sys.C*p.s.state + +initialize(p::Furuta) = nothing +finalize(p::Furuta) = foreach(close, p.stream.devices) +initialize(p::FurutaSimulator) = nothing +finalize(p::FurutaSimulator) = nothing