Setup for position control

.gitignore

@@ -1,2 +1,3 @@
 build/
-__pycache__*
+__pycache__*
+exporttest/out

BondGraph/_2D/package.mo

@@ -7,46 +7,57 @@ package _2D
         Icon(graphics = {Rectangle(lineColor = {0, 85, 0}, fillColor = {0, 85, 0},fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
     end BondPort;
 
-    model J1 "Bond graph 2D 1-junction (common flow, efforts sum to zero)"
+    model J1 "Bond graph 1-junction (common flow, efforts sum to zero)"
-        parameter Integer N(min=1) = 2 "# of power ports";
+        parameter Integer            N(min=1) = 2         "# of power ports";
-        parameter Real s[N] = fill(1.0, N)
+        parameter Real                s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
-        "Bond orientation signs used in the effort balance";
+    
-        BondPort P[N] "Power ports" annotation(
+        BondPort                    P[N]                    "Power ports"
+        annotation(
         Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-        Real f[2];
+        Modelica.Blocks.Interfaces.RealVectorOutput f[2]            "Flow in junction"
+        annotation(
+        Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
     equation
         // Efforts sum to zero, with signs from bond directions
         for j in 1:2 loop
             sum(s[i] * P[i].e[j] for i in 1:N) = 0;
         end for;
-
+    
         // Flows are all equal
         for i in 2:N loop
-            P[i].f = P[i-1].f;
+            s[i] * P[i].f = s[i-1] * P[i-1].f;
         end for;
-        f = P[1].f;
+    
+        f = P[1].f; // * s[1];
+        
         annotation(
         Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1", textStyle = {TextStyle.Bold, TextStyle.UnderLine}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
     end J1;
 
-    model J0 "Bond graph 2D 0-junction (common effort, flows sum to zero)"
+  model J0 "Bond graph 1-junction (common effort, flows sum to zero)"
-        parameter Integer N(min=1) = 2 "# of power ports";
+      parameter Integer            N(min=1) = 2            "# of power ports";
-        parameter Real s[N] = fill(1.0, N)
+      parameter Real                s[N] = fill(1.0, N)    "Bond orientation signs used in the effort balance";
-        "Bond orientation signs used in the effort balance";
+  
-        BondPort P[N] "Power ports" annotation(
+      BondPort                    P[N]                    "Power ports"
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+      annotation(
-        Real e[2];
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-    equation
+  
-        // Flows sum to zero, with signs from bond directions
+      Modelica.Blocks.Interfaces.RealVectorOutput e[2]            "Flow in junction"
-        for j in 1:2 loop
+      annotation(
-            sum(s[i] * P[i].f[j] for i in 1:N) = 0;
+      Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
-        end for;
+  equation
-
+      // Flows sum to zero, with signs from bond directions
-        // Efforts are all equal
+      for j in 1:2 loop
-        for i in 2:N loop
+       sum(s[i] * P[i].f[j] for i in 1:N) = 0;
-            P[i].e = P[1].e;
+      end for;
-        end for;
+  
-        e = P[1].e;
+      // Efforts are all equal
+      for i in 2:N loop
+          s[i] * P[i].e = s[i-1] * P[i-1].e;
+      end for;
+  
+      e = P[1].e; // * s[1] ;
+      
         annotation(
         Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0", textStyle = {TextStyle.Bold, TextStyle.UnderLine}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
     Diagram(graphics));
@@ -119,6 +130,23 @@ package _2D
     Diagram(graphics));
     end Sf;
 
+    model MuxDemux "Combine/split two 1D bond ports and one 2D multibond port"
+        BondGraph.BondPort p1 "1D input channel 1" annotation(
+        Placement(transformation(extent = {{-110, 30}, {-90, 50}}), iconTransformation(extent = {{-110, 30}, {-90, 50}})));
+        BondGraph.BondPort p2 "1D input channel 2" annotation(
+        Placement(transformation(extent = {{-110, -50}, {-90, -30}}), iconTransformation(extent = {{-110, -50}, {-90, -30}})));
+        BondPort p "2D multibond output" annotation(
+        Placement(transformation(extent = {{90, -10}, {110, 10}}), iconTransformation(extent = {{90, -10}, {110, 10}})));
+    equation
+        // Power-preserving grouping of effort/flow pairs
+        p.e = {p1.e, p2.e};
+        p1.f = p.f[1];
+        p2.f = p.f[2];
+        annotation(
+        Icon(graphics = {Line(points = {{-70, 40}, {-30, 40}, {-10, 0}, {70, 0}}, thickness = 2), Line(points = {{-70, -40}, {-30, -40}, {-10, 0}}, thickness = 2), Line(points = {{0, 30}, {0, -30}}, thickness = 8), Text(origin = {0, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+        Diagram(graphics));
+    end MuxDemux;
+
     model TF "Bond graph 2D transformer"
         BondPort p1 "Port 1" annotation(
         Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
@@ -206,64 +234,8 @@ package _2D
         Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY", textStyle = {TextStyle.Bold, TextStyle.UnderLine}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}));
     end mGY;
 
-    package TransRotUtils
-
-        model mTFrot2lin
-            _1D.BondPort pR annotation(
-            Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-            BondPort pT annotation(
-            Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-            parameter Real r_body[2] = {1,0};
-        protected
-            Real B[2];
-        equation
-            B = {-r_body[2], r_body[1]};
-            pT.f = B * pR.f;
-            pR.e = B[1]*pT.e[1] + B[2]*pT.e[2];
-            annotation(
-            Icon(graphics = {Text(extent = {{-70, 100}, {70, -100}}, textString = "rlTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine})}));
-        end mTFrot2lin;
-
-        model rTF
-            BondPort p1 "Port 1" annotation(
-            Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-            BondPort p2 "Port 2" annotation(
-            Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-            Modelica.Blocks.Interfaces.RealInput phi "angle" annotation(
-            Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-        protected
-            Real R[2,2];
-        equation
-            R = [cos(phi), -sin(phi);
-            sin(phi), cos(phi)];
-            p1.e = R * p2.e;
-            transpose(R) * p1.f = p2.f;
-            annotation(
-            Diagram(graphics),
-            Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "rTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine}) }));
-        end rTF;
-
-    end TransRotUtils;
-    
-    model fsensor2d
-		BondPort p annotation(
-		Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
-		Modelica.Blocks.Interfaces.RealOutput f0 "Flow output" annotation(
-		Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 20}, extent = {{-8, -8}, {8, 8}})));
-		Modelica.Blocks.Interfaces.RealOutput f1 "Flow output" annotation(
-		Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, -20}, extent = {{-8, -8}, {8, 8}})));
-	equation
-		// Ideal flow sensor in bond-graph form: zero effort loading.
-		p.e = {0,0};
-		f0 = p.f[1];
-		f1 = p.f[2];
-		annotation(
-		Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "f", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}));
-	end fsensor2d;
-
-
     annotation(
     Icon(graphics = {Text(origin = {50, 0}, extent = {{-50, 100}, {50, -100}}, textString = "R", textStyle = {TextStyle.Bold}), Line(origin = {-45.22, 20.19}, points = {{-58.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {-78, 16}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
     uses(Modelica(version = "4.1.0")),
     Diagram(graphics));
-end _2D;
+end _2D;

BondGraph/_2D/package.order

@@ -14,5 +14,3 @@ mSe
 mSf
 mTF
 mGY
-TransRotUtils
-fsensor2d

BondGraph/package.mo

@@ -9,32 +9,31 @@ package BondGraph
         Icon(graphics = {Rectangle(lineColor = {0, 0, 127}, fillColor = {0, 0, 127},fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
     end BondPort;
 
-    model J1 "Bond graph 1-junction (common flow, efforts sum to zero)"
+  model J1 "Bond graph 1-junction (common flow, efforts sum to zero)"
-        parameter Integer            N(min=1) = 2         "# of power ports";
+      parameter Integer            N(min=1) = 2         "# of power ports";
-        parameter Real                s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
+      parameter Real                s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
-
+  
-        BondPort                    P[N]                    "Power ports"
+      BondPort                    P[N]                    "Power ports"
-        annotation(
+      annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
+      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-
+      Modelica.Blocks.Interfaces.RealOutput f            "Flow in junction"
-        Modelica.Blocks.Interfaces.RealOutput f            "Flow in junction"
+      annotation(
-        annotation(
+      Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
-        Placement(transformation(origin = {-100, -6}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {60, -60}, extent = {{-10, -10}, {10, 10}})));
+  equation
-    equation
+      // Efforts sum to zero, with signs from bond directions
-        // Efforts sum to zero, with signs from bond directions
+      sum(s[i] * P[i].e for i in 1:N) = 0;
-        sum(s[i] * P[i].e for i in 1:N) = 0;
+  
-
+      // Flows are all equal
-        // Flows are all equal
+      for i in 2:N loop
-        for i in 2:N loop
+          s[i] * P[i].f = s[i-1] * P[i-1].f;
-            P[i].f = P[i-1].f;
+      end for;
-        end for;
+  
-
+      f = P[1].f; // * s[1];
-        f = P[1].f * s[1];
+  
-
+      annotation(
-        annotation(
+      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
-        Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
+      Diagram(graphics));
-        Diagram(graphics));
+  end J1;
-    end J1;
 
     model J0 "Bond graph 1-junction (common effort, flows sum to zero)"
         parameter Integer            N(min=1) = 2            "# of power ports";
@@ -53,10 +52,10 @@ package BondGraph
 
         // Efforts are all equal
         for i in 2:N loop
-            P[i].f = P[i-1].f;
+            s[i] * P[i].e = s[i-1] * P[i-1].e;
         end for;
 
-        e = P[1].e * s[1];
+        e = P[1].e; // * s[1];
 
         annotation(
         Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0"), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name")}),
@@ -221,4 +220,4 @@ package BondGraph
     Diagram(graphics),
     Icon(graphics = {Line(origin = {-45.22, 20.19}, points = {{-50.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {156.78, 20.19}, points = {{-60.7774, -20.1934}, {-134.777, -20.1934}, {-76.7774, 19.8066}}, thickness = 5), Line(origin = {28.19, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {66, 12}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
     uses(Modelica(version = "4.1.0")));
-end BondGraph;
+end BondGraph;

BondGraph/package.order

@@ -1 +1,17 @@
-_1D
+BondPort
+J1
+J0
+OnePortPassive
+OnePortEnergetic
+C
+I
+R
+Se
+Sf
+TF
+GY
+mSe
+mSf
+mTF
+mGY
+_2D

EmbeddedControl.mo

@@ -0,0 +1,369 @@
+package EmbeddedControl
+  extends Modelica.Icons.Package;
+
+  package Boundary
+    model ADC
+    	extends Modelica.Blocks.Icons.DiscreteBlock;
+        parameter Real stepSize = 0.1;
+        parameter Real scale = 1;
+        Modelica.Blocks.Interfaces.RealInput u annotation(
+        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)));
+    	discrete Integer y_internal(start=0, fixed=true);
+    	
+    algorithm
+    	when tickInput then
+    		y_internal := integer(floor((scale*u)/stepSize+ 0.5));
+    	end when;
+    
+    equation
+    	y = y_internal;
+        
+        annotation(
+        Documentation(info = "<html>
+    <p>Integer analog-to-digital conversion block.</p>
+    <p>The real-valued input <code>u</code> is quantized to an integer code using:</p>
+    <p><code>y = floor((scale*u)/stepSize + 0.5)</code></p>
+    <p>This produces a rounded integer representation of the scaled input, intended for embedded-style control chains that operate on integer counts.</p>
+    <p>Parameters:</p>
+    <ul>
+    <li><code>stepSize</code>: physical value represented by one integer step</li>
+    <li><code>scale</code>: additional scaling applied before quantization</li>
+    </ul>
+    </html>"),
+        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;
+    
+    model DAC
+    	extends Modelica.Blocks.Icons.DiscreteBlock;
+        parameter Real stepSize = 0.1;
+        parameter Real scale = 1;
+        Modelica.Blocks.Interfaces.IntegerInput u annotation(
+        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
+      y = yReal;
+    
+        annotation(
+        Documentation(info = "<html>
+    <p>Integer digital-to-analog conversion block.</p>
+    <p>The integer input <code>u</code> is mapped back to a real-valued physical signal using:</p>
+    <p><code>y = (stepSize*scale)*u</code></p>
+    <p>When used with matching parameter values, this block acts as the inverse conversion of the ADC block.</p>
+    <p>Parameters:</p>
+    <ul>
+    <li><code>stepSize</code>: physical value represented by one integer step</li>
+    <li><code>scale</code>: output scaling factor</li>
+    </ul>
+    </html>"),
+        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;
+
+  package Math
+  extends Modelica.Icons.Package;
+
+    model DDifference
+    	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 := u - pre(x);
+    		x := u;
+    	end if;
+    	
+    equation
+    	y = y_internal;
+    
+    annotation(
+        Diagram(graphics),
+        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;
+
+    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;
+
+  package Bases
+    extends Modelica.Icons.BasesPackage;
+
+    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 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")));
+end EmbeddedControl;

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;

_3D/package.mo

@@ -1,275 +0,0 @@
-within BondGraph;
-
-package _3D
-  connector BondPort "Bond graph 3D multibond power port"
-    Real e[3] "Effort vector";
-    flow Real f[3] "Flow vector";
-    annotation(
-      Icon(graphics = {Rectangle(lineColor = {170, 0, 0}, fillColor = {170, 0, 0}, fillPattern = FillPattern.Solid, extent = {{-60, 60}, {60, -60}})}));
-  end BondPort;
-
-  model J1 "Bond graph 3D 1-junction (common flow, efforts sum to zero)"
-    parameter Integer N(min = 1) = 2 "# of power ports";
-    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
-    BondPort P[N] "Power ports" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-    Real f[3];
-  equation
-// Efforts sum to zero, with signs from bond directions
-    for j in 1:3 loop
-      sum(s[i]*P[i].e[j] for i in 1:N) = 0;
-    end for;
-// Flows are all equal
-    for i in 2:N loop
-      P[i].f = P[i - 1].f;
-    end for;
-    f = P[1].f;
-    annotation(
-      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "1", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
-  end J1;
-
-  model J0 "Bond graph 3D 0-junction (common effort, flows sum to zero)"
-    parameter Integer N(min = 1) = 2 "# of power ports";
-    parameter Real s[N] = fill(1.0, N) "Bond orientation signs used in the effort balance";
-    BondPort P[N] "Power ports" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-    Real e[3];
-  equation
-// Flows sum to zero, with signs from bond directions
-    for j in 1:3 loop
-      sum(s[i]*P[i].f[j] for i in 1:N) = 0;
-    end for;
-// Efforts are all equal
-    for i in 2:N loop
-      P[i].e = P[1].e;
-    end for;
-    e = P[1].e;
-    annotation(
-      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "0", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end J0;
-
-  partial model OnePortPassive "One-port passive 3D multibond element"
-    BondPort p "Generic power port" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(extent = {{-10, -10}, {10, 10}})));
-  end OnePortPassive;
-
-  partial model OnePortEnergetic "One-port 3D multibond storage element"
-    extends OnePortPassive annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}})));
-    Real state[3] "Conserved quantity";
-  end OnePortEnergetic;
-
-  model C "Bond graph 3D C element"
-    extends OnePortEnergetic(state(start = q0, each fixed = true));
-    parameter Real c[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Capacitance matrix inverse denominator form";
-    parameter Real q0[3] = {0, 0, 0} "Initial stored quantity (charge)";
-  equation
-    der(state) = p.f;
-    c*p.e = state;
-    annotation(
-      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "C", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end C;
-
-  model I "Bond graph 3D I element"
-    extends OnePortEnergetic(state(start = p0, each fixed = true));
-    parameter Real I[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Inertance / inductance / mass matrix";
-    parameter Real p0[3] = {0, 0, 0} "Initial stored quantity (momentum / flux)";
-  equation
-    der(state) = p.e;
-    I*p.f = state;
-    annotation(
-      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "I", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end I;
-
-  model R "Bond graph 3D resistor"
-    extends OnePortPassive;
-    parameter Real R[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Resistance matrix";
-  equation
-    p.e = R*p.f;
-    annotation(
-      Icon(graphics = {Text(extent = {{-100, 100}, {100, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end R;
-
-  model Se "Effort source"
-    BondPort p annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    parameter Real e0[3] "Imposed effort";
-  equation
-    p.e = e0;
-    annotation(
-      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Se", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end Se;
-
-  model Sf "Flow source"
-    BondPort p annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    parameter Real f0[3] "Imposed flow";
-  equation
-    p.f = f0;
-    annotation(
-      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "Sf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end Sf;
-
-  model TF "Bond graph 3D transformer"
-    BondPort p1 "Port 1" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-    BondPort p2 "Port 2" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    parameter Real n[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Transformer ratio matrix";
-  equation
-    p1.e = n*p2.e;
-    transpose(n)*p1.f = p2.f;
-    annotation(
-      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "TF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end TF;
-
-  model GY "Bond graph 3D gyrator"
-    BondPort p1 "Port 1" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-    BondPort p2 "Port 2" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    parameter Real r[3, 3] = [1, 0, 0; 0, 1, 0; 0, 0, 1] "Gyrator modulus matrix";
-  equation
-    p1.e = r*p2.f;
-    p2.e = transpose(r)*p1.f;
-    annotation(
-      Icon(graphics = {Text(extent = {{-80, 100}, {80, -100}}, textString = "GY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end GY;
-
-  model mSe "Bond graph modulated effort source"
-    BondPort p annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Interfaces.RealInput e0 "Imposed effort" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    Modelica.Blocks.Interfaces.RealInput e1 "Imposed effort" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    Modelica.Blocks.Interfaces.RealInput e2 "Imposed effort" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-  equation
-    p.e = {e0, e1, e2};
-    annotation(
-      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSe", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end mSe;
-
-  model mSf "Bond graph modulated flow source"
-    BondPort p annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Interfaces.RealInput f0 "Imposed flow" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    Modelica.Blocks.Interfaces.RealInput f1 "Imposed flow" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    Modelica.Blocks.Interfaces.RealInput f2 "Imposed flow" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-  equation
-    p.f = {f0, f1, f2};
-    annotation(
-      Icon(graphics = {Text(origin = {-20, 0}, extent = {{-80, 100}, {80, -100}}, textString = "mSf", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}),
-      Diagram(graphics));
-  end mSf;
-
-  model mTF "Bond graph modulated transformer"
-    BondPort p1 "Port 1" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-    BondPort p2 "Port 2" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-  equation
-    p1.e = m*p2.e;
-    transpose(m)*p1.f = p2.f;
-    annotation(
-      Diagram(graphics),
-      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
-  end mTF;
-
-  model mGY "Bond graph modulated gyrator"
-    BondPort p1 "Port 1" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-    BondPort p2 "Port 2" annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Interfaces.RealInput m[3, 3] "Modulation" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-  equation
-    p1.e = m*p2.f;
-    p2.e = transpose(m)*p1.f;
-    annotation(
-      Diagram(graphics),
-      Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "mGY", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic}), Text(origin = {50, 80}, textColor = {0, 0, 255}, extent = {{-50, 20}, {50, -20}}, textString = "%name", textStyle = {TextStyle.Italic})}));
-  end mGY;
-
-  package TransRotUtils
-    model mTFrot3lin
-      BondPort pR annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-      BondPort pT annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-      parameter Real r_body[3] = {1, 0, 0} "Vector from rotational reference to translational point (body frame)";
-    protected
-      Real S[3, 3];
-    equation
-// Skew matrix such that S*x = r_body x x
-      S = [0, -r_body[3], r_body[2]; r_body[3], 0, -r_body[1]; -r_body[2], r_body[1], 0];
-// v = w x r_body = -S*w
-      pT.f = transpose(S)*pR.f;
-// tau = r_body x F = S*F
-      pR.e = S*pT.e;
-      annotation(
-        Icon(graphics = {Text(extent = {{-70, 100}, {70, -100}}, textString = "rlTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine, TextStyle.Italic})}));
-    end mTFrot3lin;
-
-    model rTF3D
-      BondPort p1 "Port 1" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
-      BondPort p2 "Port 2" annotation(
-        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
-      Modelica.Blocks.Interfaces.RealInput phi "roll angle" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-      Modelica.Blocks.Interfaces.RealInput theta "pitch angle" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-      Modelica.Blocks.Interfaces.RealInput psi "yaw angle" annotation(
-        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {30, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
-    protected
-      Real R[3, 3];
-    equation
-// ZYX rotation matrix from body to inertial frame.
-      R = [cos(psi)*cos(theta), cos(psi)*sin(theta)*sin(phi) - sin(psi)*cos(phi), cos(psi)*sin(theta)*cos(phi) + sin(psi)*sin(phi); sin(psi)*cos(theta), sin(psi)*sin(theta)*sin(phi) + cos(psi)*cos(phi), sin(psi)*sin(theta)*cos(phi) - cos(psi)*sin(phi); -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)];
-      p1.e = R*p2.e;
-      transpose(R)*p1.f = p2.f;
-      annotation(
-        Diagram(graphics),
-        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "rTF", textStyle = {TextStyle.Bold, TextStyle.Italic, TextStyle.UnderLine})}));
-    end rTF3D;
-  end TransRotUtils;
-
-  model fsensor3d
-    BondPort p annotation(
-      Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-52, 0}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Interfaces.RealOutput f0 "Flow output" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 26}, extent = {{-8, -8}, {8, 8}})));
-    Modelica.Blocks.Interfaces.RealOutput f1 "Flow output" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, 0}, extent = {{-8, -8}, {8, 8}})));
-    Modelica.Blocks.Interfaces.RealOutput f2 "Flow output" annotation(
-      Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {54, -26}, extent = {{-8, -8}, {8, 8}})));
-  equation
-// Ideal flow sensor in bond-graph form: zero effort loading.
-    p.e = {0, 0, 0};
-    f0 = p.f[1];
-    f1 = p.f[2];
-    f2 = p.f[3];
-    annotation(
-      Icon(graphics = {Text(origin = {-10, 0}, extent = {{-50, 60}, {50, -60}}, textString = "f", textStyle = {TextStyle.Italic}), Ellipse(origin = {-2, 0}, lineThickness = 5, extent = {{-50, 50}, {50, -50}})}));
-  end fsensor3d;
-  annotation(
-    Icon(graphics = {Text(origin = {50, 0}, extent = {{-50, 100}, {50, -100}}, textString = "R", textStyle = {TextStyle.Bold, TextStyle.Italic}), Line(origin = {-45.22, 20.19}, points = {{-58.7774, -20.1934}, {21.2226, -20.1934}, {-38.7774, 19.8066}}, thickness = 5), Line(origin = {-9.81, -8.19}, points = {{-10.1934, 48.1934}, {-10.1934, -31.8066}}, thickness = 5), Line(origin = {-78, 16}, points = {{-26, 0}, {30, 0}}, thickness = 5), Line(origin = {-78, -16}, points = {{-26, 0}, {30, 0}}, thickness = 5)}),
-    uses(Modelica(version = "4.1.0")),
-    Diagram(graphics));
-end _3D;

_3D/package.order

@@ -1,18 +0,0 @@
-BondPort
-J1
-J0
-OnePortPassive
-OnePortEnergetic
-C
-I
-R
-Se
-Sf
-TF
-GY
-mSe
-mSf
-mTF
-mGY
-TransRotUtils
-fsensor3d

exporttest/export_controller.log

@@ -0,0 +1,17 @@
+true
+true
+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() = true
+
+

exporttest/export_controller.mos

@@ -0,0 +1,14 @@
+setCommandLineOptions("--simCodeTarget=ExperimentalEmbeddedC");
+setCommandLineOptions("--target=gcc");
+setCommandLineOptions("-d=initialization");
+
+loadFile("/work/EmbeddedControl.mo");
+loadFile("/work/test.mo");
+
+okCheck := checkModel(test.Controller);
+print("checkModel() = " + String(okCheck) + "\n");
+print(getErrorString());
+
+okTranslate := translateModel(test.Controller, fileNamePrefix="controller");
+print("translateModel() = " + String(okTranslate) + "\n");
+print(getErrorString());

exporttest/run_mos_docker.sh

@@ -0,0 +1,19 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+ROOT_DIR="$(cd "$(dirname "$0")/.." && pwd)"
+MOS_FILE="/work/exporttest/export_controller.mos"
+LOG_FILE="$ROOT_DIR/exporttest/export_controller.log"
+
+docker run --rm \
+  -v "$ROOT_DIR:/work" \
+  -v "$HOME/.openmodelica/libraries:/home/ubuntu/.openmodelica/libraries:ro" \
+  -w /work/exporttest/out \
+  openmodelica omc "$MOS_FILE" 2>&1 | tee "$LOG_FILE"
+
+if grep -E "Error:|= false" "$LOG_FILE" >/dev/null; then
+  echo "Failed. See: $LOG_FILE" >&2
+  exit 1
+fi
+
+echo "Success. Log: $LOG_FILE"

test.mo

@@ -0,0 +1,132 @@
+model test
+  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.01)  annotation(
+    Placement(transformation(origin = {10, 0}, extent = {{-10, -10}, {10, 10}})));
+  model Controller
+    extends EmbeddedControl.Bases.ControlBase;
+  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 = {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(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, 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 = {-10, -40}, extent = {{-10, -10}, {10, 10}})));
+
+  inner Modelica.Mechanics.MultiBody.World world annotation(
+    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(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(controller.y, dac.u) annotation(
+    Line(points = {{71, 0}, {94, 0}}, color = {255, 127, 0}));
+  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 = {{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 = {{-220, 80}, {140, -140}})),
+  version = "",
+  experiment(StartTime = 0, StopTime = 25, Tolerance = 1e-06, Interval = 0.02));
+end test;

test1d.mo

@@ -2,16 +2,23 @@ model test1d
   BondGraph.J1 j1(N = 3, s = {-1, -1, 1})  annotation(
     Placement(transformation(extent = {{-10, -10}, {10, 10}})));
   BondGraph.R r1(R = 1)  annotation(
-    Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}})));
+    Placement(transformation(origin = {0, 42}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph.R r2(R = 1)  annotation(
+  BondGraph.mSe mSe annotation(
-    Placement(transformation(origin = {0, -40}, extent = {{-10, -10}, {10, 10}})));
-  BondGraph.Se se(e0 = 1)  annotation(
     Placement(transformation(origin = {-40, 0}, extent = {{-10, -10}, {10, 10}})));
+  BondGraph.C c(c = 0.1)  annotation(
+    Placement(transformation(origin = {0, -40}, extent = {{-10, -10}, {10, 10}})));
+  Modelica.Blocks.Sources.Step step(startTime = 0.5)  annotation(
+    Placement(transformation(origin = {-70, 0}, extent = {{-10, -10}, {10, 10}})));
 equation
   connect(j1.P[1], r1.p) annotation(
-    Line(points = {{0, 0}, {0, 40}}, color = {0, 0, 127}));
+    Line(points = {{0, 0}, {0, 42}}, color = {0, 0, 127}));
-  connect(j1.P[2], r2.p) annotation(
+  connect(mSe.p, j1.P[3]) annotation(
-    Line(points = {{0, 0}, {0, -40}}, color = {0, 0, 127}));
-  connect(se.p, j1.P[3]) annotation(
     Line(points = {{-32, 0}, {0, 0}}, color = {0, 0, 127}));
+  connect(j1.P[2], c.p) annotation(
+    Line(points = {{0, 0}, {0, -40}}, color = {0, 0, 127}));
+  connect(step.y, mSe.e0) annotation(
+    Line(points = {{-58, 0}, {-40, 0}, {-40, -8}}, color = {0, 0, 127}));
+
+annotation(
+    uses(Modelica(version = "4.1.0")));
 end test1d;

test2d.mo

@@ -1,99 +1,568 @@
-model test2d
+package test2d
-    import _1D = BondGraph._1D;
+
-    import _2D = BondGraph._2D;
+    model vis2d
-    _1D.J1 w_com(N = 4, s = {1, -1, -1, -1}) annotation(
+        import Modelica.Mechanics.MultiBody.Visualizers;
-    Placement(transformation(origin = {0, 120}, extent = {{-10, -10}, {10, 10}})));
+        import Modelica.Mechanics.MultiBody.Frames;
-    _2D.TransRotUtils.mTFrot2lin mTFrot2lin(r_body = {0.5, 0}) annotation(
+
-    Placement(transformation(origin = {0, 80}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
+        parameter Real L = 1;
-    _2D.J0 j0(N = 3, s = {1, 1, -1}) annotation(
+        parameter Real w = 0.02;
-    Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real orbRadius = 0.04;
-    _2D.J1 v_p_bff(N = 2, s = {1, 1}) annotation(
+        parameter Real axisLength = 0.25;
-    Placement(transformation(origin = {-40, 40}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real axisWidth = 0.01;
-    _2D.J1 v_com_bff(N = 2, s = {-1, -1}) annotation(
+        parameter Real planeSize = 2.0;
-    Placement(transformation(origin = {40, 40}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real planeThickness = 0.002;
-    _2D.TransRotUtils.rTF rTF annotation(
+        parameter Real planeOffset = -0.001;
-    Placement(transformation(origin = {40, 0}, extent = {{-10, -10}, {10, 10}}, rotation = 90)));
+
-    _1D.fsensor wsensor annotation(
+        Modelica.Blocks.Interfaces.RealInput phi annotation(
-    Placement(transformation(origin = {50, 90}, extent = {{-10, -10}, {10, 10}})));
+        Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}})));
-    Modelica.Blocks.Continuous.Integrator w2phi(k = 1) annotation(
+        Modelica.Blocks.Interfaces.RealInput y annotation(
-    Placement(transformation(origin = {90, 90}, extent = {{-10, -10}, {10, 10}})));
+        Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}})));
-    _2D.J1 v_com_in(N = 4, s = {1, -1, 1, 1}) annotation(
+        Modelica.Blocks.Interfaces.RealInput x annotation(
-    Placement(transformation(origin = {40, -40}, extent = {{-10, -10}, {10, 10}})));
+        Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}})));
-    _2D.I mass(I = [1, 0; 0, 1]) annotation(
+    protected
-    Placement(transformation(origin = {80, -40}, extent = {{-10, -10}, {10, 10}})));
+        Real Rz[3,3];
-    BondGraph._2D.Sf ground(f0 = {0, 0}) annotation(
+        Real r[3];
-    Placement(transformation(origin = {-80, 40}, extent = {{-10, -10}, {10, 10}})));
+
-    BondGraph._1D.I m_inertial(I = 0.001) annotation(
+        Visualizers.Advanced.Shape link(
-    Placement(transformation(origin = {40, 120}, extent = {{-10, -10}, {10, 10}})));
+        shapeType = "cylinder",
-    BondGraph._1D.Sf ground2(f0 = 0) annotation(
+        length = L/2,
-    Placement(transformation(origin = {-80, 120}, extent = {{-10, -10}, {10, 10}})));
+        width = w,
-    BondGraph._1D.J0 j1(N = 3, s = {1, -1, -1}) annotation(
+        height = w,
-    Placement(transformation(origin = {-40, 120}, extent = {{-10, -10}, {10, 10}})));
+        r_shape = {L / 2, 0, 0},
-    BondGraph._1D.J1 w_hinge(N = 2, s = {1, -1}) annotation(
+        lengthDirection = {-1, 0, 0},
-    Placement(transformation(origin = {-40, 90}, extent = {{-10, -10}, {10, 10}})));
+        widthDirection = {0, 1, 0},
-    BondGraph._1D.R hinge_r(R = 0.08) annotation(
+        r = r,
-    Placement(transformation(origin = {-80, 90}, extent = {{-10, -10}, {10, 10}})));
+        R = Frames.from_T(Rz, zeros(3)));
-    BondGraph._2D.Se g(e0 = {0, -9.81}) annotation(
+
-    Placement(transformation(origin = {2, -40}, extent = {{-10, -10}, {10, 10}})));
+        Visualizers.Advanced.Shape orb(
-    BondGraph._2D.fsensor2d v_inertial annotation(
+        shapeType = "sphere",
-    Placement(transformation(origin = {40, -60}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
+        length = 2 * orbRadius,
-    Modelica.Blocks.Continuous.Integrator vx_to_x(k = 1, y_start = -0.5)  annotation(
+        width = 2 * orbRadius,
-    Placement(transformation(origin = {70, -70}, extent = {{-10, -10}, {10, 10}})));
+        height = 2 * orbRadius,
-    Modelica.Blocks.Continuous.Integrator vy_to_y(k = 1)  annotation(
+        r_shape = {0, 0, 0},
-    Placement(transformation(origin = {70, -100}, extent = {{-10, -10}, {10, 10}})));
+        lengthDirection = {1, 0, 0},
-    vis2d vis2d1 annotation(
+        widthDirection = {0, 1, 0},
-    Placement(transformation(origin = {150, -6}, extent = {{-10, -10}, {10, 10}})));
+        r = r,
-equation
+        R = Frames.from_T(Rz, zeros(3)));
-    connect(w_com.P[2], mTFrot2lin.pR) annotation(
+
-    Line(points = {{0, 120}, {0, 88}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        Visualizers.Advanced.Shape xAxis(
-    connect(mTFrot2lin.pT, j0.P[1]) annotation(
+        shapeType = "cylinder",
-    Line(points = {{0, 72}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        length = axisLength,
-    connect(v_com_bff.P[1], j0.P[2]) annotation(
+        width = axisWidth,
-    Line(points = {{40, 40}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        height = axisWidth,
-    connect(j0.P[3], v_p_bff.P[1]) annotation(
+        color = {255, 0, 0},
-    Line(origin = {-1, 0}, points = {{0, 40}, {-40, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        specularCoefficient = 0.1,
-    connect(wsensor.f, w2phi.u) annotation(
+        r_shape = {axisLength / 2, 0, 0},
-    Line(points = {{58, 90}, {78, 90}}, color = {0, 0, 127}));
+        lengthDirection = {1, 0, 0},
-    connect(w2phi.y, rTF.phi) annotation(
+        widthDirection = {0, 1, 0},
-    Line(points = {{102, 90}, {110, 90}, {110, 0}, {48, 0}}, color = {0, 0, 127}, arrow = {Arrow.None, Arrow.Filled}));
+        r = zeros(3),
-    connect(v_com_in.P[2], mass.p) annotation(
+        R = Frames.from_T([1, 0, 0;
-    Line(points = {{40, -40}, {80, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        0, 1, 0;
-    connect(w_com.P[3], wsensor.p) annotation(
+        0, 0, 1], zeros(3)));
-    Line(points = {{0, 120}, {44, 90}}, color = {0, 0, 127}, pattern = LinePattern.Dash, thickness = 0.75, arrow = {Arrow.None, Arrow.Open}, arrowSize = 6));
+
-    connect(w_com.P[1], m_inertial.p) annotation(
+        Visualizers.Advanced.Shape yAxis(
-    Line(points = {{0, 120}, {40, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        shapeType = "cylinder",
-    connect(ground2.p, j1.P[1]) annotation(
+        length = axisLength,
-    Line(points = {{-72, 120}, {-40, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        width = axisWidth,
-    connect(j1.P[2], w_com.P[4]) annotation(
+        height = axisWidth,
-    Line(points = {{-40, 120}, {0, 120}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.Half, Arrow.None}, arrowSize = 6));
+        color = {0, 200, 0},
-    connect(ground.p, v_p_bff.P[2]) annotation(
+        specularCoefficient = 0.1,
-    Line(points = {{-72, 40}, {-40, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        r_shape = {0, axisLength / 2, 0},
-    connect(j1.P[3], w_hinge.P[1]) annotation(
+        lengthDirection = {0, 1, 0},
-    Line(points = {{-40, 120}, {-40, 90}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        widthDirection = {0, 0, 1},
-    connect(w_hinge.P[2], hinge_r.p) annotation(
+        r = zeros(3),
-    Line(points = {{-40, 90}, {-80, 90}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        R = Frames.from_T([1, 0, 0;
-    connect(g.p, v_com_in.P[3]) annotation(
+        0, 1, 0;
-    Line(points = {{10, -40}, {40, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        0, 0, 1], zeros(3)));
-    connect(v_com_bff.P[2], rTF.p2) annotation(
+
-    Line(points = {{40, 40}, {40, 8}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        Visualizers.Advanced.Shape zAxis(
-    connect(rTF.p1, v_com_in.P[1]) annotation(
+        shapeType = "cylinder",
-    Line(points = {{40, -8}, {40, -40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        length = axisLength,
-    connect(v_com_in.P[4], v_inertial.p) annotation(
+        width = axisWidth,
-    Line(points = {{40, -40}, {40, -54}}, color = {0, 85, 0}, pattern = LinePattern.Dash, thickness = 0.75, arrow = {Arrow.None, Arrow.Open}, arrowSize = 6));
+        height = axisWidth,
-    connect(v_inertial.f0, vx_to_x.u) annotation(
+        color = {0, 90, 255},
-    Line(points = {{42, -66}, {42, -70}, {58, -70}}, color = {0, 0, 127}));
+        specularCoefficient = 0.1,
-    connect(v_inertial.f1, vy_to_y.u) annotation(
+        r_shape = {0, 0, axisLength / 2},
-    Line(points = {{38, -66}, {38, -100}, {58, -100}}, color = {0, 0, 127}));
+        lengthDirection = {0, 0, 1},
-    connect(w2phi.y, vis2d1.phi) annotation(
+        widthDirection = {1, 0, 0},
-    Line(points = {{102, 90}, {110, 90}, {110, 0}, {141, 0}}, color = {0, 0, 127}));
+        r = zeros(3),
-    connect(vx_to_x.y, vis2d1.x) annotation(
+        R = Frames.from_T([1, 0, 0;
-    Line(points = {{82, -70}, {110, -70}, {110, -6}, {141, -6}}, color = {0, 0, 127}));
+        0, 1, 0;
-    connect(vy_to_y.y, vis2d1.y) annotation(
+        0, 0, 1], zeros(3)));
-    Line(points = {{82, -100}, {118, -100}, {118, -12}, {141, -12}}, color = {0, 0, 127}));
+    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;
+        parameter Modelica.Units.SI.MomentOfInertia momentOfInertia;
+        parameter Modelica.Units.SI.Angle startAngle;
+
+        BondGraph.J1 w_com(N = 3, s = {-1, -1, 1}) annotation(
+        Placement(transformation(origin = {0, 100}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph.I moi(I = momentOfInertia, state(fixed = false)) annotation(
+        Placement(transformation(origin = {40, 100}, extent = {{-10, -10}, {10, 10}})));
+        mTFrot2lin mTFrot2lin1(r_body = {-0.5*length, 0}) annotation(
+        Placement(transformation(origin = {0, 70}, extent = {{-10, -10}, {10, 10}}, rotation = -90)));
+        BondGraph._2D.J0 j0(N = 3, s = {1, 1, -1}) annotation(
+        Placement(transformation(origin = {0, 40}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.J1 v_com_i(N = 3, s = {1, -1, 1}) annotation(
+        Placement(transformation(origin = {40, -20}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.J1 v_P_bff(N = 2, s = {1, -1}) annotation(
+        Placement(transformation(origin = {-40, 40}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.J1 v_com_bff(N = 2, s = {-1, -1}) annotation(
+        Placement(transformation(origin = {40, 40}, extent = {{-10, -10}, {10, 10}})));
+        rTF rTF1 annotation(
+        Placement(transformation(origin = {40, 10}, extent = {{-10, -10}, {10, 10}}, rotation = 90)));
+        Modelica.Blocks.Continuous.Integrator w2phi(y_start=startAngle)  annotation(
+        Placement(transformation(origin = {30, 70}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.I m(I = [mass, 0; 0, mass]) annotation(
+        Placement(transformation(origin = {80, -20}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.Se g(e0 = {0, -9.81*mass}) annotation(
+        Placement(transformation(origin = {0, -20}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Continuous.Integrator vx2x(y_start=-0.5*length*cos(startAngle))  annotation(
+        Placement(transformation(origin = {70, -50}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Continuous.Integrator vy2y(y_start=-0.5*length*sin(startAngle))  annotation(
+        Placement(transformation(origin = {70, -80}, extent = {{-10, -10}, {10, 10}})));
+        vis2d vis2d1 annotation(
+        Placement(transformation(origin = {118, -64}, extent = {{-10, -10}, {10, 10}})));
+        rTF rTF2 annotation(
+        Placement(transformation(origin = {-40, 10}, extent = {{-10, -10}, {10, 10}}, rotation = 90)));
+        BondGraph._2D.J1 v_P_i(N = 2, s = {1, 1})  annotation(
+        Placement(transformation(origin = {-40, -20}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph.BondPort rotIn annotation(
+        Placement(transformation(origin = {-30, 100}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, 198}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.BondPort trans_i annotation(
+        Placement(transformation(origin = {-80, -20}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {0, -198}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealOutput phi annotation(
+        Placement(transformation(origin = {120, 70}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {202, 80}, extent = {{-10, -10}, {10, 10}})));
+    equation
+        connect(w_com.f, w2phi.u) annotation(
+        Line(points = {{6, 94}, {18, 70}}, color = {0, 0, 127}));
+        connect(w2phi.y, rTF1.phi) annotation(
+        Line(points = {{41, 70}, {100, 70}, {100, 10}, {48, 10}}, color = {0, 0, 127}));
+        connect(w_com.P[1], moi.p) annotation(
+        Line(points = {{0, 100}, {40, 100}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(w_com.P[2], mTFrot2lin1.pR) annotation(
+        Line(points = {{0, 100}, {0, 78}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(v_com_bff.P[1], j0.P[1]) annotation(
+        Line(points = {{40, 40}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(mTFrot2lin1.pT, j0.P[2]) annotation(
+        Line(points = {{0, 62}, {0, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(j0.P[3], v_P_bff.P[1]) annotation(
+        Line(points = {{0, 40}, {-40, 40}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(v_com_bff.P[2], rTF1.p2) annotation(
+        Line(points = {{40, 40}, {40, 18}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(rTF1.p1, v_com_i.P[1]) annotation(
+        Line(points = {{40, 2}, {40, -20}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(g.p, v_com_i.P[3]) annotation(
+        Line(points = {{8, -20}, {40, -20}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(v_com_i.P[2], m.p) annotation(
+        Line(points = {{40, -20}, {80, -20}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(v_com_i.f[1], vx2x.u) annotation(
+        Line(points = {{46, -26}, {50, -50}, {58, -50}}, color = {0, 0, 127}));
+        connect(v_com_i.f[2], vy2y.u) annotation(
+        Line(points = {{46, -26}, {50, -80}, {58, -80}}, color = {0, 0, 127}));
+        connect(vy2y.y, vis2d1.y) annotation(
+        Line(points = {{82, -80}, {94, -80}, {94, -70}, {108, -70}}, color = {0, 0, 127}));
+        connect(vx2x.y, vis2d1.x) annotation(
+        Line(points = {{82, -50}, {94, -50}, {94, -64}, {108, -64}}, color = {0, 0, 127}));
+        connect(w2phi.y, vis2d1.phi) annotation(
+        Line(points = {{42, 70}, {100, 70}, {100, -58}, {108, -58}}, color = {0, 0, 127}));
+        connect(w2phi.y, rTF2.phi) annotation(
+        Line(points = {{42, 70}, {100, 70}, {100, 26}, {-20, 26}, {-20, 10}, {-32, 10}}, color = {0, 0, 127}));
+        connect(v_P_bff.P[2], rTF2.p2) annotation(
+        Line(points = {{-40, 40}, {-40, 18}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(rTF2.p1, v_P_i.P[1]) annotation(
+        Line(points = {{-40, 2}, {-40, -20}}, color = {0, 85, 0}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(rotIn, w_com.P[3]) annotation(
+        Line(points = {{-30, 100}, {0, 100}}, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(trans_i, v_P_i.P[2]) annotation(
+        Line(points = {{-80, -20}, {-40, -20}}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(w2phi.y, phi) annotation(
+        Line(points = {{42, 70}, {120, 70}}, color = {0, 0, 127}));
+        annotation(
+        uses(Modelica(version = "4.1.0")),
+        Diagram(coordinateSystem(extent = {{-100, 120}, {140, -100}})),
+        Icon(coordinateSystem(extent = {{-200, -200}, {200, 200}}), graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, lineThickness = 2, extent = {{-200, 200}, {200, -200}}, radius = 50), Ellipse(origin = {0, -60},lineColor = {255, 0, 0}, fillColor = {255, 0, 0}, fillPattern = FillPattern.Solid, extent = {{-15, 15}, {15, -15}}), Line(origin = {29, 19}, points = {{-29, -79}, {29, 79}}, color = {255, 0, 0}, thickness = 5), Line(origin = {0, -76}, points = {{-130, 0}, {130, 0}}, thickness = 1.25)}),
+        version = "",
+        experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002));
+    end pendulum;
+
+    model mTFrot2lin
+        BondGraph.BondPort pR annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.BondPort pT annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        parameter Real r_body[2] = {1, 0};
+    protected
+        Real B[2];
+    equation
+        B = {-r_body[2], r_body[1]};
+        pT.f = B*pR.f;
+        pR.e = B[1]*pT.e[1] + B[2]*pT.e[2];
+        annotation(
+        Icon(graphics = {Text(extent = {{-70, 100}, {70, -100}}, textString = "rlTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine})}));
+    end mTFrot2lin;
+
+    model rTF
+        BondGraph._2D.BondPort p1 "Port 1" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {-80, 0}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.BondPort p2 "Port 2" annotation(
+        Placement(transformation(origin = {-44, 18}, extent = {{-10, -10}, {10, 10}}), iconTransformation(origin = {80, 0}, extent = {{-10, -10}, {10, 10}})));
+        Modelica.Blocks.Interfaces.RealInput phi "angle" annotation(
+        Placement(transformation(origin = {-8, -64}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {0, -78}, extent = {{-8, -8}, {8, 8}}, rotation = 90)));
+    protected
+        Real R[2, 2];
+    equation
+        R = [cos(phi), -sin(phi); sin(phi), cos(phi)];
+        p1.e = R*p2.e;
+        transpose(R)*p1.f = p2.f;
+        annotation(
+        Diagram(graphics),
+        Icon(graphics = {Text(extent = {{-60, 100}, {60, -100}}, textString = "rTF", textStyle = {TextStyle.Bold, TextStyle.UnderLine})}));
+    end rTF;
+
+    model test
+        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 = 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}})));
+        BondGraph.R r(R = 0.02) annotation(
+        Placement(transformation(origin = {-18, 40}, extent = {{-10, -10}, {10, 10}})));
+        //BondGraph._2D.Sf sf(f0 = {0, 0})  annotation(
+        //    Placement(transformation(origin = {60, -58}, extent = {{-10, -10}, {10, 10}}, rotation = 90)));
+        BondGraph._2D.J1 j2(N = 2, s = {-1, 1})  annotation(
+        Placement(transformation(origin = {60, -40}, extent = {{-10, -10}, {10, 10}})));
+        BondGraph._2D.MuxDemux muxDemux annotation(
+        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 = 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}})));
+        Modelica.Blocks.Sources.TimeTable timeTable(table = [
+        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, 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));
+        connect(r.p, w_hinge.P[2]) annotation(
+        Line(points = {{-18, 40}, {20, 40}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        connect(j1.P[2], pendulum1.rotIn) annotation(
+        Line(points = {{60, 40}, {60, 20}}, color = {0, 0, 127}, thickness = 0.75, arrow = {Arrow.None, Arrow.Half}, arrowSize = 6));
+        //connect(sf.p, pendulum1.trans_i) annotation(
+        //    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(
+        Line(points = {{40, -40}, {60, -40}}, color = {0, 85, 0}));
+        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(
+        Line(points = {{-20, -20}, {8, -20}, {8, -36}, {20, -36}}, color = {0, 0, 127}));
+        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(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, 100}, {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 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(
+        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 = 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}})));
+        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)));
+        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}));
+        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}));
+        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, 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")),
+    uses(Modelica(version = "4.1.0")));
-    experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02),
+end test2d;
-    Diagram(coordinateSystem(extent = {{-100, 140}, {160, -120}})),
-    Icon(coordinateSystem(extent = {{-200, -200}, {200, 200}})),
-    version = "");
-end test2d;

test3d.mo

@@ -1,158 +0,0 @@
-model test3d
-  import _3D = BondGraph._3D;
-
-  // ---------------------------------------------------------------------------
-  // Parameters
-  // ---------------------------------------------------------------------------
-  parameter Real J[3,3] = [0.05, 0, 0;
-                           0, 0.06, 0;
-                           0, 0, 0.02] "Body inertia tensor in body frame";
-  parameter Real m[3,3] = [1, 0, 0;
-                           0, 1, 0;
-                           0, 0, 1] "Translational inertia (mass matrix)";
-  parameter Real r_body[3] = {0.5, 0, 0} "Vector from hinge point to COM in body frame";
-  parameter Real R_rot[3,3] = [0.02, 0, 0;
-                               0, 0.02, 0;
-                               0, 0, 0.02] "Rotational damping matrix";
-  parameter Real g = 9.81 "Gravity magnitude";
-
-  // ---------------------------------------------------------------------------
-  // Rotational dynamics (body frame)
-  // ---------------------------------------------------------------------------
-  _3D.J1 w_com(N = 6, s = {1, -1, -1, -1, -1, -1}) "Common angular velocity junction";
-  _3D.I J_inertial(I = J) "Angular momentum storage";
-  _3D.R hinge_r(R = R_rot) "Rotational damping";
-  _3D.fsensor3d wsensor "Measures body angular velocity omega";
-
-  // Gyroscopic term implementation (power-conserving structure):
-  // - TF maps omega -> h = J*omega (angular momentum)
-  // - mGY with modulation S(omega) generates tau_g = omega x h
-  _3D.TF Hmap(n = J) "Maps between omega and angular-momentum port";
-  _3D.mGY gyro "Modulated gyrator for omega x h coupling";
-
-  // ---------------------------------------------------------------------------
-  // Kinematics and frame transforms
-  // ---------------------------------------------------------------------------
-  _3D.TransRotUtils.mTFrot3lin mTFrot3lin(r_body = r_body) "v = omega x r_body coupling";
-  _3D.J0 j0(N = 3, s = {1, 1, -1}) "Velocity composition at COM in body frame";
-  _3D.J1 v_p_bff(N = 2, s = {1, 1}) "Hinge-point velocity (body frame)";
-  _3D.J1 v_com_bff(N = 2, s = {-1, -1}) "COM velocity (body frame)";
-  _3D.TransRotUtils.rTF3D rTF3D "Body <-> inertial frame transform";
-
-  // ---------------------------------------------------------------------------
-  // Translational dynamics (inertial frame)
-  // ---------------------------------------------------------------------------
-  _3D.J1 v_com_in(N = 4, s = {1, -1, -1, 1}) "COM velocity in inertial frame";
-  _3D.I mass(I = m) "Translational momentum storage";
-  _3D.Sf ground(f0 = {0, 0, 0}) "Ground translational velocity";
-  _3D.mSe gravity "Gravity effort source";
-  _3D.fsensor3d v_inertial "Measures inertial COM velocity";
-
-  // ---------------------------------------------------------------------------
-  // States and signals
-  // ---------------------------------------------------------------------------
-  Real omega[3] "Body angular velocity";
-  Real h[3] "Body angular momentum J*omega";
-  Real S_omega[3,3] "Skew matrix of omega";
-
-  Real phi(start = 0) "Roll";
-  Real theta(start = 0.1) "Pitch";
-  Real psi(start = 0) "Yaw";
-  Real euler_dot[3] "{phi_dot, theta_dot, psi_dot}";
-
-  Real x(start = -0.5) "COM x (inertial)";
-  Real y(start = 0) "COM y (inertial)";
-  Real z(start = -0.2) "COM z (inertial)";
-
-  Modelica.Blocks.Sources.RealExpression xSig(y = x);
-  Modelica.Blocks.Sources.RealExpression ySig(y = y);
-  Modelica.Blocks.Sources.RealExpression zSig(y = z);
-  Modelica.Blocks.Sources.RealExpression phiSig(y = phi);
-  Modelica.Blocks.Sources.RealExpression thetaSig(y = theta);
-  Modelica.Blocks.Sources.RealExpression psiSig(y = psi);
-  vis3d vis3d1;
-
-equation
-  // ---------------------------------------------------------------------------
-  // Rotational network connections
-  // ---------------------------------------------------------------------------
-  connect(w_com.P[1], J_inertial.p);
-  connect(w_com.P[2], mTFrot3lin.pR);
-  connect(w_com.P[3], wsensor.p);
-  connect(w_com.P[4], hinge_r.p);
-  connect(w_com.P[5], gyro.p1);
-  connect(w_com.P[6], Hmap.p1);
-  connect(Hmap.p2, gyro.p2);
-
-  // ---------------------------------------------------------------------------
-  // Body-frame COM kinematics and frame transform
-  // ---------------------------------------------------------------------------
-  connect(mTFrot3lin.pT, j0.P[1]);
-  connect(v_com_bff.P[1], j0.P[2]);
-  connect(j0.P[3], v_p_bff.P[1]);
-  connect(ground.p, v_p_bff.P[2]);
-
-  connect(v_com_bff.P[2], rTF3D.p2);
-  connect(rTF3D.p1, v_com_in.P[1]);
-
-  // Feed frame-transform angles from state variables.
-  connect(phiSig.y, rTF3D.phi);
-  connect(thetaSig.y, rTF3D.theta);
-  connect(psiSig.y, rTF3D.psi);
-  connect(phiSig.y, vis3d1.phi);
-  connect(thetaSig.y, vis3d1.theta);
-  connect(psiSig.y, vis3d1.psi);
-
-  // ---------------------------------------------------------------------------
-  // Translational dynamics connections
-  // ---------------------------------------------------------------------------
-  connect(v_com_in.P[2], mass.p);
-  connect(gravity.p, v_com_in.P[3]);
-  connect(v_com_in.P[4], v_inertial.p);
-
-  // Gravity as effort in inertial frame.
-  gravity.e0 = 0;
-  gravity.e1 = 0;
-  gravity.e2 = g;
-
-  // ---------------------------------------------------------------------------
-  // Gyroscopic modulation and H-map related equations
-  // ---------------------------------------------------------------------------
-  omega = {wsensor.f0, wsensor.f1, wsensor.f2};
-  h = J * omega;
-
-  // Skew matrix S(omega) so that S(omega)*x = omega x x.
-  S_omega = [0, -omega[3], omega[2];
-             omega[3], 0, -omega[1];
-             -omega[2], omega[1], 0];
-  gyro.m = S_omega;
-
-  // ---------------------------------------------------------------------------
-  // 3D attitude kinematics (explicit H(q) mapping form)
-  // Using ZYX convention with body rates omega -> Euler rates.
-  // Note: singular at cos(theta)=0.
-  // ---------------------------------------------------------------------------
-  euler_dot[1] = omega[1] + sin(phi) * tan(theta) * omega[2] + cos(phi) * tan(theta) * omega[3];
-  euler_dot[2] = cos(phi) * omega[2] - sin(phi) * omega[3];
-  euler_dot[3] = sin(phi) / cos(theta) * omega[2] + cos(phi) / cos(theta) * omega[3];
-
-  der(phi) = euler_dot[1];
-  der(theta) = euler_dot[2];
-  der(psi) = euler_dot[3];
-
-  // ---------------------------------------------------------------------------
-  // Position integration
-  // ---------------------------------------------------------------------------
-  der(x) = v_inertial.f0;
-  der(y) = v_inertial.f1;
-  der(z) = v_inertial.f2;
-
-  connect(xSig.y, vis3d1.x);
-  connect(ySig.y, vis3d1.y);
-  connect(zSig.y, vis3d1.z);
-
-  annotation(
-    uses(Modelica(version = "4.1.0")),
-    experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02),
-    Diagram(coordinateSystem(extent = {{-140, 160}, {200, -160}})));
-end test3d;

vis2d.mo

@@ -1,99 +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};
-  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;

vis3d.mo

@@ -1,103 +0,0 @@
-model vis3d
-  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;
-
-  Modelica.Blocks.Interfaces.RealInput psi annotation(
-    Placement(transformation(origin = {-98, -70}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 20}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Interfaces.RealInput theta annotation(
-    Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 50}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Interfaces.RealInput phi annotation(
-    Placement(transformation(origin = {-98, -14}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 80}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Interfaces.RealInput z annotation(
-    Placement(transformation(origin = {-98, 14}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -80}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Interfaces.RealInput y annotation(
-    Placement(transformation(origin = {-98, 42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -50}, extent = {{-10, -10}, {10, 10}})));
-  Modelica.Blocks.Interfaces.RealInput x annotation(
-    Placement(transformation(origin = {-98, 70}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -20}, extent = {{-10, -10}, {10, 10}})));
-protected
-  Real R[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(R, 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(R, 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, z};
-  R = [cos(psi) * cos(theta), cos(psi) * sin(theta) * sin(phi) - sin(psi) * cos(phi), cos(psi) * sin(theta) * cos(phi) + sin(psi) * sin(phi);
-       sin(psi) * cos(theta), sin(psi) * sin(theta) * sin(phi) + cos(psi) * cos(phi), sin(psi) * sin(theta) * cos(phi) - cos(psi) * sin(phi);
-       -sin(theta), cos(theta) * sin(phi), cos(theta) * cos(phi)];
-
-  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 vis3d;