From d5e320c65442e091cd6fd9dfdc423aafc94eb4e7 Mon Sep 17 00:00:00 2001
From: Martin Karlsson <cont-mkr@ulund.org>
Date: Mon, 21 Aug 2017 17:29:10 +0200
Subject: [PATCH] Implement ethheli
---
.../eth_helicopter.jl | 44 +++++++++++++++++++
1 file changed, 44 insertions(+)
create mode 100644 src/interface_implementations/eth_helicopter.jl
diff --git a/src/interface_implementations/eth_helicopter.jl b/src/interface_implementations/eth_helicopter.jl
new file mode 100644
index 0000000..1011c99
--- /dev/null
+++ b/src/interface_implementations/eth_helicopter.jl
@@ -0,0 +1,44 @@
+# Interface implementation Ball And Beam ====================================================
+
+export ETHHelicopter, ETHHelicopterSimulator, ETHHelicopterType
+
+struct ETHHelicopter <: PhysicalProcess
+ h::Float64
+end
+ETHHelicopter() = ETHHelicopter(0.050)
+
+struct ETHHelicopterSimulator <: SimulatedProcess
+ h::Float64
+ state::Vector{Float64}
+end
+ETHHelicopterSimulator() = ETHHelicopterSimulator(0.01, zeros(4))
+
+const ETHHelicopterType = Union{ETHHelicopter, ETHHelicopterSimulator}
+num_outputs(p::ETHHelicopterType) = 2
+num_inputs(p::ETHHelicopterType) = 2
+outputrange(p::ETHHelicopterType) = [(-10,10),(-10,10)] # Beam angle, Ball position
+inputrange(p::ETHHelicopterType) = [(-10,10)]
+isstable(p::ETHHelicopterType) = false
+isasstable(p::ETHHelicopterType) = false
+sampletime(p::ETHHelicopterType) = p.h
+
+function control(p::ETHHelicopter, u)
+ ccall((:comedi_write, comedipath),Int32,(Int32,Int32,Int32,Int32), 0,1,0,num2io(u[1]))
+ ccall((:comedi_write, comedipath),Int32,(Int32,Int32,Int32,Int32), 0,1,1,num2io(u[2]))
+end
+
+measure(p::ETHHelicopter) = [io2num(ccall((:comedi_read, comedipath),Int32,(Int32,Int32,Int32), 0,0,i)) for i = 0:1] #i=0 for pitch, i=1 for yaw
+
+
+control(p::ETHHelicopterSimulator, u) = error("Not yet implemented")
+measure(p::ETHHelicopterSimulator) = error("Not yet implemented")
+
+const comedipath = Pkg.dir("LabProcesses","c","comedi_bridge.so")
+const conversion = 65535/20
+io2num(x) = x/conversion -10 # Converts from io to float
+num2io(x) = round(Int32,(x + 10)*conversion) # Converts from regular number to io
+
+initialize(p::ETHHelicopter) = ccall((:comedi_start, comedipath),Int32,(Int32,), 0)
+finalize(p::ETHHelicopter) = (control(p,0);ccall((:comedi_stop, comedipath),Int32,(Int32,), 0))
+initialize(p::ETHHelicopterSimulator) = nothing
+finalize(p::ETHHelicopterSimulator) = nothing
--
GitLab