utilities.jl 2.61 KB
Newer Older
1
export @periodically, init_sysfilter, sysfilter!, SysFilter
Fredrik Bagge Carlson's avatar
Update  
Fredrik Bagge Carlson committed
2
3
4
5
6
7
8
9
10
11

"""
	@periodically(h, body)
Ensures that the body is run with an interval of `h >= 0.001` seconds.
"""
macro periodically(h, body)
	quote
		local start_time = time()
		$(esc(body))
		local execution_time = time()-start_time
Fredrik Bagge Carlson's avatar
Fredrik Bagge Carlson committed
12
		sleep(max(0,$(esc(h))-execution_time))
Fredrik Bagge Carlson's avatar
Update  
Fredrik Bagge Carlson committed
13
14
	end
end
15

16
17
18
19
20
21
22
23
24
25
"""
	@periodically(h, simulation::Bool, body)
Ensures that the body is run with an interval of `h >= 0.001` seconds.
If `simulation == false`, no sleep is done
"""
macro periodically(h, simulation, body)
	quote
		local start_time = time()
		$(esc(body))
		local execution_time = time()-start_time
26
		$(esc(simulation)) || Libc.systemsleep(max(0,$(esc(h))-execution_time))
27
28
29
	end
end

30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

"""
	Csf = SysFilter(sys_discrete::StateSpace)
	Csf = SysFilter(sys_continuous::StateSpace, sampletime)
	Csf = SysFilter(sys::StateSpace, state::AbstractVector)
Returns an object used for filtering signals through LTI systems.
Create a SysFilter object that can be used to implement control loops and simulators
with LTI systems, i.e., `U(z) = C(z)E(z)`. To filter a signal `u` through the filter,
call like `y = Csf(u)`. Calculates the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
"""
struct SysFilter
	sys::StateSpace
	state::Vector{Float64}
	function SysFilter(sys::StateSpace, state::AbstractVector)
		@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model."
		@assert length(state) == sys.nx "length(state) != sys.nx"
		new(sys, state)
	end
	function SysFilter(sys::StateSpace)
		@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model. Supply sample time."
		new(sys, init_sysfilter(sys))
	end
	function SysFilter(sys::StateSpace, h::Real)
		@assert ControlSystems.iscontinuous(sys) "Sample time supplied byt system model is already in discrete time."
		sysd = c2d(sys, h)[1]
		new(sysd, init_sysfilter(sysd))
	end
end
(s::SysFilter)(input) = sysfilter!(s.state, s.sys, input)

60
61
62
63
64
"""
	state = init_sysfilter(sys::StateSpace)
Use together with [`sysfilter!`](@ref)
"""
function init_sysfilter(sys::StateSpace)
65
 	zeros(sys.nx)
66
67
68
end

"""
69
	output = sysfilter!(s::SysFilter, input)
70
71
72
73
74
75
	output = sysfilter!(state, sys::StateSpace, input)
Returns the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`

This function is used to implement control loops where a signal is filtered through a
dynamical system, i.e., `U(z) = C(z)E(z)`. Initialize `state` using [`init_sysfilter`](@ref).
"""
76
77
78
function sysfilter!(state::AbstractVector, sys::StateSpace, input)
	state .= vec(sys.A*state + sys.B*input)
	output = vec(sys.C*state + sys.D*input)
79
end
80
81

sysfilter!(s::SysFilter, input) = sysfilter!(s.state, s.sys, input)