Commit 615a470c authored by Martin Heyden's avatar Martin Heyden
Browse files

Added first implementation of flexible servo

parent e91be74a
Pipeline #726 canceled with stage
# Interface implementation Flexible Servo ===========================
#TODO export get_states and define process
export FlexibleServo, FlexibleServoSimulator, AbstractFlexibleServo, define_flexible_servo
struct FlexibleServo <: PhysicalProcess
h::Float64
stream::LabStream
measure1::AnalogInput10V
measure2::AnalogInput10V
control::AnalogOutput10V
end
function FlexibleServo(;
h::Float64 = 0.01,
stream:LabStream = ComediStream(),
measure1::AnalogInput10V = AnalogInput10V(0),
measure2::AnalogInput10V = AnalogInput10V(1),
control::AnalogOutput10V = AnalogOutput10V(1))
p = FlexibleServo(Float64(h), stream, measure1, measure2, control)
init_devices!(p.stream, p.measure1, p.measure2, p.control) #TODO no idea what this does
return p
end
function define_flexible_servo()
# Creates linear process model
m1 = 2.29; m2 = 2.044 # Masses
d1 = 3.12; d2 = 3.73 # Damping coefficients
k = 400 # Spring constant
km = 2.96 # Motor constant
ky = 280 # Measurement constant
# Process matrices
A = [0 1 0 0;
-k/m1 -d1/m1 k/m1 0
0 0 0 1
; k/m2 0 -k/m2 -d2/m2]
B = [0; km/m1; 0; 0]
C = [ky 0 0 0 ;
0 0 ky 0]
D = 0
return ss(A,B,C,D)
end
struct FlexibleServoSimulator <: SimulatedProcess
x::Array{Float64,1}
h::Float64
Φ::Array{Float64,2}
Γ::Array{Float64,2}
C::Array{Float64,2}
end
function FlexibleServoSimulator (;h::Float64 =0.01)
Gp = ss(A,B,C,D)
Gp_disc = c2d(Gp, h)
return FlexibleServo(h, Gp_disc[1].A, Gp_disc[1].B, Gp_disc[1].C)
end
const AbstractFlexibleServo = Union{FlexibleServo, FlexibleServoSimulator}
num_outputs(p::AbstractFlexibleServo) = 2
num_inputs(p::AbstractFlexibleServo) = 1
outputrange(p::AbstractFlexibleServo) = [(-10,10),(-10,10)] #Process has two outputs, only one uses in AK lab3
inputrange(p::AbstractFlexibleServo) = [(-10,10)]
isstable(p::AbstractFlexibleServo) = true
isasstable(p::AbstractFlexibleServo) = false
sampletime(p::AbstractFlexibleServo) = p.h
bias(p::AbstractFlexibleServo) = 0 #TODO: Add bias to structs?
function control(p::AbstractFlexibleServo, 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::FlexibleServo, u) = send(p.control,u)
function control(p::FlexibleServoSimulator, u)
x = p.Φ*x + p.Γ*u
nothing
end
measure(p::FlexibleServo) = [read(p.measure1) read(p.measure2)]
measure(p::FlexibleServoSimulator) = p.C*p.x
get_state(p::FlexibleServoSimulator) = p.x
initialize(p::AbstractProcess) = error("Function not implemented for $(typeof(p))")
finalize(p::AbstractProcess) = error("Function not implemented for $(typeof(p))")
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