initial animation setup

Author: Brad Carman

Project.toml

@@ -1,25 +1,28 @@
 name = "DyadBotComponents"
 uuid = "6baa8624-4e17-4d27-96ac-5a32f577bc5c"
-version = "0.1.0"
 authors = ["Fredrik Bagge Carlson"]
+version = "0.2.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
 ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
 DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
+Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
-
+WGLMakie = "276b4fcb-3e11-5398-bf8b-a0c2d153d008"
 
 [compat]
 ControlSystemsMTK = "2.5.0"
+Makie = "0.24.8"
 ModelingToolkitStandardLibrary = "2.25.0"
 Plots = "1.41.2"
 StaticArrays = "1.9.15"
+WGLMakie = "0.13.8"
 
 [extras]
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

scripts/animation.jl

@@ -0,0 +1,67 @@
+using DyadBotComponents
+using WGLMakie
+using ModelingToolkit
+using OrdinaryDiffEq
+
+f = Figure(size = (600, 600); aspect=DataAspect())
+
+ax = Axis(f[1, 1])
+
+#TODO: connect to the bot parameters
+y1 = 1.0
+r = 1.0
+
+y2 = 3.0
+w = 2
+
+
+base_transform = Transformation(origin = Vec3d(0.0, y1, 0))
+spoke_transform = Transformation(base_transform; origin = Vec3d(0.0, y1, 0))
+linesegments!(ax, [-r,+r], [y1,y1]; transformation=spoke_transform, color=:blue)
+linesegments!(ax, [0.0,0.0], [y1-r,y1+r]; transformation=spoke_transform, color=:blue)
+poly!(ax, Circle(Point2f(0.0,y1), r); color=:transparent, strokecolor=:blue, strokewidth=2, transformation=spoke_transform)
+
+body_transform = Transformation(base_transform; origin = Vec3d(0.0, y1, 0))
+poly!(ax, Rect(0.0-0.25/2, y1, 0.25, y2-y1); color=:transparent, strokecolor=:red, strokewidth=2, transformation=body_transform)
+poly!(ax, Rect(0.0-w/2, y2, w, w); color=:transparent, strokecolor=:green, strokewidth=2, transformation=body_transform)
+
+poly!(ax, Rect(-2.5, 0 , 5, 5); color=:transparent)
+
+rotate!(spoke_transform, π*0.1)
+rotate!(spoke_transform, π*0.2)
+rotate!(spoke_transform, π*0.3)
+
+translate!(base_transform, 0.5)
+
+rotate!(body_transform, -π*0.1)
+
+
+
+@mtkcompile bot = DyadBotComponents.CascadeControlledFlatDyadBot()
+prob = ODEProblem(bot, [bot.x_ref => 0.75], (0,200))
+sol = solve(prob)
+
+plot(sol; idxs=bot.plant.x)
+plot(sol; idxs=bot.plant.theta)
+plot(sol; idxs=bot.ref.output.u)
+
+nframes = 200
+framerate = 30
+times = range(0, 200, length=nframes)
+
+translate!(base_transform, 0.0)
+rotate!(body_transform, 0.0)
+rotate!(spoke_transform, 0.0)
+
+record(f, "dyadbot.mp4", times;
+        framerate = 10) do t
+    x = sol(t; idxs=bot.plant.x)
+    theta = sol(t; idxs=bot.plant.theta)
+
+    # x = wheel*r*π
+    
+    translate!(base_transform, x)
+    rotate!(body_transform, -theta + π)
+    rotate!(spoke_transform, -x/(r*π))
+     
+end

src/DyadBotComponents.jl

@@ -1,6 +1,8 @@
 module DyadBotComponents
 
 include("DiscreteKalmanFilter.jl")
+include("planar_flat.jl")
+include("cascase_planar_flat.jl")
 
 # include("planar_multibody.jl")
 # include("segway_3d.jl")

src/cascase_planar_flat.jl

@@ -0,0 +1,39 @@
+
+# Cascade control: outer velocity loop + inner angle loop
+@component function CascadeControlledFlatDyadBot(; name)
+    pars = @parameters begin
+        x_ref = 0.15  # Reference velocity
+    end
+
+    systems = @named begin
+        plant = FlatDyadBot()
+        # Inner loop: angle controller
+        inner_controller = Blocks.LimPID(k=15.6, Ti=Inf, Td=0.16, Nd=25, u_max=7)
+        # Outer loop: velocity controller
+        outer_controller = Blocks.LimPID(k=0.54, Ti=2.48, Td=0, Nd=600, wd=1, wp=0.5)
+        neg_gain = Blocks.Gain(k=1)
+        # ref = Blocks.Step(height=x_ref, start_time=5)
+        ref = Blocks.Square(; frequency = 1/50, amplitude = x_ref, offset = 0.0, start_time = 0.0, smooth = true)
+        # Add pi offset to inner loop reference
+        pi_offset = Blocks.Constant(k=pi)
+        add_pi = Blocks.Add(k1=1, k2=1)
+    end
+
+    eqs = [
+        # Outer loop: velocity reference -> angle reference
+        connect(ref.output, :r2, outer_controller.reference)
+        connect(plant.x_output, neg_gain.input)
+        connect(neg_gain.output, :y2, outer_controller.measurement)
+
+        # Add pi to outer controller output for inner loop reference
+        connect(outer_controller.ctr_output, :u2, add_pi.input1)
+        connect(pi_offset.output, add_pi.input2)
+
+        # Inner loop: angle reference -> torque
+        connect(add_pi.output, inner_controller.reference)
+        connect(plant.theta_output, :y, inner_controller.measurement)
+        connect(inner_controller.ctr_output, :u, plant.control_input)
+    ]
+
+    System(eqs, t, [], pars; systems, name)
+end

src/planar_flat.jl

@@ -1,16 +1,16 @@
 using ModelingToolkit
 import ModelingToolkit: t_nounits as t, D_nounits as D
-import ModelingToolkitStandardLibrary.Mechanical.Rotational 
+# import ModelingToolkitStandardLibrary.Mechanical.Rotational 
 import ModelingToolkitStandardLibrary.Blocks 
-using OrdinaryDiffEq
-using DyadControlSystems, ControlSystemsBase, ControlSystemsMTK
-using Plots
-import DyadControlSystems as JSC
-using LinearAlgebra
+# using OrdinaryDiffEq
+# using DyadControlSystems, ControlSystemsBase, ControlSystemsMTK
+# # using Plots
+# import DyadControlSystems as JSC
+# # using LinearAlgebra
 
 
-Plots.default(size=(1200,1200))
-connect = ModelingToolkit.connect
+# # Plots.default(size=(1200,1200))
+# connect = ModelingToolkit.connect
 
 
 ##