Skip to content

Commit 4009bfa

Browse files
author
Brad Carman
committed
added bot app
1 parent 1aaa808 commit 4009bfa

6 files changed

Lines changed: 329 additions & 7 deletions

File tree

‎Project.toml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,23 +4,33 @@ authors = ["Fredrik Bagge Carlson"]
44
version = "0.2.0"
55

66
[deps]
7+
Bonito = "824d6782-a2ef-11e9-3a09-e5662e0c26f8"
78
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
89
ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
910
DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
1011
Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
1112
ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
13+
ModelingToolkitInputs = "9c47c465-b61a-4984-aca2-8d7e8f154266"
14+
ModelingToolkitParameters = "d96c1b6d-73fa-42cf-8af9-45e6140feccb"
1215
ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
1316
OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
1417
OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
1518
Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
19+
SciMLBase = "0bca4576-84f4-4d90-8ffe-ffa030f20462"
20+
ShoelaceWidgets = "1dc53738-a592-4c0c-95ab-f92b84dc4c89"
1621
StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
1722
WGLMakie = "276b4fcb-3e11-5398-bf8b-a0c2d153d008"
1823

1924
[compat]
25+
Bonito = "4.2.0"
2026
ControlSystemsMTK = "2.5.0"
2127
Makie = "0.24.8"
28+
ModelingToolkitInputs = "0.2.0"
29+
ModelingToolkitParameters = "0.3.0"
2230
ModelingToolkitStandardLibrary = "2.25.0"
2331
Plots = "1.41.2"
32+
SciMLBase = "2.132.0"
33+
ShoelaceWidgets = "0.8.7"
2434
StaticArrays = "1.9.15"
2535
WGLMakie = "0.13.8"
2636

‎scripts/app.jl‎

Lines changed: 244 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,244 @@
1+
using DyadBotComponents
2+
using ModelingToolkit
3+
using ModelingToolkit: t_nounits as t, D_nounits as D
4+
using ModelingToolkitInputs
5+
using ModelingToolkitParameters
6+
using OrdinaryDiffEq
7+
using SciMLBase
8+
9+
# --------------------------------------------------------------
10+
# Precompiled Constants
11+
# --------------------------------------------------------------
12+
@named bot = DyadBotComponents.CascadeControlledFlatDyadBot()
13+
bot_ns = ModelingToolkit.toggle_namespacing(bot, false)
14+
inputs = [bot_ns.v_ref]
15+
sys = mtkcompile(bot; inputs)
16+
sys, input_functions = ModelingToolkitInputs.build_input_functions(sys, inputs)
17+
bot_params = DyadBotComponents.CascadeControlledFlatDyadBotParams()
18+
prob = ODEProblem(sys, sys => bot_params, (0, Inf))
19+
bot_setters = cache(sys, DyadBotComponents.CascadeControlledFlatDyadBotParams);
20+
21+
22+
23+
# --------------------------------------------------------------
24+
# APP
25+
# --------------------------------------------------------------
26+
using ShoelaceWidgets
27+
using Bonito
28+
using WGLMakie
29+
30+
@kwdef struct AppState
31+
integrator = Ref{SciMLBase.DEIntegrator}()
32+
33+
v_ref = Ref(0.0)
34+
35+
setup = SLButton("setup")
36+
stop = SLButton("stop"; disabled=true)
37+
38+
loop = Ref(true)
39+
loop_thread = Ref{Task}()
40+
41+
left1 = SLButton("<")
42+
left2 = SLButton("<<")
43+
left3 = SLButton("<<<")
44+
pause = SLButton("||")
45+
right1 = SLButton(">")
46+
right2 = SLButton(">>")
47+
right3 = SLButton(">>>")
48+
49+
L = SLInput(0.5; label="length [m]")
50+
R = SLInput(0.1; label="wheel radius [m]")
51+
52+
kpo = SLInput(0.54; label="outer P")
53+
kio = SLInput(2.48; label="outer i")
54+
kdo = SLInput(0.0; label="outer i")
55+
56+
kpi = SLInput(15.6; label="inner P")
57+
kii = SLInput(1e6; label="inner i") #TODO: put to Inf
58+
kdi = SLInput(0.16; label="inner i")
59+
60+
61+
f = Figure()
62+
ax = Axis(f[1,1])
63+
64+
bot_params = DyadBotComponents.CascadeControlledFlatDyadBotParams()
65+
66+
base_transform = Ref{Makie.Transformation}()
67+
spoke_transform = Ref{Makie.Transformation}()
68+
body_transform = Ref{Makie.Transformation}()
69+
70+
end
71+
72+
left1(app::AppState, x) = app.v_ref[] = -1
73+
left2(app::AppState, x) = app.v_ref[] = -2
74+
left3(app::AppState, x) = app.v_ref[] = -3
75+
76+
pause(app::AppState, x) = app.v_ref[] = 0
77+
78+
right1(app::AppState, x) = app.v_ref[] = +1
79+
right2(app::AppState, x) = app.v_ref[] = +2
80+
right3(app::AppState, x) = app.v_ref[] = +3
81+
82+
function stop(app::AppState, x)
83+
84+
app.loop[] = false
85+
wait(app.loop_thread[])
86+
app.setup.disabled[] = false
87+
app.stop.disabled[] = true
88+
89+
end
90+
91+
92+
function build_fig!(app::AppState)
93+
94+
empty!(app.ax)
95+
96+
r = app.bot_params.plant.R #wheel radius
97+
y1 = r
98+
99+
L = app.bot_params.plant.L # body length
100+
y2 = y1 + L
101+
102+
w1 = r*0.5
103+
w2 = r*2
104+
105+
# wheel
106+
app.base_transform[] = Transformation(origin = Vec3d(0.0, y1, 0))
107+
app.spoke_transform[] = Transformation(app.base_transform[]; origin = Vec3d(0.0, y1, 0))
108+
linesegments!(app.ax, [-r,+r], [y1,y1]; transformation=app.spoke_transform[], color=:blue)
109+
linesegments!(app.ax, [0.0,0.0], [y1-r,y1+r]; transformation=app.spoke_transform[], color=:blue)
110+
poly!(app.ax, Circle(Point2f(0.0,y1), r); color=:transparent, strokecolor=:blue, strokewidth=2, transformation=app.spoke_transform[])
111+
112+
# body
113+
app.body_transform[] = Transformation(app.base_transform[]; origin = Vec3d(0.0, y1, 0))
114+
poly!(app.ax, Rect(0.0-w1/2, y1, w1, y2-y1); color=:transparent, strokecolor=:red, strokewidth=2, transformation=app.body_transform[])
115+
poly!(app.ax, Rect(0.0-w2/2, y2, w2, w2); color=:transparent, strokecolor=:green, strokewidth=2, transformation=app.body_transform[])
116+
117+
# frame
118+
poly!(app.ax, Rect(-3/2, 0 , 3, 3); color=:transparent)
119+
end
120+
121+
122+
function setup(app::AppState, x)
123+
124+
app.bot_params.plant.R = app.R.value[]
125+
app.bot_params.plant.L = app.L.value[]
126+
127+
app.bot_params.outer_controller.k = app.kpo.value[]
128+
app.bot_params.outer_controller.Ti = app.kio.value[]
129+
app.bot_params.outer_controller.Td = app.kdo.value[]
130+
131+
app.bot_params.inner_controller.k = app.kpi.value[]
132+
app.bot_params.inner_controller.Ti = app.kii.value[]
133+
app.bot_params.inner_controller.Td = app.kdi.value[]
134+
135+
build_fig!(app)
136+
137+
138+
# Update Model Parameters
139+
# -- precompiled constants: prob, bot_setters, sys
140+
prob_ = remake(prob, bot_setters, sys => app.bot_params)
141+
142+
# Initialize the integrator
143+
app.integrator[] = init(prob_, Rodas5P())
144+
145+
step_size = 0.1
146+
app.loop[] = true
147+
148+
r = app.bot_params.plant.R
149+
150+
app.loop_thread[] = Threads.@spawn while app.loop[]
151+
t0 = Base.time()
152+
153+
# input
154+
set_input!(input_functions, app.integrator[], sys.v_ref, app.v_ref[])
155+
156+
# step
157+
step!(app.integrator[], 0.1, true)
158+
159+
# update plot
160+
x = app.integrator[][sys.plant.x]
161+
theta = app.integrator[][sys.plant.theta]
162+
163+
translate!(app.base_transform[], x)
164+
rotate!(app.body_transform[], -theta + π)
165+
rotate!(app.spoke_transform[], -x/r)
166+
167+
# real time pause
168+
compute_time = Base.time() - t0
169+
sleep_time = max(step_size - compute_time, 0)
170+
sleep(sleep_time)
171+
end
172+
173+
app.setup.disabled[] = true
174+
app.stop.disabled[] = false
175+
176+
end
177+
178+
179+
function build_app!(app::AppState)
180+
181+
on(Base.Fix1(left1, app), app.left1.value)
182+
on(Base.Fix1(left2, app), app.left2.value)
183+
on(Base.Fix1(left3, app), app.left3.value)
184+
on(Base.Fix1(pause, app), app.pause.value)
185+
on(Base.Fix1(right1, app), app.right1.value)
186+
on(Base.Fix1(right2, app), app.right2.value)
187+
on(Base.Fix1(right3, app), app.right3.value)
188+
189+
on(Base.Fix1(setup, app), app.setup.value)
190+
on(Base.Fix1(stop, app), app.stop.value)
191+
192+
end
193+
194+
function initialize()
195+
app = AppState()
196+
build_app!(app)
197+
return app
198+
end
199+
200+
function get_head()
201+
DOM.head(
202+
DOM.title("JuliaHub | DyadBot"),
203+
DOM.head(
204+
get_shoelace()...
205+
)
206+
)
207+
end
208+
209+
210+
211+
function get_body(session, app::AppState)
212+
DOM.body(
213+
214+
DOM.div(
215+
app.R,
216+
app.L,
217+
DOM.hr(), #---------------
218+
app.kpo,
219+
app.kio,
220+
app.kdo,
221+
app.kpi,
222+
app.kii,
223+
app.kdi,
224+
DOM.hr(), #---------------
225+
DOM.div(app.setup, app.stop),
226+
DOM.hr(), #---------------
227+
DOM.div(app.left3, app.left2, app.left1, app.pause, app.right1, app.right2, app.right3),
228+
DOM.hr(), #---------------
229+
app.f
230+
)
231+
232+
233+
)
234+
end
235+
236+
app = initialize()
237+
238+
239+
App() do session
240+
DOM.html(
241+
get_head(),
242+
get_body(session, app)
243+
)
244+
end

‎src/DyadBotComponents.jl‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,13 @@
11
module DyadBotComponents
22

3+
include("blocks.jl") # Blocks parameter definitions
4+
35
include("DiscreteKalmanFilter.jl")
46
include("planar_flat.jl")
57
include("cascade_planar_flat.jl")
68

9+
10+
711
# include("planar_multibody.jl")
812
# include("segway_3d.jl")
913

‎src/blocks.jl‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
import ModelingToolkitParameters: Params
2+
3+
@kwdef mutable struct LimPIDParams <: Params
4+
k = 1
5+
Ti = 0.1
6+
Td = 0.01
7+
wp = 1
8+
wd = 1
9+
Ni = Ti == 0 ? Inf : √(max(Td / Ti, 1.0e-6))
10+
Nd = 10
11+
u_max = Inf
12+
u_min = u_max > 0 ? -u_max : -Inf
13+
end
14+
15+
Base.@kwdef mutable struct GainParams <: Params
16+
# parameters
17+
u_start::Real = 0.0
18+
y_start::Real = 0.0
19+
k::Real
20+
end
21+
22+
Base.@kwdef mutable struct SquareParams <: Params
23+
# parameters
24+
frequency::Real = 1.0
25+
amplitude::Real = 1.0
26+
offset::Real = 0.0
27+
start_time::Real = 0.0
28+
end
29+
30+
Base.@kwdef mutable struct ConstantParams <: Params
31+
# parameters
32+
k::Real = 0.0
33+
end
34+
35+
Base.@kwdef mutable struct AddParams <: Params
36+
# parameters
37+
k1::Real = 1.0
38+
k2::Real = 1.0
39+
end
40+

‎src/cascade_planar_flat.jl‎

Lines changed: 16 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,27 +1,36 @@
11

2+
Base.@kwdef mutable struct CascadeControlledFlatDyadBotParams <: Params
3+
# systems
4+
plant::FlatDyadBotParams = FlatDyadBotParams()
5+
inner_controller::LimPIDParams = LimPIDParams(k=15.6, Ti=Inf, Td=0.16, Nd=25, u_max=7)
6+
outer_controller::LimPIDParams = LimPIDParams(k=0.54, Ti=2.48, Td=0, Nd=600, wd=1, wp=0.5)
7+
end
8+
9+
210
# Cascade control: outer velocity loop + inner angle loop
311
@component function CascadeControlledFlatDyadBot(; name)
4-
pars = @parameters begin
5-
x_ref = 0.15 # Reference velocity
12+
vars = @variables begin
13+
v_ref(t), [input=true]
614
end
715

816
systems = @named begin
917
plant = FlatDyadBot()
1018
# Inner loop: angle controller
11-
inner_controller = Blocks.LimPID(k=15.6, Ti=Inf, Td=0.16, Nd=25, u_max=7)
19+
inner_controller = Blocks.LimPID()
1220
# Outer loop: velocity controller
13-
outer_controller = Blocks.LimPID(k=0.54, Ti=2.48, Td=0, Nd=600, wd=1, wp=0.5)
21+
outer_controller = Blocks.LimPID()
1422
neg_gain = Blocks.Gain(k=1)
1523
# ref = Blocks.Step(height=x_ref, start_time=5)
16-
ref = Blocks.Square(; frequency = 1/50, amplitude = x_ref, offset = 0.0, start_time = 0.0, smooth = true)
24+
# ref = Blocks.Square(; smooth = true)
1725
# Add pi offset to inner loop reference
1826
pi_offset = Blocks.Constant(k=pi)
1927
add_pi = Blocks.Add(k1=1, k2=1)
2028
end
2129

2230
eqs = [
2331
# Outer loop: velocity reference -> angle reference
24-
connect(ref.output, :r2, outer_controller.reference)
32+
# connect(ref.output, :r2, outer_controller.reference)
33+
outer_controller.reference.u ~ v_ref
2534
connect(plant.x_output, neg_gain.input)
2635
connect(neg_gain.output, :y2, outer_controller.measurement)
2736

@@ -35,5 +44,5 @@
3544
connect(inner_controller.ctr_output, :u, plant.control_input)
3645
]
3746

38-
System(eqs, t, [], pars; systems, name)
47+
System(eqs, t, vars, []; systems, name)
3948
end

0 commit comments

Comments
 (0)