Initial commit

README.md

+# Installation
+1. Open a terminal
+2. Type `export JULIA_PKGDIR=/work/$USER\n export JULIA_EDITOR=gedit >> .bashrc`. Restart the terminal.
+3. Type `mkdir FRTN35_lab1; cd FRTN35_lab1`
+4. Start `julia`
+5. Install `BallAndBeam.jl` using command `Pkg.clone("https://gitlab.control.lth.se/labdev/software/tree/master/julia_ballandbeam/BallAndBeam.jl.git")` Lots of packages will now be installed, this will take some time.
+6. Type `cp(Pkg.dir("BallAndBeam","src","FRTN35_lab1.jl"), "."); edit("FRTN35_lab1.jl")`. The script `FRTN35_lab1.jl` will now open in a text editor, edit it while following the lab manual. If unsure about how a function works, type `?function_name` for help.
+
+# Documentation

REQUIRE

+Plots
+GR
+ControlSystems
+Polynomials
+ProgressMeter
+JLD

c/Makefile

+CC=gcc 
+
+CFLAGS=-c -Wall -fPIC
+
+SOURCES=comedi_bridge.c 
+OBJECTS=$(SOURCES:.c=.o)
+
+.c.o:
+	$(CC) $(CFLAGS) $< -o $@ 
+
+lib: $(OBJECTS)
+	$(CC) -shared -fPIC -lcomedi -lm -o comedi_bridge.so $(OBJECTS)
+
+clean:
+	rm *.o *.so

c/comedi_bridge.c

+#include <stdio.h>      /* for printf() */
+#include <comedilib.h>
+
+int range = 0;
+int aref = AREF_GROUND;
+comedi_t *it;
+
+int comedi_write(int comediNbr, int subdev, int chan, int writeValue) {
+	static int retval;
+	retval = comedi_data_write(it, subdev, chan, range, aref, writeValue);
+    return retval;
+}
+
+int comedi_read(int comediNbr, int subdev, int chan) {
+	lsampl_t data;
+	comedi_data_read(it, subdev, chan, range, aref, &data);
+    return data;
+}
+
+int comedi_start(int comediNbr) {
+	it = comedi_open("/dev/comedi0");
+		if(it == NULL)
+	{
+		comedi_perror("comedi_open_error");
+		return -1;
+	}
+	return 0;
+}
+
+void comedi_stop(int comediNbr) {
+	// Ad-hoc. Should be updated:
+	comedi_data_write(it, 1, 0, range, aref, 32767);
+	comedi_data_write(it, 1, 1, range, aref, 32767);
+	comedi_close(it);
+}
+
+
+

docs/make.jl

+using Documenter, BallAndBeam
+
+makedocs(doctest = false) # Due to lots of plots, this will just have to be run on my local machine
+
+deploydocs(
+    deps   = Deps.pip("pygments", "mkdocs", "python-markdown-math", "mkdocs-cinder"),
+    repo = "https://gitlab.control.lth.se/labdev/software/tree/master/julia_ballandbeam/BallAndBeam.jl.git",
+    julia  = "0.6",
+    osname = "linux"
+)

docs/mkdocs.yml

+site_name:        BallAndBeam.jl
+repo_url:         https://gitlab.control.lth.se/labdev/software/tree/master/julia_ballandbeam/BallAndBeam.jl
+site_description: Documentation for BallAndBeam.jl
+site_author:      Fredri Bagge Carlson
+
+theme: cinder
+
+extra_css:
+  - assets/Documenter.css
+
+extra_javascript:
+  - https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_HTML
+  - assets/mathjaxhelper.js
+
+markdown_extensions:
+  - extra
+  - tables
+  - fenced_code
+  - mdx_math:
+      enable_dollar_delimiter: True
+
+docs_dir: 'build'
+
+pages:
+  - Home: index.md

docs/src/index.md

+# BallAndBeam
+
+```@contents
+Depth = 3
+```
+
+# Exported functions and types
+```@autodocs
+Modules = [BallAndBeam]
+Private = false
+Pages   = ["BallAndBeam.jl"]
+```
+
+
+# Usage
+1. Open a terminal
+2. Type `export JULIA_PKGDIR=/work/$USER\n export JULIA_EDITOR=gedit >> .bashrc`. Restart the terminal.
+3. Type `mkdir FRTN35_lab1; cd FRTN35_lab1`
+4. Start `julia`
+5. Install `BallAndBeam.jl` using command `Pkg.clone("https://gitlab.control.lth.se/labdev/software/tree/master/julia_ballandbeam/BallAndBeam.jl.git")` Lots of packages will now be installed, this will take some time.
+6. Type `cp(Pkg.dir("BallAndBeam","src","FRTN35_lab1.jl"), "."); edit("FRTN35_lab1.jl")`. The script `FRTN35_lab1.jl` will now open in a text editor, edit it while following the lab manual. If unsure about how a function works, type `?function_name` for help.
+
+
+# Lab 1 FRTN35
+```@autodocs
+Modules = [BallAndBeam]
+Private = false
+Pages   = ["FRTN35_lab1.jl"]
+```
+
+
+# Index
+
+```@index
+```

src/BallAndBeam.jl

+"""
+This module contains a controller for the Ball and Beam. It connects to the
+process through the gray analog IO boxes.
+
+Apart from 2DOF controller funcitonality [`run_control`](@ref), the module contains functions for performing
+frequency response analysis [`fra`](@ref).
+"""
+module BallAndBeam
+
+export io_control, io_measurement, @periodically, run_control, run_experiment, fra, sortfqs,
+bopl, bopl!, nypl, nypl!, plot, fbdesign, ffdesign, init_sysfilter, sysfilter!
+
+using Plots, Polynomials, ControlSystems, ProgressMeter
+
+const h = 1e-2
+const comedipath = "../c/comedi_bridge.so"
+
+const conversion = 65535/20
+io2num(x) = x/conversion - 10 			# Converts from io to float
+num2io(x) = round(Int32,x*100 + 2050)   # Converts from regular number to io
+
+
+"""
+	y = io_measurement()
+Gets a measurement from the analog IO box
+See also [`io_control`](@ref)
+"""
+io_measurement() = io2num(ccall((:comedi_read, comedipath),Int32,(Int32,Int32,Int32), 0,0,0))
+
+"""
+	io_control(u::AbstractArray)
+Sends a control signal to the analog IO box
+See also [`io_measurement`](@ref)
+"""
+function io_control(u::Number)
+	ccall((:comedi_write, comedipath),Int32,(Int32,Int32,Int32,Int32), 0,1,1,num2io(u[1]))
+end
+
+"""
+	@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
+		sleep(max(0,$h-execution_time))
+	end
+end
+
+
+"""
+	run_control(bias,sysFB, sysFF, duration = 10)
+Perform control experiemnt where the feedback and feedforward controllers are given by
+`sysFB` and `sysFF`, both of type `StateSpace`. See [`fbdesign`](@ref) [`ffdesign`](@ref).
+"""
+function run_control(bias,sysFB, sysFF, duration = 10)
+	y       = zeros(0:h:duration)
+	u       = zeros(0:h:duration)
+	r       = zeros(0:h:duration)
+	rfstate = zeros(length(Aff)-1)
+	stateFB = init_sysfilter(sysFB)
+	stateFF = init_sysfilter(sysFF)
+
+	function control(i)
+		rf = sysfilter!(stateFF, sysFF, r[i])
+		e  = rf-y[i]
+		ui = sysfilter!(stateFB, sysFB, e)
+	end
+
+	ccall((:comedi_start, comedipath),Int32,(Int32,), 0)
+	for (i,t) = enumerate(0:h:duration)
+		@periodically h begin
+			y[i]       = io_measurement()
+			r[i]       = reference(t)
+			u[i]       = control(i) # y,r must be updated before u
+			io_control(u[i] + bias)
+		end
+	end
+	io_control(0+bias)
+	ccall((:comedi_stop, "../c/comedi_bridge.so"),Int32,(Int32,), 0)
+	y,u,r
+end
+
+"""
+	run_experiment(w, duration, settling_time, amplitude, bias)
+Perform experiemnt For a single frequency `ω`. Called from inside `fra`
+"""
+function run_experiment(ω, duration, settling_time, amplitude, bias)
+	data = zeros(0:h:duration)
+	ccall((:comedi_start, comedipath),Int32,(Int32,), 0)
+	for t = 0:h:settling_time
+		@periodically h begin
+			u              = amplitude*sin(ω*t)
+			io_control(u+bias)
+		end
+	end
+	for (i,t) = enumerate(0:h:duration)
+		@periodically h begin
+			data[i]        = io_measurement()
+			u              = amplitude*sin(ω*t)
+			io_control(u+bias)
+		end
+	end
+	io_control(0+bias)
+	ccall((:comedi_stop, "../c/comedi_bridge.so"),Int32,(Int32,), 0)
+	data
+end
+
+"""
+	∫ = integrate(fun::Function, data::Vector, ω::Real)
+Calculate sin or cos channel, call like this `integrate(sin, data, ω)` for sin-channel
+integration
+"""
+integrate(fun,data,ω) = h*sum(fun(ω*(i-1)*h).*data[i] for i = 1:length(data))
+
+"""
+	fra(Ω::AbstractVector; kwargs...)
+
+# Arguments
+`bias           = 0`: Change the bias if the beam angle is drifting over time
+`settling_time  = 2`: In seconds, rounded up to closest integer periods
+`nbr_of_periods = 10`:
+`amplitude 	   = 1`: Very low freqs might require smaller amplitude
+"""
+function fra(Ω::AbstractVector;
+			bias           = 0,
+			settling_time  = 2,
+			nbr_of_periods = 10,
+			amplitude 	   = 1)
+
+	G = zeros(Complex, size(Ω)) 				      # Transfer function
+
+	@showprogress 1 "Running experiments"  for (i,ω) = enumerate(Ω)
+		T               = 2π/ω 			      # Period time
+		settling_time_i = ceil(settling_time/T)*T # Settling time even number of periods
+		duration        = nbr_of_periods*T
+		data            = run_experiment(ω,duration,settling_time_i,amplitude,bias)
+		sin_channel     = integrate(sin, data, ω)
+		cos_channel     = integrate(cos, data, ω)
+		G[i]            = 2/(amplitude*duration)*Complex(sin_channel, cos_channel)
+	end
+	Number[Ω G]
+end
+
+"""
+	sortfqs(G)
+Sorts matrix `G` based on the first column (which should contain frequencies)
+"""
+function sortfqs(G)
+	perm = sortperm(G[:,1])
+	G[perm,:]
+end
+
+function bopl!(G, kwargs...)
+	plot!(G[:,1].*(180/π), abs.(G[:,2]), ylabel="Magnitude", title="Bode plot", xscale=:log10, yscale=:log10, subplot=1, kwargs...)
+	plot!(G[:,1].*(180/π), angle.(G[:,2]), xlabel="Frequency [1/s]", ylabel="Phase", xscale=:log10, subplot=2, kwargs...)
+    plot!(link=:x)
+end
+
+nypl!(f,G, kwargs...) = plot!(f,real.(G[:,2]), imag.(G[:,2]),
+	xlabel="\$Re(G)\$", ylabel="\$Im(G)\$", title="Nyquist plot", kwargs...)
+
+"""
+	bopl(G)
+Plot a bodeplot when `G` is a matrix with frequencies in first column and complex magnitudes
+in the second. Add to an existing plot with `bopl!(...)` See also [`nypl`](@ref)
+"""
+bopl(args...; kwargs...) = (plot(layout=(2,1),link=:x);bopl!(args...;kwargs...))
+
+"""
+	nypl(G)
+Plot a nyquistplot when `G` is a matrix with frequencies in first column and complex magnitudes
+in the second. Add to an existing plot with `nypl!(...)` See also [`bopl`](@ref)
+"""
+nypl(args...; kwargs...) = (plot();nypl!(args...;kwargs...))
+
+
+"""
+	sysFB,L,T,C = fbdesign(G, polevect, zerovect, gain)
+Frequency Compensation to determine polynominals in compensator C=S/R.
+Frequency responses for loop transfer, closed loop and controller are calculated.
+`sys` is of type `StateSpace`, to be used together with [`sysfilter!`](@ref)
+"""
+function fbdesign(G, polevect, zerovect, gain)
+	ω = G[:,1]
+	r = poles2poly(polevect)
+	s = poles2poly(zerovect)
+	s = s*r[end]/s[end]*gain # Ensure static gain correct
+	C = frc(s,r,0,ω)
+	L = Number[ω G[:,2].*C[:,2]]
+	T = Number[ω L[:,2]./(1+L[:,2])]
+	sysFB = ss(tf(s,r))
+
+	sysFB,L,T,C
+end
+
+"""
+	sysFF,YR,FF = ffdesign(T, polevect, zerovect, gain)
+Feedforward filter BFF/AFF to shape transfer function from `r` to `y`.
+Frequency responses for closed loop with FF-filter and FF-filter are calculated.
+`sys` is of type `StateSpace`, to be used together with [`sysfilter!`](@ref)
+"""
+function ffdesign(T, polevect, zerovect, gain)
+	sys,YR,_,FF = fbdesign(G, polevect, zerovect, gain)
+	sys,YR,FF
+end
+
+"""
+	fr = frc(b,a,τ,ω)
+Computes the frequency response of a continuous time transfer function.
+
+The value of `G(s) = b(s)/a(s)*exp(-τ*s)` is calculated for
+the frequencies in ω [rad/s]
+"""
+function frc(b,a,τ,w)
+	Number[w gval(b,a,τ,im.*w)]
+end
+
+"""
+	gvec = gval(b,a,τ,ω)
+Evaluates a set of transfer functions at a vector of frequency points
+
+The value of `G(s) = b(s)/a(s)*exp(-τ*s)` is calculated for the complex frequency points
+in ω [rad/s]
+ """
+gval(b,a,τ,ω) = exp.(-τ.*ω).*polyval(Poly(reverse(b)),ω)./polyval(Poly(reverse(a)),ω)
+
+"""
+	poles2poly(poles)
+Returns the polynomial coefficients representing the polynomial with `poles`
+"""
+poles2poly(poles) = reduce((r,l) -> conv(r,[1, l]), [1], poles) #|> reverse |> Poly
+
+"""
+	state = init_sysfilter(sys::StateSpace)
+"""
+function init_sysfilter(sys::StateSpace)
+ 	zeros(sys.nx,1)
+end
+
+"""
+	output = sysfilter!(state, sys::StateSpace, input)
+Returns the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
+"""
+function sysfilter!(state, sys, input)
+	state .= sys.A*state + sys.B*input
+	output = sys.C*state + sys.D*input
+end
+
+
+
+end # module BallAndBeam

src/FRTN35_lab1.jl

+using BallAndBeam
+using JLD
+# @load "workspace.jld" # Run this command to restore a saved workspace
+
+bias           = 0 # Change this if your process drifts over time
+settling_time  = 1
+nbr_of_periods = 5
+
+# Below we define some frequency vectors (using logarithmic spacing)
+# and run three experiments. You may modify the freqency vectors
+# any way you want and add/remove experiments as needed.
+
+w1_100 = logspace(log10(1),log10(300),8)
+G1     = fra(w1_100, amplitude=1, bias=bias, nbr_of_periods=nbr_of_periods, settling_time=settling_time)
+@save "workspace.jld"
+
+w1_200 = logspace(log10(5),log10(50),20)
+G2     = fra(w1_200, amplitude=2, bias=bias, nbr_of_periods=nbr_of_periods, settling_time=settling_time)
+@save "workspace.jld"
+
+w1_300 = logspace(log10(10),log10(30),20)
+G3     = fra(w1_300, amplitude=2, bias=bias, nbr_of_periods=nbr_of_periods, settling_time=settling_time)
+@save "workspace.jld"
+
+
+# Concatenate (overlapping) estimates to be used and sort based on freq
+G12 = sortfqs([G1; G2; G3])
+
+bopl(G12)
+nypl(G12)
+
+
+
+# Control ==================================================================================
+# polevect = [1]
+# zerovect = []
+# gain     = 1
+
+# sysFBc,L,T,C = fbdesign(G, polevect, zerovect, gain)
+# sysFFc,YR,FF = ffdesign(T, polevect, zerovect, gain)
+# sysFB,sysFF  = c2d(sysFBc,h),c2d(sysFFc,h)
+# y,u,r        = run_control(bias, sysFB, sysFF, duration=10)
+# plot([y u r], lab = ["y" "u" "r"])

test/runtests.jl

+using Base.Test
+using BallAndBeam
+
+function test_sysfilter()
+        N = 10
+        u = randn(N)
+        b = [1, 1]
+        a = [1, 0.1, 1]
+	  sys = ss(tf(b,a,1))
+	state = init_sysfilter(sys)
+       yf = filt(b,a,u)
+	  yff = similar(yf)
+	for i in eachindex(u)
+		yff[i] = sysfilter!(state, sys, u[i])[1]
+	end
+	@test sum(abs,yf - yff) < √(eps())
+end
+
+test_sysfilter()
+
+bias           = 0 # Change this if your process drifts over time
+settling_time  = 1
+nbr_of_periods = 5
+
+w1_100 = logspace(log10(1),log10(300),8)
+G1 = Number[w1_100 Complex.(ones(8),0)]
+
+w1_200 = logspace(log10(5),log10(50),20)
+G2 = Number[w1_200 Complex.(ones(20),0)]
+
+# Concatenate (overlapping) estimates to be used and sort based on freq
+G12 = sortfqs([G1; G2])
+
+bopl(G12)
+nypl(G12)
+
+Control ==================================================================================
+polevect = [1]
+zerovect = []
+gain     = 1
+
+sysFBc,L,T,C = fbdesign(G12, polevect, zerovect, gain)
+sysFFc,YR,FF = ffdesign(T, polevect, zerovect, gain)
+sysFB,sysFF  = c2d(sysFBc,h),c2d(sysFFc,h)