Skip to content

Commit ae3eb48

Browse files
baggepinnenclaude
baggepinnen
and
claude
committed
Refactor control systems into swappable controller subcomponents
Move the entire control system (PID controllers, actuator/inter-loop gains, reference constants, and — for the discrete models — the samplers, ZOH and clock) out of the top-level closed-loop models into dedicated controller subcomponents with a purely continuous interface (measurements in, torque out). A continuous and a discrete closed-loop model now differ only by which controller component they instantiate. New subcomponents (continuous / discrete): - AngleController / DiscreteAngleController (dyad/closed_loop.dyad) - CascadeController / DiscreteCascadeController (dyad/closed_loop.dyad) - CascadeFFController / DiscreteCascadeFFController (dyad/cascade_ff.dyad) The six top-level models (AngleControlledDyadBot, CascadeControlledDyadBot, CascadeFFDyadBot and their Discrete* variants) become thin: world + plant + (reference/feed-forward generator) + controller. Tunable gains stay top-level parameters forwarded into the controller; the analysis points (y/u/y2/u2) live in the continuous controllers and are reached as model.controller.y etc. The discrete feed-forward controller uses the native u_ff input of DiscretePIDStandard (with_ff = true, k_ff = -1 on the position loop to replace the negating gain2), matching the continuous LimPID wiring, and adopts the LimPID-style connector names (u_s, u_m, u_ff, y) from DiscreteComponents. Tuning scripts (tune_angle_pid, tune_cascade_pid, tune_cascade_structured, compute_feedforward) updated to the namespaced analysis-point paths. Verified: all six models simulate via DyadInterface.TransientAnalysis, the full test suite passes, and tune_angle_pid.jl runs against the refactored model. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01FxiqD9h4podoj5e4tpLeGk
1 parent 48c35cc commit ae3eb48

33 files changed

Lines changed: 1853 additions & 978 deletions

‎dyad/cascade_ff.dyad‎

Lines changed: 292 additions & 272 deletions
Large diffs are not rendered by default.

‎dyad/closed_loop.dyad‎

Lines changed: 431 additions & 346 deletions
Large diffs are not rendered by default.

‎generated/AngleControlledDyadBot_definition.jl‎

Lines changed: 9 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,10 @@ tilt angle to zero. The robot starts from a tilted initial configuration and
1414
the controller recovers the upright pose. Since only the angle is controlled,
1515
the position of the robot drifts freely.
1616
17+
The control system is encapsulated in the `AngleController` subcomponent; the
18+
discrete-time model `DiscreteAngleControlledDyadBot` is identical except that
19+
`controller` is a `DiscreteAngleController`.
20+
1721
The controller gains can be tuned with the script `scripts/tune_angle_pid.jl`.
1822
1923
## Parameters:
@@ -87,30 +91,9 @@ The controller gains can be tuned with the script `scripts/tune_angle_pid.jl`.
8791
# Subcomponent plant of type DyadBotComponents.PlanarDyadBot
8892
plant_overrides = __pop_subcomponent_overrides!(__overrides, "plant")
8993
push!(__systems, @named plant = DyadBotComponents.PlanarDyadBot(; phi0=phi0, plant_overrides...))
90-
# Subcomponent angle_controller of type BlockComponents.Continuous.LimPID
91-
angle_controller_overrides = __pop_subcomponent_overrides!(__overrides, "angle_controller")
92-
push!(__systems, @named angle_controller = BlockComponents.Continuous.LimPID(; Nd=119.368, y_max=0.1, angle_controller_overrides...))
93-
__bindings[angle_controller.k] = k_angle
94-
__bindings[angle_controller.Ti] = Ti_angle
95-
__bindings[angle_controller.Td] = Td_angle
96-
# Now remove initial conditions in angle_controller that correspond to the bindings just added
97-
__angle_controller_ics = ModelingToolkit.get_initial_conditions(angle_controller)
98-
__no_namespace_angle_controller = ModelingToolkit.toggle_namespacing(angle_controller, false)
99-
__angle_controller_k = Symbolics.unwrap(__no_namespace_angle_controller.k)::Symbolics.SymbolicT
100-
delete!(__angle_controller_ics, __angle_controller_k)
101-
__angle_controller_Ti = Symbolics.unwrap(__no_namespace_angle_controller.Ti)::Symbolics.SymbolicT
102-
delete!(__angle_controller_ics, __angle_controller_Ti)
103-
__angle_controller_Td = Symbolics.unwrap(__no_namespace_angle_controller.Td)::Symbolics.SymbolicT
104-
delete!(__angle_controller_ics, __angle_controller_Td)
105-
# Subcomponent ref of type BlockComponents.Sources.Constant
106-
ref_overrides = __pop_subcomponent_overrides!(__overrides, "ref")
107-
push!(__systems, @named ref = BlockComponents.Sources.Constant(; k=Float64(0), ref_overrides...))
108-
# Subcomponent gain of type BlockComponents.Math.Gain
109-
gain_overrides = __pop_subcomponent_overrides!(__overrides, "gain")
110-
push!(__systems, @named gain = BlockComponents.Math.Gain(; k=Float64(-1), gain_overrides...))
111-
# Subcomponent constant_ff of type BlockComponents.Sources.Constant
112-
constant_ff_overrides = __pop_subcomponent_overrides!(__overrides, "constant_ff")
113-
push!(__systems, @named constant_ff = BlockComponents.Sources.Constant(; k=Float64(0), constant_ff_overrides...))
94+
# Subcomponent controller of type DyadBotComponents.AngleController
95+
controller_overrides = __pop_subcomponent_overrides!(__overrides, "controller")
96+
push!(__systems, @named controller = DyadBotComponents.AngleController(; k_angle=k_angle, Ti_angle=Ti_angle, Td_angle=Td_angle, controller_overrides...))
11497

11598
### Check there are no unmatched overrides
11699
isempty(__overrides) || throw(ArgumentError("overrides: [$(join(keys(__overrides), ", "))] don't match names found in model. These names may exist in the model but could have been conditionally excluded."))
@@ -123,13 +106,8 @@ The controller gains can be tuned with the script `scripts/tune_angle_pid.jl`.
123106
__assertions = []
124107

125108
### Equations
126-
push!(__eqs, connect(plant.theta, :y, angle_controller.u_m))
127-
push!(__eqs, connect(angle_controller.y, :u, gain.u))
128-
push!(__eqs, connect(plant.theta, angle_controller.u_m))
129-
push!(__eqs, connect(angle_controller.y, gain.u))
130-
push!(__eqs, connect(gain.y, plant.torque))
131-
push!(__eqs, connect(constant_ff.y, angle_controller.u_ff))
132-
push!(__eqs, connect(ref.y, angle_controller.u_s))
109+
push!(__eqs, connect(plant.theta, controller.measurement))
110+
push!(__eqs, connect(controller.torque, plant.torque))
133111

134112
# Return completely constructed System
135113
return System(__eqs, t, __vars, __params; systems=__systems, initial_conditions=__initial_conditions, guesses=__guesses, name, initialization_eqs=__initialization_eqs, bindings=__bindings, assertions=__assertions)

‎generated/AngleController_definition.jl‎

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
### DO NOT EDIT THIS FILE
2+
### This file is auto-generated by the Dyad command-line compiler.
3+
### If you edit this code it is likely to get overwritten.
4+
### Instead, update the Dyad source code and regenerate this file
5+
6+
7+
import Moshi as __Ext__Moshi
8+
9+
@doc Markdown.doc"""
10+
AngleController(; name, k_angle, Ti_angle, Td_angle)
11+
12+
Continuous-time angle control system for the balancing robot: a single PID
13+
controller (`BlockComponents.Continuous.LimPID`) regulating the body tilt
14+
angle to zero, followed by the sign-flipping actuator gain. The control system
15+
has a purely continuous interface (tilt-angle `measurement` in, motor `torque`
16+
out) so that it can be swapped for its discrete-time counterpart
17+
`DiscreteAngleController` without any other change to the closed-loop model.
18+
19+
The reference is held internally at zero. The analysis points `y` (angle
20+
measurement) and `u` (controller output) are exposed for loop-shaping and
21+
PID autotuning.
22+
23+
## Parameters:
24+
25+
| Name | Description | Units | Default value |
26+
| ------------ | ----------------------------------- | ------ | --------------- |
27+
| `k_angle` | Proportional gain of the angle controller | -- | 0.487401 |
28+
| `Ti_angle` | Integrator time constant of the angle controller | s | 0.0587352 |
29+
| `Td_angle` | Derivative time constant of the angle controller | s | 0.0420526 |
30+
31+
## Connectors
32+
33+
* `measurement` - This connector represents a real signal as an input to a component ([`RealInput`](@ref))
34+
* `torque` - This connector represents a real signal as an output from a component ([`RealOutput`](@ref))
35+
"""
36+
@component function AngleController(; name = nothing, k_angle=0.487401, Ti_angle=0.0587352, Td_angle=0.0420526, kwargs...)
37+
isnothing(name) && throw(ArgumentError("""
38+
The `name` keyword must be provided. Please consider using the `@named` macro,
39+
like so:
40+
41+
@named model = AngleController()
42+
"""))
43+
44+
__overrides = __build_overrides(kwargs)
45+
__params = Symbolics.SymbolicT[]
46+
__vars = Symbolics.SymbolicT[]
47+
__systems = System[]
48+
__guesses = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()
49+
__initial_conditions = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()
50+
__initialization_eqs = Equation[]
51+
__eqs = Equation[]
52+
__bindings = Dict{Symbolics.SymbolicT, Symbolics.SymbolicT}()
53+
54+
### Structural Parameters (functions)
55+
56+
### Structural Parameters (Final)
57+
58+
### Path Parameters (functions)
59+
60+
### Path Parameters (non-final)
61+
62+
### Final Parameters (declarations)
63+
64+
### Deferred assignment (default values that depend on final parameters)
65+
66+
### Symbolic Parameters
67+
__local__k_angle = k_angle
68+
append!(__params, @parameters (k_angle::Real), [description = "Proportional gain of the angle controller"])
69+
__initial_conditions[k_angle] = __local__k_angle
70+
__local__Ti_angle = Ti_angle
71+
append!(__params, @parameters (Ti_angle::Real), [description = "Integrator time constant of the angle controller"])
72+
__initial_conditions[Ti_angle] = __local__Ti_angle
73+
__local__Td_angle = Td_angle
74+
append!(__params, @parameters (Td_angle::Real), [description = "Derivative time constant of the angle controller"])
75+
__initial_conditions[Td_angle] = __local__Td_angle
76+
77+
### Final Parameters (assignments)
78+
79+
### Final Path Parameters
80+
append!(__vars, @variables (measurement(t)::Real), [input = true])
81+
append!(__vars, @variables (torque(t)::Real), [output = true])
82+
83+
### Variables (declarations)
84+
85+
### Variables (assignments)
86+
87+
### Constants
88+
__constants = Any[]
89+
90+
### Components
91+
# Subcomponent angle_controller of type BlockComponents.Continuous.LimPID
92+
angle_controller_overrides = __pop_subcomponent_overrides!(__overrides, "angle_controller")
93+
push!(__systems, @named angle_controller = BlockComponents.Continuous.LimPID(; Nd=119.368, y_max=0.1, angle_controller_overrides...))
94+
__bindings[angle_controller.k] = k_angle
95+
__bindings[angle_controller.Ti] = Ti_angle
96+
__bindings[angle_controller.Td] = Td_angle
97+
# Now remove initial conditions in angle_controller that correspond to the bindings just added
98+
__angle_controller_ics = ModelingToolkit.get_initial_conditions(angle_controller)
99+
__no_namespace_angle_controller = ModelingToolkit.toggle_namespacing(angle_controller, false)
100+
__angle_controller_k = Symbolics.unwrap(__no_namespace_angle_controller.k)::Symbolics.SymbolicT
101+
delete!(__angle_controller_ics, __angle_controller_k)
102+
__angle_controller_Ti = Symbolics.unwrap(__no_namespace_angle_controller.Ti)::Symbolics.SymbolicT
103+
delete!(__angle_controller_ics, __angle_controller_Ti)
104+
__angle_controller_Td = Symbolics.unwrap(__no_namespace_angle_controller.Td)::Symbolics.SymbolicT
105+
delete!(__angle_controller_ics, __angle_controller_Td)
106+
# Subcomponent ref of type BlockComponents.Sources.Constant
107+
ref_overrides = __pop_subcomponent_overrides!(__overrides, "ref")
108+
push!(__systems, @named ref = BlockComponents.Sources.Constant(; k=Float64(0), ref_overrides...))
109+
# Subcomponent gain of type BlockComponents.Math.Gain
110+
gain_overrides = __pop_subcomponent_overrides!(__overrides, "gain")
111+
push!(__systems, @named gain = BlockComponents.Math.Gain(; k=Float64(-1), gain_overrides...))
112+
# Subcomponent constant_ff of type BlockComponents.Sources.Constant
113+
constant_ff_overrides = __pop_subcomponent_overrides!(__overrides, "constant_ff")
114+
push!(__systems, @named constant_ff = BlockComponents.Sources.Constant(; k=Float64(0), constant_ff_overrides...))
115+
116+
### Check there are no unmatched overrides
117+
isempty(__overrides) || throw(ArgumentError("overrides: [$(join(keys(__overrides), ", "))] don't match names found in model. These names may exist in the model but could have been conditionally excluded."))
118+
119+
### Guesses
120+
121+
### Initialization Equations
122+
123+
### Assertions
124+
__assertions = []
125+
126+
### Equations
127+
push!(__eqs, connect(measurement, :y, angle_controller.u_m))
128+
push!(__eqs, connect(angle_controller.y, :u, gain.u))
129+
push!(__eqs, connect(measurement, angle_controller.u_m))
130+
push!(__eqs, connect(angle_controller.y, gain.u))
131+
push!(__eqs, connect(gain.y, torque))
132+
push!(__eqs, connect(constant_ff.y, angle_controller.u_ff))
133+
push!(__eqs, connect(ref.y, angle_controller.u_s))
134+
135+
# Return completely constructed System
136+
return System(__eqs, t, __vars, __params; systems=__systems, initial_conditions=__initial_conditions, guesses=__guesses, name, initialization_eqs=__initialization_eqs, bindings=__bindings, assertions=__assertions)
137+
end
138+
export AngleController

‎generated/AngleController_experiment.jl‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
### DO NOT EDIT THIS FILE
2+
### This file is auto-generated by the Dyad command-line compiler.
3+
### If you edit this code it is likely to get overwritten.
4+
### Instead, update the Dyad source code and regenerate this file
5+
6+
7+

‎generated/AngleController_test.jl‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
### DO NOT EDIT THIS FILE
2+
### This file is auto-generated by the Dyad command-line compiler.
3+
### If you edit this code it is likely to get overwritten.
4+
### Instead, update the Dyad source code and regenerate this file
5+
6+
7+

‎generated/CascadeControlledDyadBot_definition.jl‎

Lines changed: 10 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,9 @@ body tilt angle and an outer loop controls the position of the robot along
1414
the ground. The position reference is a square wave filtered through two
1515
first-order filters to obtain a smooth reference trajectory.
1616
17-
The outer position controller outputs the tilt-angle reference for the inner
18-
angle controller: to move forward, the robot first has to lean forward.
17+
The control system is encapsulated in the `CascadeController` subcomponent; the
18+
discrete-time model `DiscreteCascadeControlledDyadBot` is identical except that
19+
`controller` is a `DiscreteCascadeController`.
1920
2021
The six controller gains are exposed as top-level parameters so that they can
2122
be tuned, either one loop at a time with the scripts `scripts/tune_angle_pid.jl`
@@ -114,48 +115,9 @@ and `scripts/tune_cascade_pid.jl`, or all simultaneously with
114115
# Subcomponent firstorder1 of type BlockComponents.Continuous.FirstOrder
115116
firstorder1_overrides = __pop_subcomponent_overrides!(__overrides, "firstorder1")
116117
push!(__systems, @named firstorder1 = BlockComponents.Continuous.FirstOrder(; T=0.1, firstorder1_overrides...))
117-
# Subcomponent angle_controller of type BlockComponents.Continuous.LimPID
118-
angle_controller_overrides = __pop_subcomponent_overrides!(__overrides, "angle_controller")
119-
push!(__systems, @named angle_controller = BlockComponents.Continuous.LimPID(; Nd=119.368, y_max=0.1, angle_controller_overrides...))
120-
__bindings[angle_controller.k] = k_angle
121-
__bindings[angle_controller.Ti] = Ti_angle
122-
__bindings[angle_controller.Td] = Td_angle
123-
# Now remove initial conditions in angle_controller that correspond to the bindings just added
124-
__angle_controller_ics = ModelingToolkit.get_initial_conditions(angle_controller)
125-
__no_namespace_angle_controller = ModelingToolkit.toggle_namespacing(angle_controller, false)
126-
__angle_controller_k = Symbolics.unwrap(__no_namespace_angle_controller.k)::Symbolics.SymbolicT
127-
delete!(__angle_controller_ics, __angle_controller_k)
128-
__angle_controller_Ti = Symbolics.unwrap(__no_namespace_angle_controller.Ti)::Symbolics.SymbolicT
129-
delete!(__angle_controller_ics, __angle_controller_Ti)
130-
__angle_controller_Td = Symbolics.unwrap(__no_namespace_angle_controller.Td)::Symbolics.SymbolicT
131-
delete!(__angle_controller_ics, __angle_controller_Td)
132-
# Subcomponent pos_controller of type BlockComponents.Continuous.LimPID
133-
pos_controller_overrides = __pop_subcomponent_overrides!(__overrides, "pos_controller")
134-
push!(__systems, @named pos_controller = BlockComponents.Continuous.LimPID(; Nd=4.76616, wd=Float64(1), wp=Float64(1), y_max=deg2rad(25.0), pos_controller_overrides...))
135-
__bindings[pos_controller.k] = k_pos
136-
__bindings[pos_controller.Ti] = Ti_pos
137-
__bindings[pos_controller.Td] = Td_pos
138-
# Now remove initial conditions in pos_controller that correspond to the bindings just added
139-
__pos_controller_ics = ModelingToolkit.get_initial_conditions(pos_controller)
140-
__no_namespace_pos_controller = ModelingToolkit.toggle_namespacing(pos_controller, false)
141-
__pos_controller_k = Symbolics.unwrap(__no_namespace_pos_controller.k)::Symbolics.SymbolicT
142-
delete!(__pos_controller_ics, __pos_controller_k)
143-
__pos_controller_Ti = Symbolics.unwrap(__no_namespace_pos_controller.Ti)::Symbolics.SymbolicT
144-
delete!(__pos_controller_ics, __pos_controller_Ti)
145-
__pos_controller_Td = Symbolics.unwrap(__no_namespace_pos_controller.Td)::Symbolics.SymbolicT
146-
delete!(__pos_controller_ics, __pos_controller_Td)
147-
# Subcomponent gain of type BlockComponents.Math.Gain
148-
gain_overrides = __pop_subcomponent_overrides!(__overrides, "gain")
149-
push!(__systems, @named gain = BlockComponents.Math.Gain(; k=Float64(-1), gain_overrides...))
150-
# Subcomponent gain1 of type BlockComponents.Math.Gain
151-
gain1_overrides = __pop_subcomponent_overrides!(__overrides, "gain1")
152-
push!(__systems, @named gain1 = BlockComponents.Math.Gain(; k=Float64(-1), gain1_overrides...))
153-
# Subcomponent constant1 of type BlockComponents.Sources.Constant
154-
constant1_overrides = __pop_subcomponent_overrides!(__overrides, "constant1")
155-
push!(__systems, @named constant1 = BlockComponents.Sources.Constant(; k=Float64(0), constant1_overrides...))
156-
# Subcomponent constant2 of type BlockComponents.Sources.Constant
157-
constant2_overrides = __pop_subcomponent_overrides!(__overrides, "constant2")
158-
push!(__systems, @named constant2 = BlockComponents.Sources.Constant(; k=Float64(0), constant2_overrides...))
118+
# Subcomponent controller of type DyadBotComponents.CascadeController
119+
controller_overrides = __pop_subcomponent_overrides!(__overrides, "controller")
120+
push!(__systems, @named controller = DyadBotComponents.CascadeController(; k_angle=k_angle, Ti_angle=Ti_angle, Td_angle=Td_angle, k_pos=k_pos, Ti_pos=Ti_pos, Td_pos=Td_pos, controller_overrides...))
159121

160122
### Check there are no unmatched overrides
161123
isempty(__overrides) || throw(ArgumentError("overrides: [$(join(keys(__overrides), ", "))] don't match names found in model. These names may exist in the model but could have been conditionally excluded."))
@@ -170,21 +132,12 @@ and `scripts/tune_cascade_pid.jl`, or all simultaneously with
170132
__assertions = []
171133

172134
### Equations
173-
push!(__eqs, connect(plant.theta, :y, angle_controller.u_m))
174-
push!(__eqs, connect(plant.x, :y2, pos_controller.u_m))
175-
push!(__eqs, connect(angle_controller.y, :u, gain.u))
176-
push!(__eqs, connect(pos_controller.y, :u2, gain1.u))
177135
push!(__eqs, connect(square.y, firstorder.u))
178136
push!(__eqs, connect(firstorder.y, firstorder1.u))
179-
push!(__eqs, connect(plant.x, pos_controller.u_m))
180-
push!(__eqs, connect(pos_controller.y, gain1.u))
181-
push!(__eqs, connect(gain1.y, angle_controller.u_s))
182-
push!(__eqs, connect(plant.theta, angle_controller.u_m))
183-
push!(__eqs, connect(angle_controller.y, gain.u))
184-
push!(__eqs, connect(gain.y, plant.torque))
185-
push!(__eqs, connect(constant1.y, pos_controller.u_ff))
186-
push!(__eqs, connect(constant2.y, angle_controller.u_ff))
187-
push!(__eqs, connect(firstorder1.y, pos_controller.u_s))
137+
push!(__eqs, connect(firstorder1.y, controller.pos_reference))
138+
push!(__eqs, connect(plant.x, controller.pos_measurement))
139+
push!(__eqs, connect(plant.theta, controller.angle_measurement))
140+
push!(__eqs, connect(controller.torque, plant.torque))
188141

189142
# Return completely constructed System
190143
return System(__eqs, t, __vars, __params; systems=__systems, initial_conditions=__initial_conditions, guesses=__guesses, name, initialization_eqs=__initialization_eqs, bindings=__bindings, assertions=__assertions)

0 commit comments

Comments
 (0)