From 9e124a5e92ae9a781ed6f50b12d04c58cb6b3bab Mon Sep 17 00:00:00 2001 From: Joppe Blondel Date: Wed, 11 Mar 2026 16:37:50 +0100 Subject: [PATCH 1/2] Working and exportable PID controller --- EmbeddedControl.mo | 277 +++++++++++++++++++++++++------ exporttest/export_controller.log | 11 +- exporttest/export_controller.mos | 11 +- test.mo | 129 +++++++++++--- 4 files changed, 350 insertions(+), 78 deletions(-) diff --git a/EmbeddedControl.mo b/EmbeddedControl.mo index 785c0ab..8a96df7 100644 --- a/EmbeddedControl.mo +++ b/EmbeddedControl.mo @@ -10,27 +10,31 @@ package EmbeddedControl Placement(transformation(origin = {-122, 2}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); Modelica.Blocks.Interfaces.IntegerOutput y annotation( Placement(transformation(origin = {86, -34}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.BooleanInput tickInput annotation( + Placement(transformation(origin = {-130, -88}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 90))); protected discrete Integer yInt(start=0, fixed=true); - + algorithm - yInt := integer(floor((scale*u)/stepSize+ 0.5)); + when tickInput then + yInt := integer(floor((scale*u)/stepSize+ 0.5)); + end when; equation y = yInt; annotation( Documentation(info = " -

Integer analog-to-digital conversion block.

-

The real-valued input u is quantized to an integer code using:

-

y = floor((scale*u)/stepSize + 0.5)

-

This produces a rounded integer representation of the scaled input, intended for embedded-style control chains that operate on integer counts.

-

Parameters:

- -"), +

Integer analog-to-digital conversion block.

+

The real-valued input u is quantized to an integer code using:

+

y = floor((scale*u)/stepSize + 0.5)

+

This produces a rounded integer representation of the scaled input, intended for embedded-style control chains that operate on integer counts.

+

Parameters:

+ + "), Icon(graphics = {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 = {255, 85, 0}, thickness = 1.25), Text(origin = {49, -50}, extent = {{-51, 50}, {51, -50}}, textString = "AD")})); end ADC; @@ -42,26 +46,51 @@ package EmbeddedControl Placement(transformation(origin = {-136, -2}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); Modelica.Blocks.Interfaces.RealOutput y annotation( Placement(transformation(origin = {126, 10}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.BooleanInput tickInput annotation( + Placement(transformation(origin = {-130, -88}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 90))); + + protected + discrete Real yReal(start = 0, fixed = true); + + algorithm + when tickInput then + yReal := (stepSize * scale) * u; + end when; equation - // Convert integer DAC code back to physical Real value. - y = (stepSize * scale) * u; + y = yReal; annotation( Documentation(info = " -

Integer digital-to-analog conversion block.

-

The integer input u is mapped back to a real-valued physical signal using:

-

y = (stepSize*scale)*u

-

When used with matching parameter values, this block acts as the inverse conversion of the ADC block.

-

Parameters:

- -"), +

Integer digital-to-analog conversion block.

+

The integer input u is mapped back to a real-valued physical signal using:

+

y = (stepSize*scale)*u

+

When used with matching parameter values, this block acts as the inverse conversion of the ADC block.

+

Parameters:

+ + "), Diagram(graphics), Icon(graphics = {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 = {0, 0, 127}, thickness = 1.25, smooth = Smooth.Bezier), Text(origin = {49, -50}, extent = {{-51, 50}, {51, -50}}, textString = "DA")})); end DAC; extends Modelica.Icons.Package; + + model tickGenerator + extends Modelica.Blocks.Interfaces.partialBooleanSource; + Modelica.Blocks.Interfaces.IntegerOutput tickPeriodUs annotation( + Placement(transformation(origin = {134, -58}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, -80}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Sources.SampleTrigger sampleTrigger(period = period) annotation( + Placement(transformation(extent = {{-10, -10}, {10, 10}}))); + parameter Modelica.Units.SI.Time period = 0.1; + equation + tickPeriodUs = integer(period*1000*1000); + connect(sampleTrigger.y, y) annotation( + Line(points = {{12, 0}, {110, 0}}, color = {255, 0, 255})); + annotation( + Diagram(graphics), + Icon(graphics = {Line(points = {{-60, -70}, {-60, 70}}), Line(points = {{-20, -70}, {-20, 70}}), Line(points = {{20, -70}, {20, 70}}), Line(points = {{60, -70}, {60, 70}})})); +end tickGenerator; annotation( Icon(graphics = {Line(origin = {-48.92, -4.33}, points = {{-35.0829, -47.6744}, {-17.0829, 18.3256}, {-3.08288, -9.6744}, {10.9171, 48.3256}, {34.9171, 14.3256}, {34.9171, 14.3256}}, color = {0, 0, 127}, thickness = 1.5, smooth = Smooth.Bezier), Line( points = {{0, 81}, {0, -81}}, pattern = LinePattern.Dash, thickness = 2.75), Line(origin = {54, -10}, points = {{-36, -42}, {-36, -20}, {-30, -20}, {-30, -10}, {-20, -10}, {-20, 10}, {-6, 10}, {-6, 4}, {6, 4}, {6, 42}, {28, 42}, {28, 18}, {36, 18}, {36, 6}, {36, 6}}, color = {255, 85, 0}, thickness = 1.5)})); end Boundary; @@ -70,51 +99,203 @@ package EmbeddedControl extends Modelica.Icons.Package; model DDifference - extends EmbeddedControl.icons.discreteIntegerSISO; + extends EmbeddedControl.Bases.discreteIntegerSISO; discrete Integer x(start=0, fixed=true) "Initial or guess value of state"; + protected discrete Integer y_internal(start=0, fixed=true); + algorithm - x := u; - if x <> pre(x) then - y_internal := x - pre(x); - else - y_internal := pre(y_internal); + if tickUpdate then + y_internal := u - pre(x); + x := u; end if; + equation y = y_internal; annotation( - Documentation(info = " -

Discrete integer difference block.

-

Whenever the integer input changes, the block outputs the increment between the current and previous sample:

-

y = u[k] - u[k-1]

-

This is a raw discrete difference, not a time-scaled derivative. If a discrete-time derivative is required, include the factor 1/Ts in a downstream gain or in the controller gain.

-

The output updates only when u changes.

- "), Diagram(graphics), - Icon(graphics = {Text(textColor = {156, 156, 156}, extent = {{-30, 14}, {86, 60}}, textString = "DT1"), Line(points = {{-90, -80}, {82, -80}}, color = {156, 156, 156}), Line(points = {{-80, 78}, {-80, -90}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{-80, 90}, {-88, 68}, {-72, 68}, {-80, 90}}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{90, -80}, {68, -72}, {68, -88}, {90, -80}}), Line(origin = {-9, -4}, points = {{-63, 66}, {-63, 40}, {-53, 40}, {-53, -20}, {-39, -20}, {-39, -54}, {3, -54}, {3, -66}, {63, -66}, {63, -66}}, color = {255, 85, 0}, thickness = 0.5)})); + Icon(graphics = {Text(textColor = {156, 156, 156}, extent = {{-30, 14}, {86, 60}}, textString = "D"), Line(points = {{-90, -80}, {82, -80}}, color = {156, 156, 156}), Line(points = {{-80, 78}, {-80, -90}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{-80, 90}, {-88, 68}, {-72, 68}, {-80, 90}}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{90, -80}, {68, -72}, {68, -88}, {90, -80}}), Line(origin = {-9, -4}, points = {{-63, 66}, {-63, 40}, {-53, 40}, {-53, -20}, {-39, -20}, {-39, -54}, {3, -54}, {3, -66}, {63, -66}, {63, -66}}, color = {255, 85, 0}, thickness = 0.5)})); end DDifference; + + model DAccumulate + extends EmbeddedControl.Bases.discreteIntegerSISO; + discrete Integer y_internal(start=0, fixed=true); + + algorithm + if tickUpdate then + y_internal := u + pre(y_internal); + end if; + + equation + y = y_internal; + + annotation( + Diagram(graphics), + Icon(graphics = {Line(points = {{-90, -80}, {82, -80}}, color = {156, 156, 156}), Line(points = {{-80, 78}, {-80, -90}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{-80, 90}, {-88, 68}, {-72, 68}, {-80, 90}}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{90, -80}, {68, -72}, {68, -88}, {90, -80}}), Text(textColor = {156, 156, 156}, extent = {{0, -70}, {60, -10}}, textString = "A"), Line(origin = {-2, -1}, points = {{-70, -73}, {-70, -53}, {-48, -53}, {-48, -33}, {-26, -33}, {-26, -9}, {-2, -9}, {-2, 11}, {22, 11}, {22, 33}, {46, 33}, {46, 57}, {70, 57}, {70, 73}}, color = {255, 85, 0}, thickness = 0.5)})); +end DAccumulate; + + model UnitDelay + extends EmbeddedControl.Bases.discreteIntegerSISO; + discrete Integer x(start=0, fixed=true) "Initial or guess value of state"; + protected + discrete Integer y_internal(start=0, fixed=true); +algorithm + if tickUpdate then + y_internal := pre(x); + x := u; + end if; + equation + y = y_internal; + annotation( + Icon(graphics = {Text(textColor = {255, 85, 0},extent = {{-100, 100}, {100, -100}}, textString = "z"), Text(origin = {58, 50}, textColor = {255, 85, 0}, extent = {{-24, 22}, {24, -22}}, textString = "-1")})); +end UnitDelay; + + model Gain + extends EmbeddedControl.Bases.discreteIntegerSISO; + parameter Integer k = 1; + protected + discrete Integer y_internal(start=0, fixed=true); + algorithm + y_internal := k * u; + equation + y = y_internal; + + annotation( + Icon(graphics = {Text(textColor = {255, 85, 0}, extent = {{-100, 100}, {100, -100}}, textString = "k"), Text(extent = {{-150, -140}, {150, -100}}, textString = "k=%k")}), + Diagram(graphics)); + end Gain; + + model FractionalGain + extends Bases.discreteIntegerSISO; + parameter Integer k = 1; + parameter Integer f = 1; + protected + discrete Integer y_internal(start=0, fixed=true); + algorithm + y_internal := div((k*u),f); + equation + y = y_internal; + annotation( + Icon(graphics = {Text(textColor = {255, 85, 0}, extent = {{-100, 100}, {100, -100}}, textString = "k"), Text(extent = {{-150, -140}, {150, -100}}, textString = "k=%k f=%f")}), + Diagram(graphics)); + end FractionalGain; + + model DDifferentiate + extends EmbeddedControl.Bases.discreteIntegerSISO; + + discrete Integer x(start=0, fixed=true) "Initial or guess value of state"; + parameter Integer scaleNumerator = 1000000; + outer Integer tickPeriodUs; + protected + discrete Integer y_internal(start=0, fixed=true); + + algorithm + if tickUpdate then + y_internal := integer((u - pre(x))*scaleNumerator / tickPeriodUs); + x := u; + end if; + + equation + y = y_internal; + + annotation( + Diagram(graphics), + Icon(graphics = {Text(textColor = {156, 156, 156}, extent = {{-30, 14}, {86, 60}}, textString = "DT1"), Line(points = {{-90, -80}, {82, -80}}, color = {156, 156, 156}), Line(points = {{-80, 78}, {-80, -90}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{-80, 90}, {-88, 68}, {-72, 68}, {-80, 90}}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{90, -80}, {68, -72}, {68, -88}, {90, -80}}), Line(origin = {-9, -4}, points = {{-63, 66}, {-63, 40}, {-53, 40}, {-53, -20}, {-39, -20}, {-39, -54}, {3, -54}, {3, -66}, {63, -66}, {63, -66}}, color = {255, 85, 0}, thickness = 0.5)}), + experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002)); + end DDifferentiate; + + model DIntegrate + extends Bases.discreteIntegerSISO; + discrete Integer y_internal(start = 0, fixed = true); + parameter Integer scaleDenominator = 1000000; + outer Integer tickPeriodUs; + algorithm + if tickUpdate then + y_internal := u + pre(y_internal); + end if; + equation + y = integer(y_internal*tickPeriodUs / scaleDenominator); + annotation( + Diagram(graphics), + Icon(graphics = {Line(points = {{-90, -80}, {82, -80}}, color = {156, 156, 156}), Line(points = {{-80, 78}, {-80, -90}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{-80, 90}, {-88, 68}, {-72, 68}, {-80, 90}}), Polygon(lineColor = {156, 156, 156}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, points = {{90, -80}, {68, -72}, {68, -88}, {90, -80}}), Text(textColor = {156, 156, 156}, extent = {{0, -70}, {60, -10}}, textString = "I"), Line(origin = {-2, -1}, points = {{-70, -73}, {-70, -53}, {-48, -53}, {-48, -33}, {-26, -33}, {-26, -9}, {-2, -9}, {-2, 11}, {22, 11}, {22, 33}, {46, 33}, {46, 57}, {70, 57}, {70, 73}}, color = {255, 85, 0}, thickness = 0.5)})); + end DIntegrate; + + model ScaledGain + extends Bases.discreteIntegerSISO; + parameter Integer k = 1; + parameter Integer scaleNumerator = 1000000; + parameter Integer scaleDenominator = 1000000; + outer Integer tickPeriodUs; + parameter Boolean scaleMode = false; + protected + discrete Integer y_internal(start=0, fixed=true); + algorithm + if scaleMode then + y_internal := integer(u * k * tickPeriodUs / scaleDenominator); + else + y_internal := integer(u * k * scaleNumerator / tickPeriodUs); + end if; + equation + y = y_internal; + + annotation( + Icon(graphics = {Text(textColor = {255, 85, 0}, extent = {{-100, 100}, {100, -100}}, textString = "k"), Text(extent = {{-150, -140}, {150, -100}}, textString = "k=%k")}), + Diagram(graphics)); + end ScaledGain; annotation( Icon(graphics = {Line(origin = {-0.95, 0.62}, points = {{-75.0496, -68.6246}, {-61.0496, 1.37545}, {-29.0496, 69.3754}, {-5.04956, 1.37545}, {20.9504, -64.6246}, {50.9504, -2.62455}, {74.9504, 65.3754}, {74.9504, 65.3754}}, color = {255, 85, 0}, thickness = 2, smooth = Smooth.Bezier)})); end Math; - model icons - extends Modelica.Icons.IconsPackage; + package Bases + extends Modelica.Icons.BasesPackage; - model discreteIntegerSISO - extends Modelica.Blocks.Icons.DiscreteBlock; - Modelica.Blocks.Interfaces.IntegerInput u annotation( - Placement(transformation(origin = {-194, 2}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); - Modelica.Blocks.Interfaces.IntegerOutput y annotation( - Placement(transformation(origin = {128, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + partial model ControlBase + extends Modelica.Blocks.Icons.DiscreteBlock; + Modelica.Blocks.Interfaces.BooleanInput tickInput annotation( + Placement(iconTransformation(origin = {0, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 90))); + Modelica.Blocks.Interfaces.IntegerInput tickPeriodIn annotation( + Placement(iconTransformation(origin = {-80, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 90))); + protected + inner Boolean tickUpdate; + inner Integer tickPeriodUs; + inner Integer tickPeriodMs; + inner Integer tickPeriodS; + algorithm + if tickInput then + if pre(tickInput) then + tickUpdate := false; + else + tickUpdate := true; + end if; + else + tickUpdate := false; + end if; equation - + tickPeriodUs = tickPeriodIn; + tickPeriodMs = tickPeriodUs/1000; + tickPeriodS = tickPeriodMs/1000; + end ControlBase; + + partial model discreteInteger + extends Modelica.Blocks.Icons.DiscreteBlock; + outer Boolean tickUpdate; + end discreteInteger; + + partial model discreteIntegerSISO + extends EmbeddedControl.Bases.discreteInteger; + Modelica.Blocks.Interfaces.IntegerInput u annotation( + Placement(transformation(origin = {-100, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); + Modelica.Blocks.Interfaces.IntegerOutput y annotation( + Placement(transformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + equation + end discreteIntegerSISO; equation - end icons; + end Bases; annotation( Icon(graphics = {Text(origin = {1, 1}, textColor = {255, 170, 0}, extent = {{-99, 99}, {99, -99}}, textString = "EC", textStyle = {TextStyle.Bold})}), uses(Modelica(version = "4.1.0"))); diff --git a/exporttest/export_controller.log b/exporttest/export_controller.log index d7873b9..74b080d 100644 --- a/exporttest/export_controller.log +++ b/exporttest/export_controller.log @@ -1,14 +1,17 @@ true true -"Check of EmbeddedControl.Math.DDifference completed successfully. -Class EmbeddedControl.Math.DDifference has 4 equation(s) and 4 variable(s). -1 of these are trivial equation(s)." +true +true +true +"Check of test.Controller completed successfully. +Class test.Controller has 32 equation(s) and 32 variable(s). +16 of these are trivial equation(s)." Notification: Automatically loaded package Modelica 4.1.0 due to uses annotation from EmbeddedControl. Notification: Automatically loaded package Complex 4.1.0 due to uses annotation from Modelica. Notification: Automatically loaded package ModelicaServices 4.1.0 due to uses annotation from Modelica. true -translateModel(test2d.Dcontroller) = true +translateModel() = true diff --git a/exporttest/export_controller.mos b/exporttest/export_controller.mos index 7bbc471..c0fe966 100644 --- a/exporttest/export_controller.mos +++ b/exporttest/export_controller.mos @@ -1,11 +1,14 @@ setCommandLineOptions("--simCodeTarget=ExperimentalEmbeddedC"); +setCommandLineOptions("--target=gcc"); +setCommandLineOptions("-d=initialization"); loadFile("/work/EmbeddedControl.mo"); +loadFile("/work/test.mo"); -okCheck := checkModel(EmbeddedControl.Math.DDifference); -print("checkModel(EmbeddedControl.Math.DDifference) = " + String(okCheck) + "\n"); +okCheck := checkModel(test.Controller); +print("checkModel() = " + String(okCheck) + "\n"); print(getErrorString()); -okTranslate := translateModel(EmbeddedControl.Math.DDifference, fileNamePrefix="difference"); -print("translateModel(test2d.Dcontroller) = " + String(okTranslate) + "\n"); +okTranslate := translateModel(test.Controller, fileNamePrefix="controller"); +print("translateModel() = " + String(okTranslate) + "\n"); print(getErrorString()); diff --git a/test.mo b/test.mo index 74b0f80..276a3d5 100644 --- a/test.mo +++ b/test.mo @@ -1,27 +1,112 @@ model test - Modelica.Blocks.Sources.Sine sine(amplitude = 1) annotation( - Placement(transformation(origin = {-100, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.DDifference dDifferentiate(x(start = 0)) annotation( - Placement(transformation(origin = {20, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Boundary.DAC dac(stepSize = 0.02) annotation( + EmbeddedControl.Boundary.DAC dac(stepSize = 0.001, scale = -1) annotation( + Placement(transformation(origin = {106, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Boundary.ADC adc(stepSize = 0.001) annotation( + Placement(transformation(origin = {10, 0}, extent = {{-10, -10}, {10, 10}}))); + model Controller + extends EmbeddedControl.Bases.ControlBase; + Modelica.Blocks.Interfaces.IntegerInput u annotation( + Placement(transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); + Modelica.Blocks.Interfaces.IntegerOutput y annotation( + Placement(transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.Gain P(k = 350) annotation( + Placement(transformation(origin = {20, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.IntegerInput setpoint annotation( + Placement(transformation(origin = {-120, 48}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}))); + Modelica.Blocks.MathInteger.Sum sum(nu = 2, k = {1, -1}) annotation( + Placement(transformation(origin = {-70, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.DDifferentiate dDifferentiate(scaleNumerator = 1000000) annotation( + Placement(transformation(origin = {-20, 40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.MathInteger.Sum sum1(nu = 3) annotation( + Placement(transformation(origin = {70, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.Gain D(k = 25) annotation( + Placement(transformation(origin = {20, 40}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.DIntegrate dIntegrate(scaleDenominator = 1000000) annotation( + Placement(transformation(origin = {-18, -40}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.Gain I(k = 150) annotation( + Placement(transformation(origin = {20, -40}, extent = {{-10, -10}, {10, 10}}))); + equation + connect(u, sum.u[1]) annotation( + Line(points = {{-120, 0}, {-80, 0}}, color = {255, 127, 0})); + connect(setpoint, sum.u[2]) annotation( + Line(points = {{-120, 48}, {-96, 48}, {-96, 0}, {-80, 0}}, color = {255, 127, 0})); + connect(sum.y, dDifferentiate.u) annotation( + Line(points = {{-58, 0}, {-40, 0}, {-40, 40}, {-32, 40}}, color = {255, 127, 0})); + connect(P.y, sum1.u[1]) annotation( + Line(points = {{31, 0}, {60, 0}}, color = {255, 127, 0})); + connect(sum1.y, y) annotation( + Line(points = {{82, 0}, {110, 0}}, color = {255, 127, 0})); + connect(sum.y, P.u) annotation( + Line(points = {{-58, 0}, {8, 0}}, color = {255, 127, 0})); + connect(dDifferentiate.y, D.u) annotation( + Line(points = {{-8, 40}, {8, 40}}, color = {255, 127, 0})); + connect(D.y, sum1.u[2]) annotation( + Line(points = {{31, 40}, {40, 40}, {40, 0}, {60, 0}}, color = {255, 127, 0})); + connect(sum.y, dIntegrate.u) annotation( + Line(points = {{-58, 0}, {-40, 0}, {-40, -40}, {-30, -40}}, color = {255, 127, 0})); + connect(dIntegrate.y, I.u) annotation( + Line(points = {{-6, -40}, {8, -40}}, color = {255, 127, 0})); + connect(I.y, sum1.u[3]) annotation( + Line(points = {{32, -40}, {40, -40}, {40, 0}, {60, 0}}, color = {255, 127, 0})); + annotation( + Diagram(coordinateSystem(extent = {{-140, 20}, {120, -20}})), + experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002)); +end Controller; + + Controller controller annotation( Placement(transformation(origin = {60, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Boundary.ADC adc(stepSize = 0.02) annotation( - Placement(transformation(origin = {-20, 0}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Discrete.Sampler sampler(samplePeriod(displayUnit = "ms") = 0.01) annotation( - Placement(transformation(origin = {-60, 0}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Discrete.ZeroOrderHold zeroOrderHold(samplePeriod(displayUnit = "ms") = 0.01) annotation( - Placement(transformation(origin = {100, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Boundary.tickGenerator tickGenerator(period(displayUnit = "ms") = 0.01) annotation( + Placement(transformation(origin = {-12, -40}, extent = {{-10, -10}, {10, 10}}))); + inner Modelica.Mechanics.MultiBody.World world annotation( + Placement(transformation(origin = {-122, -30}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Joints.Revolute revolute(useAxisFlange = true, phi(start = 0, fixed = true), w(start = 0, fixed = true)) annotation( + Placement(transformation(origin = {-92, -30}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Sensors.AngleSensor angleSensor annotation( + Placement(transformation(origin = {-72, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Components.Damper damper(d = 0.1) annotation( + Placement(transformation(origin = {-94, 26}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Parts.Body body(r_CM = {0.5, 0, 0}, m = 1) annotation( + Placement(transformation(origin = {-62, -30}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Sources.Torque torque(useSupport = true) annotation( + Placement(transformation(origin = {-94, 52}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Sources.IntegerConstant integerConstant(k = 0) annotation( + Placement(transformation(origin = {10, 30}, extent = {{-10, -10}, {10, 10}}))); equation - connect(adc.y, dDifferentiate.u) annotation( - Line(points = {{-9, 0}, {7, 0}}, color = {255, 127, 0})); - connect(sine.y, sampler.u) annotation( - Line(points = {{-89, 0}, {-73, 0}}, color = {0, 0, 127})); - connect(sampler.y, adc.u) annotation( - Line(points = {{-49, 0}, {-33, 0}}, color = {0, 0, 127})); - connect(dDifferentiate.y, dac.u) annotation( - Line(points = {{31, 0}, {47, 0}}, color = {255, 127, 0})); - connect(dac.y, zeroOrderHold.u) annotation( - Line(points = {{71, 0}, {87, 0}}, color = {0, 0, 127})); + connect(adc.y, controller.u) annotation( + Line(points = {{21, 0}, {48, 0}}, color = {255, 127, 0})); + connect(tickGenerator.y, adc.tickInput) annotation( + Line(points = {{-1, -40}, {10, -40}, {10, -12}}, color = {255, 0, 255})); + connect(tickGenerator.y, controller.tickInput) annotation( + Line(points = {{-1, -40}, {60, -40}, {60, -12}}, color = {255, 0, 255})); + connect(tickGenerator.y, dac.tickInput) annotation( + Line(points = {{-1, -40}, {106, -40}, {106, -12}}, color = {255, 0, 255})); + connect(tickGenerator.tickPeriodUs, controller.tickPeriodIn) annotation( + Line(points = {{-1, -48}, {52, -48}, {52, -12}}, color = {0, 0, 127})); + connect(world.frame_b, revolute.frame_a) annotation( + Line(points = {{-112, -30}, {-102, -30}}, color = {95, 95, 95})); + connect(angleSensor.phi, adc.u) annotation( + Line(points = {{-61, 0}, {-2, 0}}, color = {0, 0, 127})); + connect(revolute.axis, angleSensor.flange) annotation( + Line(points = {{-92, -20}, {-92, 0}, {-82, 0}})); + connect(damper.flange_a, revolute.support) annotation( + Line(points = {{-104, 26}, {-112, 26}, {-112, -10}, {-98, -10}, {-98, -20}})); + connect(revolute.axis, damper.flange_b) annotation( + Line(points = {{-92, -20}, {-92, 14}, {-78, 14}, {-78, 26}, {-84, 26}})); + connect(revolute.frame_b, body.frame_a) annotation( + Line(points = {{-82, -30}, {-72, -30}}, color = {95, 95, 95})); + connect(controller.y, dac.u) annotation( + Line(points = {{71, 0}, {94, 0}}, color = {255, 127, 0})); + connect(torque.support, damper.flange_a) annotation( + Line(points = {{-94, 42}, {-94, 36}, {-104, 36}, {-104, 26}})); + connect(torque.flange, damper.flange_b) annotation( + Line(points = {{-84, 52}, {-84, 26}})); + connect(dac.y, torque.tau) annotation( + Line(points = {{118, 0}, {128, 0}, {128, 74}, {-118, 74}, {-118, 52}, {-106, 52}}, color = {0, 0, 127})); + connect(integerConstant.y, controller.setpoint) annotation( + Line(points = {{22, 30}, {32, 30}, {32, 8}, {48, 8}}, color = {255, 127, 0})); annotation( - uses(Modelica(version = "4.1.0"))); + uses(Modelica(version = "4.1.0")), + Diagram(coordinateSystem(extent = {{-140, 40}, {120, -60}})), + version = "", + experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02)); end test; \ No newline at end of file From cbe611916ec3c33371d8d9896905e3cfb8fe88b2 Mon Sep 17 00:00:00 2001 From: Joppe Blondel Date: Wed, 11 Mar 2026 19:08:49 +0100 Subject: [PATCH 2/2] Setup for position control --- EmbeddedControl.mo | 75 +++++++++++++++++- test.mo | 194 +++++++++++++++++++++++++-------------------- 2 files changed, 178 insertions(+), 91 deletions(-) diff --git a/EmbeddedControl.mo b/EmbeddedControl.mo index 8a96df7..d8624ae 100644 --- a/EmbeddedControl.mo +++ b/EmbeddedControl.mo @@ -12,16 +12,15 @@ package EmbeddedControl Placement(transformation(origin = {86, -34}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); Modelica.Blocks.Interfaces.BooleanInput tickInput annotation( Placement(transformation(origin = {-130, -88}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -120}, extent = {{-20, -20}, {20, 20}}, rotation = 90))); - protected - discrete Integer yInt(start=0, fixed=true); + discrete Integer y_internal(start=0, fixed=true); algorithm when tickInput then - yInt := integer(floor((scale*u)/stepSize+ 0.5)); + y_internal := integer(floor((scale*u)/stepSize+ 0.5)); end when; equation - y = yInt; + y = y_internal; annotation( Documentation(info = " @@ -245,6 +244,27 @@ end UnitDelay; Icon(graphics = {Text(textColor = {255, 85, 0}, extent = {{-100, 100}, {100, -100}}, textString = "k"), Text(extent = {{-150, -140}, {150, -100}}, textString = "k=%k")}), Diagram(graphics)); end ScaledGain; + + model Limiter + extends EmbeddedControl.Bases.discreteIntegerSISO; + parameter Integer limit = 100; + protected + discrete Integer y_internal(start=0, fixed=true); + algorithm + if u > limit then + y_internal := limit; + elseif u < -limit then + y_internal := -limit; + else + y_internal := u; + end if; + equation + y = y_internal; + + annotation( + Diagram(graphics), + Icon(graphics = {Line(points = {{0, -90}, {0, 68}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {192, 192, 192}, fillPattern = FillPattern.Solid, points = {{0, 90}, {-8, 68}, {8, 68}, {0, 90}}), Line(points = {{-90, 0}, {68, 0}}, color = {156, 156, 156}), Polygon(lineColor = {156, 156, 156}, fillColor = {192, 192, 192}, fillPattern = FillPattern.Solid, points = {{90, 0}, {68, -8}, {68, 8}, {90, 0}}), Line(origin = {-36, -36}, points = {{-46, -24}, {-8, -24}, {-8, -8}, {8, -8}, {8, 10}, {22, 10}, {22, 24}, {22, 24}}, color = {255, 85, 0}, thickness = 0.5), Line(origin = {34.5, 20}, points = {{-48.5, -32}, {-34.5, -32}, {-34.5, -20}, {-20.5, -20}, {-20.5, -4}, {-2.5, -4}, {-2.5, 16}, {11.5, 16}, {11.5, 32}, {49.5, 32}, {27.5, 32}}, color = {255, 85, 0}, thickness = 0.5)})); +end Limiter; annotation( Icon(graphics = {Line(origin = {-0.95, 0.62}, points = {{-75.0496, -68.6246}, {-61.0496, 1.37545}, {-29.0496, 69.3754}, {-5.04956, 1.37545}, {20.9504, -64.6246}, {50.9504, -2.62455}, {74.9504, 65.3754}, {74.9504, 65.3754}}, color = {255, 85, 0}, thickness = 2, smooth = Smooth.Bezier)})); end Math; @@ -296,6 +316,53 @@ end UnitDelay; equation end Bases; + + package Util + extends Modelica.Icons.UtilitiesPackage; + + model PID + extends EmbeddedControl.Bases.discreteIntegerSISO; + parameter Integer kP = 1; + parameter Integer kI = 1; + parameter Integer kD = 1; + parameter Integer scale = 1000000; + EmbeddedControl.Math.Gain P(k = kP) annotation( + Placement(transformation(extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.DDifferentiate dDifferentiate(scaleNumerator = scale) annotation( + Placement(transformation(origin = {-40, 40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.MathInteger.Sum sum1(nu = 3) annotation( + Placement(transformation(origin = {50, 0}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.Gain D(k = kD) annotation( + Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.DIntegrate dIntegrate(scaleDenominator = scale) annotation( + Placement(transformation(origin = {-38, -40}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Math.Gain I(k = kI) annotation( + Placement(transformation(origin = {0, -40}, extent = {{-10, -10}, {10, 10}}))); + protected outer Boolean tickUpdate; + equation + connect(P.y, sum1.u[1]) annotation( + Line(points = {{11, 0}, {40, 0}}, color = {255, 127, 0})); + connect(dDifferentiate.y, D.u) annotation( + Line(points = {{-29, 40}, {-12, 40}}, color = {255, 127, 0})); + connect(D.y, sum1.u[2]) annotation( + Line(points = {{11, 40}, {20, 40}, {20, 0}, {40, 0}}, color = {255, 127, 0})); + connect(dIntegrate.y, I.u) annotation( + Line(points = {{-27, -40}, {-12, -40}}, color = {255, 127, 0})); + connect(I.y, sum1.u[3]) annotation( + Line(points = {{11, -40}, {19, -40}, {19, 0}, {40, 0}}, color = {255, 127, 0})); + connect(u, dDifferentiate.u) annotation( + Line(points = {{-100, 0}, {-70, 0}, {-70, 40}, {-52, 40}}, color = {255, 127, 0})); + connect(u, P.u) annotation( + Line(points = {{-100, 0}, {-12, 0}}, color = {255, 127, 0})); + connect(u, dIntegrate.u) annotation( + Line(points = {{-100, 0}, {-70, 0}, {-70, -40}, {-50, -40}}, color = {255, 127, 0})); + connect(sum1.y, y) annotation( + Line(points = {{62, 0}, {100, 0}}, color = {255, 127, 0})); + annotation( + Icon(graphics = {Text(textColor = {255, 85, 0}, extent = {{-100, 100}, {100, -100}}, textString = "PID")}), + Diagram(coordinateSystem(extent = {{-120, 60}, {120, -60}}))); +end PID; + end Util; annotation( Icon(graphics = {Text(origin = {1, 1}, textColor = {255, 170, 0}, extent = {{-99, 99}, {99, -99}}, textString = "EC", textStyle = {TextStyle.Bold})}), uses(Modelica(version = "4.1.0"))); diff --git a/test.mo b/test.mo index 276a3d5..47a9a55 100644 --- a/test.mo +++ b/test.mo @@ -1,112 +1,132 @@ model test - EmbeddedControl.Boundary.DAC dac(stepSize = 0.001, scale = -1) annotation( + EmbeddedControl.Boundary.DAC dac(stepSize = 0.01, scale = -1) annotation( Placement(transformation(origin = {106, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Boundary.ADC adc(stepSize = 0.001) annotation( + EmbeddedControl.Boundary.ADC adc(stepSize = 0.01) annotation( Placement(transformation(origin = {10, 0}, extent = {{-10, -10}, {10, 10}}))); model Controller extends EmbeddedControl.Bases.ControlBase; - Modelica.Blocks.Interfaces.IntegerInput u annotation( - Placement(transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); + Modelica.Blocks.Interfaces.IntegerInput angle_crad annotation( + Placement(transformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-122, 120}, extent = {{-20, -20}, {20, 20}}))); Modelica.Blocks.Interfaces.IntegerOutput y annotation( - Placement(transformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.Gain P(k = 350) annotation( - Placement(transformation(origin = {20, 0}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Interfaces.IntegerInput setpoint annotation( - Placement(transformation(origin = {-120, 48}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}))); - Modelica.Blocks.MathInteger.Sum sum(nu = 2, k = {1, -1}) annotation( - Placement(transformation(origin = {-70, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.DDifferentiate dDifferentiate(scaleNumerator = 1000000) annotation( - Placement(transformation(origin = {-20, 40}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.MathInteger.Sum sum1(nu = 3) annotation( - Placement(transformation(origin = {70, 0}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.Gain D(k = 25) annotation( - Placement(transformation(origin = {20, 40}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.DIntegrate dIntegrate(scaleDenominator = 1000000) annotation( - Placement(transformation(origin = {-18, -40}, extent = {{-10, -10}, {10, 10}}))); - EmbeddedControl.Math.Gain I(k = 150) annotation( - Placement(transformation(origin = {20, -40}, extent = {{-10, -10}, {10, 10}}))); + Placement(transformation(origin = {162, 0}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {110, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.IntegerInput angle_setpoint annotation( + Placement(transformation(origin = {-120, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 180}, extent = {{-20, -20}, {20, 20}}))); + Modelica.Blocks.MathInteger.Sum sum(nu = 2, k = {-1, 1}) annotation( + Placement(transformation(origin = {64, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.IntegerInput position_mm annotation( + Placement(transformation(origin = {-120, 80}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, 0}, extent = {{-20, -20}, {20, 20}}))); + EmbeddedControl.Util.PID pida(kP = 200, kI = 50, kD = 10, scale = 100000) annotation( + Placement(transformation(origin = {102, 0}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Interfaces.IntegerInput position_setpoint annotation( + Placement(transformation(origin = {-120, 120}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-120, -62}, extent = {{-20, -20}, {20, 20}}))); equation - connect(u, sum.u[1]) annotation( - Line(points = {{-120, 0}, {-80, 0}}, color = {255, 127, 0})); - connect(setpoint, sum.u[2]) annotation( - Line(points = {{-120, 48}, {-96, 48}, {-96, 0}, {-80, 0}}, color = {255, 127, 0})); - connect(sum.y, dDifferentiate.u) annotation( - Line(points = {{-58, 0}, {-40, 0}, {-40, 40}, {-32, 40}}, color = {255, 127, 0})); - connect(P.y, sum1.u[1]) annotation( - Line(points = {{31, 0}, {60, 0}}, color = {255, 127, 0})); - connect(sum1.y, y) annotation( - Line(points = {{82, 0}, {110, 0}}, color = {255, 127, 0})); - connect(sum.y, P.u) annotation( - Line(points = {{-58, 0}, {8, 0}}, color = {255, 127, 0})); - connect(dDifferentiate.y, D.u) annotation( - Line(points = {{-8, 40}, {8, 40}}, color = {255, 127, 0})); - connect(D.y, sum1.u[2]) annotation( - Line(points = {{31, 40}, {40, 40}, {40, 0}, {60, 0}}, color = {255, 127, 0})); - connect(sum.y, dIntegrate.u) annotation( - Line(points = {{-58, 0}, {-40, 0}, {-40, -40}, {-30, -40}}, color = {255, 127, 0})); - connect(dIntegrate.y, I.u) annotation( - Line(points = {{-6, -40}, {8, -40}}, color = {255, 127, 0})); - connect(I.y, sum1.u[3]) annotation( - Line(points = {{32, -40}, {40, -40}, {40, 0}, {60, 0}}, color = {255, 127, 0})); + connect(angle_crad, sum.u[1]) annotation( + Line(points = {{-120, 0}, {54, 0}}, color = {255, 127, 0})); + connect(sum.y, pida.u) annotation( + Line(points = {{75.5, 0}, {90, 0}}, color = {255, 127, 0})); + connect(pida.y, y) annotation( + Line(points = {{113, 0}, {162, 0}}, color = {255, 127, 0})); + connect(angle_setpoint, sum.u[2]) annotation( + Line(points = {{-120, 40}, {-90, 40}, {-90, 0}, {54, 0}}, color = {255, 127, 0})); annotation( - Diagram(coordinateSystem(extent = {{-140, 20}, {120, -20}})), - experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002)); + Diagram(coordinateSystem(extent = {{-140, 140}, {180, -20}})), + experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002), + Icon(graphics = {Rectangle(origin = {-90, 50}, lineColor = {255, 85, 0}, fillColor = {156, 156, 156}, fillPattern = FillPattern.Solid, extent = {{-10, 150}, {10, -150}})})); end Controller; Controller controller annotation( Placement(transformation(origin = {60, 0}, extent = {{-10, -10}, {10, 10}}))); EmbeddedControl.Boundary.tickGenerator tickGenerator(period(displayUnit = "ms") = 0.01) annotation( - Placement(transformation(origin = {-12, -40}, extent = {{-10, -10}, {10, 10}}))); + Placement(transformation(origin = {-10, -40}, extent = {{-10, -10}, {10, 10}}))); + inner Modelica.Mechanics.MultiBody.World world annotation( - Placement(transformation(origin = {-122, -30}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Mechanics.MultiBody.Joints.Revolute revolute(useAxisFlange = true, phi(start = 0, fixed = true), w(start = 0, fixed = true)) annotation( - Placement(transformation(origin = {-92, -30}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Mechanics.Rotational.Sensors.AngleSensor angleSensor annotation( - Placement(transformation(origin = {-72, 0}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Mechanics.Rotational.Components.Damper damper(d = 0.1) annotation( - Placement(transformation(origin = {-94, 26}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Mechanics.MultiBody.Parts.Body body(r_CM = {0.5, 0, 0}, m = 1) annotation( - Placement(transformation(origin = {-62, -30}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Mechanics.Rotational.Sources.Torque torque(useSupport = true) annotation( - Placement(transformation(origin = {-94, 52}, extent = {{-10, -10}, {10, 10}}))); - Modelica.Blocks.Sources.IntegerConstant integerConstant(k = 0) annotation( - Placement(transformation(origin = {10, 30}, extent = {{-10, -10}, {10, 10}}))); + Placement(transformation(origin = {-190, -40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Joints.Prismatic prismatic1(animation = false, useAxisFlange = true) annotation( + Placement(transformation(origin = {-146, -40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Joints.Revolute revolute1(useAxisFlange = true, phi(start = 1.5690509975429023)) annotation( + Placement(transformation(origin = {-96, -40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.MultiBody.Parts.Body body1(m = 1, r_CM = {0.5, 0, 0}) annotation( + Placement(transformation(origin = {-50, -40}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Components.IdealRollingWheel idealRollingWheel1(radius = 0.1) annotation( + Placement(transformation(origin = {-138, 0}, extent = {{-10, -10}, {10, 10}}, rotation = -90))); + Modelica.Mechanics.Rotational.Sources.Torque torque(useSupport = true) annotation( + Placement(transformation(origin = {-102, 10}, extent = {{10, -10}, {-10, 10}}))); + Modelica.Mechanics.Rotational.Components.Damper damper1(d = 1) annotation( + Placement(transformation(origin = {-102, 34}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Mechanics.Rotational.Sensors.RelAngleSensor relAngleSensor annotation( + Placement(transformation(origin = {-36, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 90))); + Modelica.Mechanics.Translational.Sensors.PositionSensor positionSensor annotation( + Placement(transformation(origin = {-100, -74}, extent = {{-10, -10}, {10, 10}}))); + Modelica.Blocks.Sources.Constant const(k = -0.5*Modelica.Constants.pi) annotation( + Placement(transformation(origin = {-30, -90}, extent = {{-10, -10}, {10, 10}}))); + EmbeddedControl.Boundary.ADC adc1(stepSize = 0.01) annotation( + Placement(transformation(origin = {10, -90}, extent = {{-10, 10}, {10, -10}}, rotation = -0))); + EmbeddedControl.Boundary.ADC adc11(stepSize = 0.0001) annotation( + Placement(transformation(origin = {10, -130}, extent = {{-10, 10}, {10, -10}}))); + EmbeddedControl.Boundary.ADC adc111(stepSize = 0.0001) annotation( + Placement(transformation(origin = {10, -170}, extent = {{-10, 10}, {10, -10}}))); + Modelica.Blocks.Sources.Constant const1(k = 0) annotation( + Placement(transformation(origin = {-30, -170}, extent = {{-10, -10}, {10, 10}}))); equation - connect(adc.y, controller.u) annotation( - Line(points = {{21, 0}, {48, 0}}, color = {255, 127, 0})); connect(tickGenerator.y, adc.tickInput) annotation( - Line(points = {{-1, -40}, {10, -40}, {10, -12}}, color = {255, 0, 255})); + Line(points = {{1, -40}, {10, -40}, {10, -12}}, color = {255, 0, 255})); connect(tickGenerator.y, controller.tickInput) annotation( - Line(points = {{-1, -40}, {60, -40}, {60, -12}}, color = {255, 0, 255})); + Line(points = {{1, -40}, {60, -40}, {60, -12}}, color = {255, 0, 255})); connect(tickGenerator.y, dac.tickInput) annotation( - Line(points = {{-1, -40}, {106, -40}, {106, -12}}, color = {255, 0, 255})); + Line(points = {{1, -40}, {106, -40}, {106, -12}}, color = {255, 0, 255})); connect(tickGenerator.tickPeriodUs, controller.tickPeriodIn) annotation( - Line(points = {{-1, -48}, {52, -48}, {52, -12}}, color = {0, 0, 127})); - connect(world.frame_b, revolute.frame_a) annotation( - Line(points = {{-112, -30}, {-102, -30}}, color = {95, 95, 95})); - connect(angleSensor.phi, adc.u) annotation( - Line(points = {{-61, 0}, {-2, 0}}, color = {0, 0, 127})); - connect(revolute.axis, angleSensor.flange) annotation( - Line(points = {{-92, -20}, {-92, 0}, {-82, 0}})); - connect(damper.flange_a, revolute.support) annotation( - Line(points = {{-104, 26}, {-112, 26}, {-112, -10}, {-98, -10}, {-98, -20}})); - connect(revolute.axis, damper.flange_b) annotation( - Line(points = {{-92, -20}, {-92, 14}, {-78, 14}, {-78, 26}, {-84, 26}})); - connect(revolute.frame_b, body.frame_a) annotation( - Line(points = {{-82, -30}, {-72, -30}}, color = {95, 95, 95})); + Line(points = {{1, -48}, {52, -48}, {52, -12}}, color = {0, 0, 127})); connect(controller.y, dac.u) annotation( Line(points = {{71, 0}, {94, 0}}, color = {255, 127, 0})); - connect(torque.support, damper.flange_a) annotation( - Line(points = {{-94, 42}, {-94, 36}, {-104, 36}, {-104, 26}})); - connect(torque.flange, damper.flange_b) annotation( - Line(points = {{-84, 52}, {-84, 26}})); + connect(world.frame_b, prismatic1.frame_a) annotation( + Line(points = {{-180, -40}, {-156, -40}}, color = {95, 95, 95})); + connect(prismatic1.frame_b, revolute1.frame_a) annotation( + Line(points = {{-136, -40}, {-106, -40}}, color = {95, 95, 95})); + connect(revolute1.frame_b, body1.frame_a) annotation( + Line(points = {{-86, -40}, {-60, -40}}, color = {95, 95, 95})); + connect(idealRollingWheel1.flangeT, prismatic1.axis) annotation( + Line(points = {{-138, -10}, {-138, -34}})); + connect(torque.flange, idealRollingWheel1.flangeR) annotation( + Line(points = {{-112, 10}, {-138, 10}})); + connect(torque.support, revolute1.support) annotation( + Line(points = {{-102, 0}, {-102, -30}})); + connect(revolute1.support, damper1.flange_b) annotation( + Line(points = {{-102, -30}, {-102, -12}, {-76, -12}, {-76, 34}, {-92, 34}})); + connect(idealRollingWheel1.flangeR, damper1.flange_a) annotation( + Line(points = {{-138, 10}, {-138, 34}, {-112, 34}})); + connect(revolute1.axis, relAngleSensor.flange_a) annotation( + Line(points = {{-96, -30}, {-96, -24}, {-36, -24}, {-36, -10}})); + connect(relAngleSensor.flange_b, revolute1.support) annotation( + Line(points = {{-36, 10}, {-58, 10}, {-58, -12}, {-102, -12}, {-102, -30}})); + connect(relAngleSensor.phi_rel, adc.u) annotation( + Line(points = {{-24, 0}, {-2, 0}}, color = {0, 0, 127})); connect(dac.y, torque.tau) annotation( - Line(points = {{118, 0}, {128, 0}, {128, 74}, {-118, 74}, {-118, 52}, {-106, 52}}, color = {0, 0, 127})); - connect(integerConstant.y, controller.setpoint) annotation( - Line(points = {{22, 30}, {32, 30}, {32, 8}, {48, 8}}, color = {255, 127, 0})); - annotation( + Line(points = {{117, 0}, {128, 0}, {128, 66}, {-36, 66}, {-36, 22}, {-70, 22}, {-70, 10}, {-90, 10}}, color = {0, 0, 127})); + connect(prismatic1.axis, positionSensor.flange) annotation( + Line(points = {{-138, -34}, {-130, -34}, {-130, -74}, {-110, -74}}, color = {0, 127, 0})); + connect(tickGenerator.y, adc1.tickInput) annotation( + Line(points = {{2, -40}, {10, -40}, {10, -78}}, color = {255, 0, 255})); + connect(const.y, adc1.u) annotation( + Line(points = {{-18, -90}, {-2, -90}}, color = {0, 0, 127})); + connect(tickGenerator.y, adc11.tickInput) annotation( + Line(points = {{2, -40}, {10, -40}, {10, -118}}, color = {255, 0, 255})); + connect(positionSensor.s, adc11.u) annotation( + Line(points = {{-88, -74}, {-60, -74}, {-60, -130}, {-2, -130}}, color = {0, 0, 127})); + connect(const1.y, adc111.u) annotation( + Line(points = {{-18, -170}, {-2, -170}}, color = {0, 0, 127})); + connect(tickGenerator.y, adc111.tickInput) annotation( + Line(points = {{2, -40}, {10, -40}, {10, -158}}, color = {255, 0, 255})); + connect(adc.y, controller.angle_crad) annotation( + Line(points = {{22, 0}, {28, 0}, {28, 12}, {48, 12}}, color = {255, 127, 0})); + connect(adc1.y, controller.angle_setpoint) annotation( + Line(points = {{22, -90}, {34, -90}, {34, 18}, {48, 18}}, color = {255, 127, 0})); + connect(adc11.y, controller.position_mm) annotation( + Line(points = {{22, -130}, {38, -130}, {38, 0}, {48, 0}}, color = {255, 127, 0})); + connect(adc111.y, controller.position_setpoint) annotation( + Line(points = {{22, -170}, {44, -170}, {44, -6}, {48, -6}}, color = {255, 127, 0})); + annotation( uses(Modelica(version = "4.1.0")), - Diagram(coordinateSystem(extent = {{-140, 40}, {120, -60}})), + Diagram(coordinateSystem(extent = {{-220, 80}, {140, -140}})), version = "", - experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02)); + experiment(StartTime = 0, StopTime = 25, Tolerance = 1e-06, Interval = 0.02)); end test; \ No newline at end of file