Commit c0728e83 authored by Julian Salt's avatar Julian Salt
Browse files

Started furuta implementation

parent 957f11ca
Pipeline #732 failed with stage
......@@ -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")
......
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
# 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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment