diff --git a/src/interface_implementations/flexibleservo.jl b/src/interface_implementations/flexibleservo.jl new file mode 100644 index 0000000000000000000000000000000000000000..9894fdd4efa61b18b9cd94a4db5cb7d72a34d4d1 --- /dev/null +++ b/src/interface_implementations/flexibleservo.jl @@ -0,0 +1,91 @@ +# 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))")