From 67d40f2dcda394fad1a6d23b34e0a61438d0f947 Mon Sep 17 00:00:00 2001 From: Joppe Blondel Date: Tue, 10 Mar 2026 14:36:24 +0100 Subject: [PATCH] pendulum controller --- Simple2DPendulum.mo | 27 +++++ test2d.mo | 236 ++++++++++++++++++++++++++++++++++++++++++-- vis2d.mo | 100 ------------------- 3 files changed, 255 insertions(+), 108 deletions(-) create mode 100644 Simple2DPendulum.mo delete mode 100644 vis2d.mo diff --git a/Simple2DPendulum.mo b/Simple2DPendulum.mo new file mode 100644 index 0000000..c203b2c --- /dev/null +++ b/Simple2DPendulum.mo @@ -0,0 +1,27 @@ +model Simple2DPendulum + inner Modelica.Mechanics.MultiBody.World world annotation( + Placement(transformation(origin = {-40, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Joints.Revolute revolute(phi(start = -1.5707963267948966, fixed = true, displayUnit = "deg"), useAxisFlange = true) annotation( + Placement(transformation(origin = {40, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Parts.Body body(r_CM = {0, -0.5, 0}, m = 1, I_11 = 0.001, I_22 = 0.001, I_33 = 0.001) annotation( + Placement(transformation(origin = {70, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Components.Damper damper(d = 0.02) annotation( + Placement(transformation(origin = {40, 38}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Joints.Prismatic prismatic annotation( + Placement(transformation(extent = {{-10, -10}, {10, 10}}))); +equation + connect(revolute.frame_b, body.frame_a) annotation( + Line(points = {{50, 0}, {60, 0}}, color = {95, 95, 95})); + connect(damper.flange_b, revolute.axis) annotation( + Line(points = {{50, 38}, {58, 38}, {58, 20}, {40, 20}, {40, 10}})); + connect(damper.flange_a, revolute.support) annotation( + Line(points = {{30, 38}, {22, 38}, {22, 20}, {34, 20}, {34, 10}})); + connect(prismatic.frame_b, revolute.frame_a) annotation( + Line(points = {{10, 0}, {30, 0}}, color = {95, 95, 95})); + connect(world.frame_b, prismatic.frame_a) annotation( + Line(points = {{-30, 0}, {-10, 0}}, color = {95, 95, 95})); + +annotation( + uses(Modelica(version = "4.1.0")), + experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02)); +end Simple2DPendulum; \ No newline at end of file diff --git a/test2d.mo b/test2d.mo index 17ba3d0..95ed07d 100644 --- a/test2d.mo +++ b/test2d.mo @@ -1,4 +1,107 @@ package test2d + +model vis2d + import Modelica.Mechanics.MultiBody.Visualizers; + import Modelica.Mechanics.MultiBody.Frames; + + parameter Real L = 1; + parameter Real w = 0.02; + parameter Real orbRadius = 0.04; + parameter Real axisLength = 0.25; + parameter Real axisWidth = 0.01; + parameter Real planeSize = 2.0; + parameter Real planeThickness = 0.002; + parameter Real planeOffset = -0.001; + + Modelica.Blocks.Interfaces.RealInput phi annotation( + Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.RealInput y annotation( + Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.RealInput x annotation( + Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}}))); +protected + Real Rz[3,3]; + Real r[3]; + + Visualizers.Advanced.Shape link( + shapeType = "cylinder", + length = L/2, + width = w, + height = w, + r_shape = {L / 2, 0, 0}, + lengthDirection = {-1, 0, 0}, + widthDirection = {0, 1, 0}, + r = r, + R = Frames.from_T(Rz, zeros(3))); + + Visualizers.Advanced.Shape orb( + shapeType = "sphere", + length = 2 * orbRadius, + width = 2 * orbRadius, + height = 2 * orbRadius, + r_shape = {0, 0, 0}, + lengthDirection = {1, 0, 0}, + widthDirection = {0, 1, 0}, + r = r, + R = Frames.from_T(Rz, zeros(3))); + + Visualizers.Advanced.Shape xAxis( + shapeType = "cylinder", + length = axisLength, + width = axisWidth, + height = axisWidth, + color = {255, 0, 0}, + specularCoefficient = 0.1, + r_shape = {axisLength / 2, 0, 0}, + lengthDirection = {1, 0, 0}, + widthDirection = {0, 1, 0}, + r = zeros(3), + R = Frames.from_T([1, 0, 0; + 0, 1, 0; + 0, 0, 1], zeros(3))); + + Visualizers.Advanced.Shape yAxis( + shapeType = "cylinder", + length = axisLength, + width = axisWidth, + height = axisWidth, + color = {0, 200, 0}, + specularCoefficient = 0.1, + r_shape = {0, axisLength / 2, 0}, + lengthDirection = {0, 1, 0}, + widthDirection = {0, 0, 1}, + r = zeros(3), + R = Frames.from_T([1, 0, 0; + 0, 1, 0; + 0, 0, 1], zeros(3))); + + Visualizers.Advanced.Shape zAxis( + shapeType = "cylinder", + length = axisLength, + width = axisWidth, + height = axisWidth, + color = {0, 90, 255}, + specularCoefficient = 0.1, + r_shape = {0, 0, axisLength / 2}, + lengthDirection = {0, 0, 1}, + widthDirection = {1, 0, 0}, + r = zeros(3), + R = Frames.from_T([1, 0, 0; + 0, 1, 0; + 0, 0, 1], zeros(3))); +equation + r = {x, y, 0}; + // Negate phi here since shapes use inverse frame-mapping + Rz = [cos(-phi), -sin(-phi), 0; + sin(-phi), cos(-phi), 0; + 0, 0, 1]; + + annotation( + uses(Modelica(version = "4.1.0")), + Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")}), + experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002)); +end vis2d; + model pendulum parameter Modelica.Units.SI.Mass mass; parameter Modelica.Units.SI.Length length; @@ -131,9 +234,10 @@ package test2d end rTF; model test - pendulum pendulum1(mass = 1, length = 1, momentOfInertia = 0.001, startAngle = 0) annotation( + import Modelica.Constants.pi; + pendulum pendulum1(mass = 1, length = 1, momentOfInertia = 0.001, startAngle = 1.5707963267948966) annotation( Placement(transformation(origin = {60, 0}, extent = {{-20, -20}, {20, 20}}))); - BondGraph.J0 j1(N = 2, s = {1, -1}) annotation( + BondGraph.J0 j1(N = 3, s = {1, -1, 1}) annotation( Placement(transformation(origin = {60, 40}, extent = {{-10, -10}, {10, 10}}))); BondGraph.J1 w_hinge(N = 2, s = {-1, -1}) annotation( Placement(transformation(origin = {20, 40}, extent = {{-10, -10}, {10, 10}}))); @@ -147,10 +251,26 @@ package test2d Placement(transformation(origin = {30, -40}, extent = {{-10, -10}, {10, 10}}))); BondGraph.J1 j3(N = 2, s = {1, -1}) annotation( Placement(transformation(origin = {-20, -60}, extent = {{-10, -10}, {10, 10}}))); - BondGraph.J1 j4(N = 1, s = {-1}) annotation( + BondGraph.J1 j4(N = 2, s = {-1, 1}) annotation( Placement(transformation(origin = {-20, -20}, extent = {{-10, -10}, {10, 10}}))); BondGraph.Sf sf(f0 = 0) annotation( Placement(transformation(origin = {-58, -60}, extent = {{-10, -10}, {10, 10}}))); + BondGraph.Sf sf1(f0 = 0) annotation( + Placement(transformation(origin = {-56, -20}, extent = {{-10, -10}, {10, 10}}))); + BondGraph.J1 w_maxle(N = 2, s = {-1, 1}) annotation( + Placement(transformation(origin = {60, 80}, extent = {{-10, -10}, {10, 10}}))); + BondGraph.mSe T_motor annotation( + Placement(transformation(origin = {20, 80}, extent = {{-10, -10}, {10, 10}}))); + controller controller1(g_P = 50, g_I = 7.5, g_D = 5, sampleTime(displayUnit = "ms") = 0.01, limit = 20) annotation( + Placement(transformation(origin = {16, 110}, extent = {{10, -10}, {-10, 10}}))); + Modelica.Blocks.Sources.TimeTable timeTable(table = [ + 0, 0; 4,0; + 5, 1.5*pi; 9, 1.5*pi; + 10, pi; 14, pi; + 15, 0.5*pi; 19, 0.5*pi; + 20, 0; 25, 0 + ]) annotation( + Placement(transformation(origin = {50, 130}, extent = {{-10, -10}, {10, 10}}, rotation = 180))); equation connect(w_hinge.P[1], j1.P[1]) annotation( Line(points = {{20, 40}, {60, 40}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6)); @@ -162,17 +282,117 @@ package test2d // Line(points = {{60, -50}, {60, -20}}, color = {0, 85, 0}, thickness = 0.75, arrow = //{Arrow.None, Arrow.Half}, arrowSize = 6)); connect(j2.P[1], pendulum1.trans_i) annotation( Line(points = {{60, -40}, {60, -20}}, color = {0, 85, 0})); - connect(muxDemux.p, j2.P[2]) annotation( + connect(muxDemux.p, j2.P[2]) annotation( Line(points = {{40, -40}, {60, -40}}, color = {0, 85, 0})); - connect(j3.P[2], muxDemux.p2) annotation( + connect(j3.P[2], muxDemux.p2) annotation( Line(points = {{-20, -60}, {8, -60}, {8, -44}, {20, -44}}, color = {0, 0, 127})); - connect(j4.P[1], muxDemux.p1) annotation( + connect(j4.P[1], muxDemux.p1) annotation( Line(points = {{-20, -20}, {8, -20}, {8, -36}, {20, -36}}, color = {0, 0, 127})); - connect(sf.p, j3.P[1]) annotation( + connect(sf.p, j3.P[1]) annotation( Line(points = {{-50, -60}, {-20, -60}}, color = {0, 0, 127})); + connect(sf1.p, j4.P[2]) annotation( + Line(points = {{-48, -20}, {-20, -20}}, color = {0, 0, 127})); + connect(w_maxle.P[1], j1.P[3]) annotation( + Line(points = {{60, 80}, {60, 40}}, color = {0, 0, 127}, thickness = 0.5)); + connect(T_motor.p, w_maxle.P[2]) annotation( + Line(points = {{28, 80}, {60, 80}}, color = {0, 0, 127})); + connect(pendulum1.phi, controller1.angle) annotation( + Line(points = {{80, 8}, {92, 8}, {92, 110}, {26, 110}}, color = {0, 0, 127})); + connect(controller1.motor, T_motor.e0) annotation( + Line(points = {{6, 110}, {6, 109}, {0, 109}, {0, 80}, {20, 80}, {20, 72}}, color = {0, 0, 127})); + connect(timeTable.y, controller1.setpoint) annotation( + Line(points = {{39, 130}, {39, 117}, {26, 117}}, color = {0, 0, 127})); annotation( - experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02)); + experiment(StartTime = 0, StopTime = 25, Tolerance = 1e-06, Interval = 0.02), + Diagram(coordinateSystem(extent = {{-80, 140}, {100, -80}}))); end test; + + model controller + import Modelica.Blocks.Interfaces.RealInput; + import Modelica.Blocks.Interfaces.RealOutput; + + parameter Real g_P = 1; + parameter Real g_I = 1; + parameter Real g_D = 1; + parameter Modelica.Units.SI.Time sampleTime = 0.0025; + parameter Real limit = 100; + + RealInput setpoint(quantity="Angle") "Setpoint" annotation( + Placement(transformation(origin = {-158, 20}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-96, 70}, extent = {{-10, -10}, {10, 10}}))); + RealInput angle(quantity="Angle") "Angle of the pendulum" annotation( + Placement(transformation(origin = {-158, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-96, 0}, extent = {{-10, -10}, {10, 10}}))); + RealOutput motor(quantity="Torque") "Motor drive" annotation( + Placement(transformation(origin = {226, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {104, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Discrete.Sampler sampler(samplePeriod = sampleTime) annotation( + Placement(transformation(origin = {-126, -6}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.Add add(k1 = 1, k2 = -1) annotation( + Placement(transformation(origin = {-86, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.Gain gain(k = g_P) annotation( + Placement(transformation(origin = {-30, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Discrete.UnitDelay unitDelay(samplePeriod = sampleTime) annotation( + Placement(transformation(origin = {-36, 32}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.MultiSum multiSum(nu = 3) annotation( + Placement(transformation(origin = {122, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.Gain gain1(k = g_D / sampleTime) annotation( + Placement(transformation(origin = {56, 38}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.Add add1(k1 = +1, k2 = -1) annotation( + Placement(transformation(origin = {8, 38}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Discrete.Sampler sampler1(samplePeriod = sampleTime) annotation( + Placement(transformation(origin = {-126, 20}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Discrete.ZeroOrderHold zeroOrderHold(samplePeriod = sampleTime) annotation( + Placement(transformation(origin = {192, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Nonlinear.Limiter limiter(uMax = limit) annotation( + Placement(transformation(origin = {158, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Discrete.UnitDelay unitDelay1(samplePeriod = sampleTime) annotation( + Placement(transformation(origin = {50, -50}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Math.Gain gain11(k = g_I*sampleTime) annotation( + Placement(transformation(origin = {-30, -44}, extent = {{10, -10}, {-10, 10}}, rotation = 180))); + Modelica.Blocks.Math.Add add11(k1 = +1, k2 = +1) annotation( + Placement(transformation(origin = {14, -50}, extent = {{-10, -10}, {10, 10}}, rotation = -0))); + equation + connect(angle, sampler.u) annotation( + Line(points = {{-158, -6}, {-138, -6}}, color = {0, 0, 127})); + connect(sampler.y, add.u2) annotation( + Line(points = {{-115, -6}, {-99, -6}}, color = {0, 0, 127})); + connect(add.y, gain.u) annotation( + Line(points = {{-75, 0}, {-42, 0}}, color = {0, 0, 127})); + connect(gain.y, multiSum.u[1]) annotation( + Line(points = {{-19, 0}, {112, 0}}, color = {0, 0, 127})); + connect(gain1.y, multiSum.u[2]) annotation( + Line(points = {{67, 38}, {95, 38}, {95, 0}, {111, 0}}, color = {0, 0, 127})); + connect(unitDelay.y, add1.u2) annotation( + Line(points = {{-25, 32}, {-4, 32}}, color = {0, 0, 127})); + connect(add.y, add1.u1) annotation( + Line(points = {{-74, 0}, {-62, 0}, {-62, 54}, {-12, 54}, {-12, 44}, {-4, 44}}, color = {0, 0, 127})); + connect(add.y, unitDelay.u) annotation( + Line(points = {{-74, 0}, {-62, 0}, {-62, 32}, {-48, 32}}, color = {0, 0, 127})); + connect(add1.y, gain1.u) annotation( + Line(points = {{20, 38}, {44, 38}}, color = {0, 0, 127})); + connect(setpoint, sampler1.u) annotation( + Line(points = {{-158, 20}, {-138, 20}}, color = {0, 0, 127})); + connect(sampler1.y, add.u1) annotation( + Line(points = {{-114, 20}, {-110, 20}, {-110, 6}, {-98, 6}}, color = {0, 0, 127})); + connect(multiSum.y, limiter.u) annotation( + Line(points = {{134, 0}, {146, 0}}, color = {0, 0, 127})); + connect(limiter.y, zeroOrderHold.u) annotation( + Line(points = {{170, 0}, {180, 0}}, color = {0, 0, 127})); + connect(zeroOrderHold.y, motor) annotation( + Line(points = {{204, 0}, {226, 0}}, color = {0, 0, 127})); + connect(unitDelay1.y, multiSum.u[3]) annotation( + Line(points = {{61, -50}, {94, -50}, {94, 0}, {112, 0}}, color = {0, 0, 127})); + connect(unitDelay1.y, add11.u2) annotation( + Line(points = {{62, -50}, {80, -50}, {80, -74}, {-10, -74}, {-10, -56}, {2, -56}}, color = {0, 0, 127})); + connect(gain11.y, add11.u1) annotation( + Line(points = {{-19, -44}, {2, -44}}, color = {0, 0, 127})); + connect(add.y, gain11.u) annotation( + Line(points = {{-74, 0}, {-62, 0}, {-62, -44}, {-42, -44}}, color = {0, 0, 127})); + connect(add11.y, unitDelay1.u) annotation( + Line(points = {{26, -50}, {38, -50}}, color = {0, 0, 127})); + annotation( + Icon(graphics = {Rectangle(lineColor = {211, 211, 211}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, lineThickness = 2, extent = {{-100, 100}, {100, -100}}, radius = 25)}), + experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002), + Diagram(coordinateSystem(extent = {{-180, 60}, {240, -20}}))); +end controller; annotation( uses(Modelica(version = "4.1.0"))); end test2d; \ No newline at end of file diff --git a/vis2d.mo b/vis2d.mo deleted file mode 100644 index c12f56b..0000000 --- a/vis2d.mo +++ /dev/null @@ -1,100 +0,0 @@ -model vis2d - import Modelica.Mechanics.MultiBody.Visualizers; - import Modelica.Mechanics.MultiBody.Frames; - - parameter Real L = 1; - parameter Real w = 0.02; - parameter Real orbRadius = 0.04; - parameter Real axisLength = 0.25; - parameter Real axisWidth = 0.01; - parameter Real planeSize = 2.0; - parameter Real planeThickness = 0.002; - parameter Real planeOffset = -0.001; - - Modelica.Blocks.Interfaces.RealInput phi annotation( - Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Interfaces.RealInput y annotation( - Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Interfaces.RealInput x annotation( - Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}}))); -protected - Real Rz[3,3]; - Real r[3]; - - Visualizers.Advanced.Shape link( - shapeType = "cylinder", - length = L, - width = w, - height = w, - r_shape = {-L / 2, 0, 0}, - lengthDirection = {1, 0, 0}, - widthDirection = {0, 1, 0}, - r = r, - R = Frames.from_T(Rz, zeros(3))); - - Visualizers.Advanced.Shape orb( - shapeType = "sphere", - length = 2 * orbRadius, - width = 2 * orbRadius, - height = 2 * orbRadius, - r_shape = {L / 2 - orbRadius, 0, 0}, - lengthDirection = {1, 0, 0}, - widthDirection = {0, 1, 0}, - r = r, - R = Frames.from_T(Rz, zeros(3))); - - Visualizers.Advanced.Shape xAxis( - shapeType = "cylinder", - length = axisLength, - width = axisWidth, - height = axisWidth, - color = {255, 0, 0}, - specularCoefficient = 0.1, - r_shape = {axisLength / 2, 0, 0}, - lengthDirection = {1, 0, 0}, - widthDirection = {0, 1, 0}, - r = zeros(3), - R = Frames.from_T([1, 0, 0; - 0, 1, 0; - 0, 0, 1], zeros(3))); - - Visualizers.Advanced.Shape yAxis( - shapeType = "cylinder", - length = axisLength, - width = axisWidth, - height = axisWidth, - color = {0, 200, 0}, - specularCoefficient = 0.1, - r_shape = {0, axisLength / 2, 0}, - lengthDirection = {0, 1, 0}, - widthDirection = {0, 0, 1}, - r = zeros(3), - R = Frames.from_T([1, 0, 0; - 0, 1, 0; - 0, 0, 1], zeros(3))); - - Visualizers.Advanced.Shape zAxis( - shapeType = "cylinder", - length = axisLength, - width = axisWidth, - height = axisWidth, - color = {0, 90, 255}, - specularCoefficient = 0.1, - r_shape = {0, 0, axisLength / 2}, - lengthDirection = {0, 0, 1}, - widthDirection = {1, 0, 0}, - r = zeros(3), - R = Frames.from_T([1, 0, 0; - 0, 1, 0; - 0, 0, 1], zeros(3))); -equation - r = {x, y, 0}; - // Negate phi here since shapes use inverse frame-mapping - Rz = [cos(-phi), -sin(-phi), 0; - sin(-phi), cos(-phi), 0; - 0, 0, 1]; - - annotation( - uses(Modelica(version = "4.1.0")), - Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")})); -end vis2d; \ No newline at end of file