joppe/BondGraph
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

QDcontroller

Browse Source
This commit is contained in:
Joppe Blondel
2026-03-10 16:38:55 +01:00
parent 67d40f2dcd
commit a23e051024
1 changed files with 550 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
test2d.mo
View File

@@ -261,16 +261,16 @@ end vis2d;
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;
0, 0.5*pi; 1,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)));
Placement(transformation(origin = {-50, 62}, extent = {{10, -10}, {-10, 10}}, rotation = -180)));
controller2 controller(g_P = 400, g_I = 15, g_D = 20, sampleTime(displayUnit = "ms") = 0.01, limit = 20, quantizerDelta = 0.01, dacDelta = 0.01, quantizerScale = 1, dacScale = 1) annotation(
Placement(transformation(origin = {-8, 80}, extent = {{-10, 10}, {10, -10}}, rotation = -0)));
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));
@@ -296,15 +296,15 @@ end vis2d;
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}));
connect(controller.motor, T_motor.e0) annotation(
Line(points = {{2, 80}, {20, 80}, {20, 72}}, color = {0, 0, 127}));
connect(timeTable.y, controller.setpoint) annotation(
Line(points = {{-39, 62}, {-24, 62}, {-24, 73}, {-18, 73}}, color = {0, 0, 127}));
connect(pendulum1.phi, controller.angle) annotation(
Line(points = {{80, 8}, {86, 8}, {86, 104}, {-32, 104}, {-32, 80}, {-18, 80}}, color = {0, 0, 127}));
annotation(
experiment(StartTime = 0, StopTime = 25, Tolerance = 1e-06, Interval = 0.02),
Diagram(coordinateSystem(extent = {{-80, 140}, {100, -80}})));
Diagram(coordinateSystem(extent = {{-80, 100}, {100, -80}})));
end test;
model controller
@@ -314,9 +314,13 @@ end test;
parameter Real g_P = 1;
parameter Real g_I = 1;
parameter Real g_D = 1;
parameter Real g_G = 1;
parameter Modelica.Units.SI.Time sampleTime = 0.0025;
parameter Real limit = 100;
parameter Modelica.Units.SI.Mass mass = 1;
parameter Modelica.Units.SI.Length lCOM = 0.5;
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(
@@ -331,7 +335,7 @@ end test;
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(
Modelica.Blocks.Math.MultiSum multiSum(nu = 4) 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}})));
@@ -349,6 +353,10 @@ end test;
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)));
Modelica.Blocks.Math.Sin sin annotation(
Placement(transformation(origin = {-36, 110}, extent = {{-10, -10}, {10, 10}})));
Modelica.Blocks.Math.Gain gain12(k = -9.81*mass*lCOM*g_G) annotation(
Placement(transformation(origin = {10, 110}, extent = {{-10, -10}, {10, 10}})));
equation
connect(angle, sampler.u) annotation(
Line(points = {{-158, -6}, {-138, -6}}, color = {0, 0, 127}));
@@ -388,11 +396,173 @@ end test;
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}));
connect(sampler.y, sin.u) annotation(
Line(points = {{-114, -6}, {-104, -6}, {-104, 110}, {-48, 110}}, color = {0, 0, 127}));
connect(sin.y, gain12.u) annotation(
Line(points = {{-24, 110}, {-2, 110}}, color = {0, 0, 127}));
connect(gain12.y, multiSum.u[4]) annotation(
Line(points = {{22, 110}, {94, 110}, {94, 0}, {112, 0}}, 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}})));
Diagram(coordinateSystem(extent = {{-180, 120}, {240, -80}})));
end controller;
model Dcontroller
parameter Integer g_P = 1;
parameter Integer g_I = 1;
parameter Integer g_D = 1;
parameter Integer limit = 100;
parameter Modelica.Units.SI.Time sampleTime = 0.0025;
Modelica.Blocks.Interfaces.IntegerOutput motor annotation(
Placement(transformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}})));
Modelica.Blocks.Interfaces.IntegerInput setpoint annotation(
Placement(transformation(origin = {-100, 20}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-92, 60}, extent = {{-20, -20}, {20, 20}})));
Modelica.Blocks.Interfaces.IntegerInput angle annotation(
Placement(transformation(origin = {-100, -20}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-90, -60}, extent = {{-20, -20}, {20, 20}})));
Integer error;
Integer pOutput;
Integer iOutput;
Integer dOutput;
Integer drive;
Integer iInternal;
Integer motorInt;
protected
parameter Integer D = integer(g_D/sampleTime);
parameter Integer I = integer(g_I*sampleTime);
equation
when sample(0, sampleTime) then
error = setpoint - angle;
pOutput = error * g_P;
iInternal = (error * I) + pre(iOutput);
iOutput = iInternal;
dOutput = (error - pre(error)) * D;
drive = pOutput + iOutput + dOutput;
motorInt = min(limit, max(-1*limit, drive));
end when;
motor = motorInt;
annotation(
Icon(graphics = {Rectangle(lineColor = {189, 189, 189}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, lineThickness = 1.5, extent = {{-100, 100}, {100, -100}}, radius = 5)}));
end Dcontroller;
model Quantizer
parameter Real Delta = 1;
parameter Real Scale = 1;
parameter Modelica.Units.SI.Time sampleTime = 0.01;
Modelica.Blocks.Interfaces.RealInput u annotation(
Placement(transformation(origin = {-122, 2}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-90, 0}, extent = {{-20, -20}, {20, 20}})));
Modelica.Blocks.Interfaces.IntegerOutput y annotation(
Placement(transformation(origin = {86, -34}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}})));
protected
discrete Integer yInt(start=0, fixed=true);
equation
when sample(0, sampleTime) then
yInt = integer(floor((Scale*u)/Delta + 0.5));
end when;
y = yInt;
annotation(
Icon(graphics = {Rectangle(lineColor = {202, 202, 202}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, lineThickness = 1.25, extent = {{-100, 100}, {100, -100}}, radius = 25), Line(origin = {5, 10}, points = {{-61, -68}, {-35, -68}, {-35, -22}, {-21, -22}, {-21, 38}, {7, 38}, {7, 68}, {41, 68}, {41, 24}, {61, 24}, {61, 24}}, color = {170, 0, 0}, thickness = 1.25), Text(origin = {49, -50}, extent = {{-51, 50}, {51, -50}}, textString = "Q")}));
end Quantizer;
model controller2
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 Real g_G = 1;
parameter Modelica.Units.SI.Time sampleTime = 0.0025;
parameter Real limit = 100;
parameter Real quantizerDelta = 0.1;
parameter Real dacDelta = 0.1;
parameter Real quantizerScale = 1;
parameter Real dacScale = 1;
parameter Real quantizerQ = quantizerScale/quantizerDelta;
parameter Real dacQ = dacScale/dacDelta;
parameter Modelica.Units.SI.Mass mass = 1;
parameter Modelica.Units.SI.Length lCOM = 0.5;
RealInput setpoint(quantity="Angle") "Setpoint" annotation(
Placement(transformation(origin = {-124, 16}, 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 = {-124, -10}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-96, 0}, extent = {{-10, -10}, {10, 10}})));
RealOutput motor(quantity="Torque") "Motor drive" annotation(
Placement(transformation(origin = {124, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {104, 0}, extent = {{-10, -10}, {10, 10}})));
Modelica.Blocks.Discrete.Sampler sampler1(samplePeriod = sampleTime) annotation(
Placement(transformation(origin = {-90, -10}, extent = {{-10, -10}, {10, 10}})));
Modelica.Blocks.Discrete.Sampler sampler2(samplePeriod = sampleTime) annotation(
Placement(transformation(origin = {-90, 16}, extent = {{-10, -10}, {10, 10}})));
Modelica.Blocks.Discrete.ZeroOrderHold zeroOrderHold(samplePeriod = sampleTime) annotation(
Placement(transformation(origin = {90, 0}, extent = {{-10, -10}, {10, 10}})));
Dcontroller dcontroller(
g_P = integer(g_P*quantizerQ/dacQ),
g_I = integer(g_I*quantizerQ/dacQ),
g_D = integer(g_D*quantizerQ/dacQ),
limit = integer(limit*dacQ), sampleTime = sampleTime) annotation(
Placement(transformation(extent = {{-10, -10}, {10, 10}})));
Quantizer quantizer1(sampleTime = sampleTime, Delta = quantizerDelta, Scale = quantizerScale) annotation(
Placement(transformation(origin = {-50, 16}, extent = {{-10, -10}, {10, 10}})));
Quantizer quantizer2(sampleTime = sampleTime, Delta = quantizerDelta, Scale = quantizerScale) annotation(
Placement(transformation(origin = {-50, -10}, extent = {{-10, -10}, {10, 10}})));
DAC dac(sampleTime = sampleTime, Delta = dacDelta, Scale = dacScale) annotation(
Placement(transformation(origin = {50, 0}, extent = {{-10, -10}, {10, 10}})));
equation
connect(zeroOrderHold.y, motor) annotation(
Line(points = {{101, 0}, {123, 0}}, color = {0, 0, 127}));
connect(setpoint, sampler2.u) annotation(
Line(points = {{-124, 16}, {-102, 16}}, color = {0, 0, 127}));
connect(sampler2.y, quantizer1.u) annotation(
Line(points = {{-79, 16}, {-58, 16}}, color = {0, 0, 127}));
connect(quantizer1.y, dcontroller.setpoint) annotation(
Line(points = {{-40, 16}, {-20, 16}, {-20, 6}, {-10, 6}}, color = {255, 127, 0}));
connect(angle, sampler1.u) annotation(
Line(points = {{-124, -10}, {-102, -10}}, color = {0, 0, 127}));
connect(sampler1.y, quantizer2.u) annotation(
Line(points = {{-79, -10}, {-58, -10}}, color = {0, 0, 127}));
connect(quantizer2.y, dcontroller.angle) annotation(
Line(points = {{-40, -10}, {-20, -10}, {-20, -6}, {-8, -6}}, color = {255, 127, 0}));
connect(dcontroller.motor, dac.u) annotation(
Line(points = {{10, 0}, {41, 0}}, color = {255, 127, 0}));
connect(dac.y, zeroOrderHold.u) annotation(
Line(points = {{60, 0}, {78, 0}}, 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), Text(extent = {{-80, 80}, {80, -80}}, textString = "QDc")}),
experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002),
Diagram(coordinateSystem(extent = {{-140, 40}, {140, -20}})));
end controller2;
model DAC
parameter Real Delta = 1;
parameter Real Scale = 1;
parameter Modelica.Units.SI.Time sampleTime = 0.01;
Modelica.Blocks.Interfaces.IntegerInput u annotation(
Placement(transformation(origin = {-136, -2}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-90, 0}, extent = {{-20, -20}, {20, 20}})));
Modelica.Blocks.Interfaces.RealOutput y annotation(
Placement(transformation(origin = {126, 10}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}})));
equation
// Convert integer DAC code back to physical Real value.
y = (Delta / Scale) * u;
annotation(
Diagram(graphics),
Icon(graphics = {Rectangle(lineColor = {202, 202, 202}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, lineThickness = 1.25, extent = {{-100, 100}, {100, -100}}, radius = 25), Line(origin = {5, 10}, points = {{-61, -68}, {-35, -68}, {-35, -22}, {-21, -22}, {-21, 38}, {7, 38}, {7, 68}, {41, 68}, {41, 24}, {61, 24}, {61, 24}}, color = {170, 0, 0}, thickness = 1.25, smooth = Smooth.Bezier), Text(origin = {49, -50}, extent = {{-51, 50}, {51, -50}}, textString = "DA")}));
end DAC;
annotation(
uses(Modelica(version = "4.1.0")));
end test2d;