Commit d5e320c6 authored by Martin Karlsson's avatar Martin Karlsson
Browse files

Implement ethheli

parent 0047c953
# 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
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