Compare commits
2 Commits
75b690b19b
...
a23e051024
| Author | SHA1 | Date | |
|---|---|---|---|
| a23e051024 | |||
| 67d40f2dcd |
27
Simple2DPendulum.mo
Normal file
27
Simple2DPendulum.mo
Normal file
@@ -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;
|
||||
404
test2d.mo
404
test2d.mo
@@ -1,4 +1,107 @@
|
||||
package test2d
|
||||
|
||||
model vis2d
|
||||
import Modelica.Mechanics.MultiBody.Visualizers;
|
||||
import Modelica.Mechanics.MultiBody.Frames;
|
||||
|
||||
parameter Real L = 1;
|
||||
parameter Real w = 0.02;
|
||||
parameter Real orbRadius = 0.04;
|
||||
parameter Real axisLength = 0.25;
|
||||
parameter Real axisWidth = 0.01;
|
||||
parameter Real planeSize = 2.0;
|
||||
parameter Real planeThickness = 0.002;
|
||||
parameter Real planeOffset = -0.001;
|
||||
|
||||
Modelica.Blocks.Interfaces.RealInput phi annotation(
|
||||
Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}})));
|
||||
Modelica.Blocks.Interfaces.RealInput y annotation(
|
||||
Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}})));
|
||||
Modelica.Blocks.Interfaces.RealInput x annotation(
|
||||
Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}})));
|
||||
protected
|
||||
Real Rz[3,3];
|
||||
Real r[3];
|
||||
|
||||
Visualizers.Advanced.Shape link(
|
||||
shapeType = "cylinder",
|
||||
length = L/2,
|
||||
width = w,
|
||||
height = w,
|
||||
r_shape = {L / 2, 0, 0},
|
||||
lengthDirection = {-1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = r,
|
||||
R = Frames.from_T(Rz, zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape orb(
|
||||
shapeType = "sphere",
|
||||
length = 2 * orbRadius,
|
||||
width = 2 * orbRadius,
|
||||
height = 2 * orbRadius,
|
||||
r_shape = {0, 0, 0},
|
||||
lengthDirection = {1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = r,
|
||||
R = Frames.from_T(Rz, zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape xAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {255, 0, 0},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {axisLength / 2, 0, 0},
|
||||
lengthDirection = {1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape yAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {0, 200, 0},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {0, axisLength / 2, 0},
|
||||
lengthDirection = {0, 1, 0},
|
||||
widthDirection = {0, 0, 1},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape zAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {0, 90, 255},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {0, 0, axisLength / 2},
|
||||
lengthDirection = {0, 0, 1},
|
||||
widthDirection = {1, 0, 0},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
equation
|
||||
r = {x, y, 0};
|
||||
// Negate phi here since shapes use inverse frame-mapping
|
||||
Rz = [cos(-phi), -sin(-phi), 0;
|
||||
sin(-phi), cos(-phi), 0;
|
||||
0, 0, 1];
|
||||
|
||||
annotation(
|
||||
uses(Modelica(version = "4.1.0")),
|
||||
Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")}),
|
||||
experiment(StartTime = 0, StopTime = 1, Tolerance = 1e-06, Interval = 0.002));
|
||||
end vis2d;
|
||||
|
||||
model pendulum
|
||||
parameter Modelica.Units.SI.Mass mass;
|
||||
parameter Modelica.Units.SI.Length length;
|
||||
@@ -131,9 +234,10 @@ package test2d
|
||||
end rTF;
|
||||
|
||||
model test
|
||||
pendulum pendulum1(mass = 1, length = 1, momentOfInertia = 0.001, startAngle = 0) annotation(
|
||||
import Modelica.Constants.pi;
|
||||
pendulum pendulum1(mass = 1, length = 1, momentOfInertia = 0.001, startAngle = 1.5707963267948966) annotation(
|
||||
Placement(transformation(origin = {60, 0}, extent = {{-20, -20}, {20, 20}})));
|
||||
BondGraph.J0 j1(N = 2, s = {1, -1}) annotation(
|
||||
BondGraph.J0 j1(N = 3, s = {1, -1, 1}) annotation(
|
||||
Placement(transformation(origin = {60, 40}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.J1 w_hinge(N = 2, s = {-1, -1}) annotation(
|
||||
Placement(transformation(origin = {20, 40}, extent = {{-10, -10}, {10, 10}})));
|
||||
@@ -147,10 +251,26 @@ package test2d
|
||||
Placement(transformation(origin = {30, -40}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.J1 j3(N = 2, s = {1, -1}) annotation(
|
||||
Placement(transformation(origin = {-20, -60}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.J1 j4(N = 1, s = {-1}) annotation(
|
||||
BondGraph.J1 j4(N = 2, s = {-1, 1}) annotation(
|
||||
Placement(transformation(origin = {-20, -20}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.Sf sf(f0 = 0) annotation(
|
||||
Placement(transformation(origin = {-58, -60}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.Sf sf1(f0 = 0) annotation(
|
||||
Placement(transformation(origin = {-56, -20}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.J1 w_maxle(N = 2, s = {-1, 1}) annotation(
|
||||
Placement(transformation(origin = {60, 80}, extent = {{-10, -10}, {10, 10}})));
|
||||
BondGraph.mSe T_motor annotation(
|
||||
Placement(transformation(origin = {20, 80}, extent = {{-10, -10}, {10, 10}})));
|
||||
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));
|
||||
@@ -158,8 +278,8 @@ package test2d
|
||||
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(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(
|
||||
@@ -170,9 +290,279 @@ package test2d
|
||||
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 = 10, Tolerance = 1e-06, Interval = 0.02));
|
||||
end test;
|
||||
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")));
|
||||
end test2d;
|
||||
100
vis2d.mo
100
vis2d.mo
@@ -1,100 +0,0 @@
|
||||
model vis2d
|
||||
import Modelica.Mechanics.MultiBody.Visualizers;
|
||||
import Modelica.Mechanics.MultiBody.Frames;
|
||||
|
||||
parameter Real L = 1;
|
||||
parameter Real w = 0.02;
|
||||
parameter Real orbRadius = 0.04;
|
||||
parameter Real axisLength = 0.25;
|
||||
parameter Real axisWidth = 0.01;
|
||||
parameter Real planeSize = 2.0;
|
||||
parameter Real planeThickness = 0.002;
|
||||
parameter Real planeOffset = -0.001;
|
||||
|
||||
Modelica.Blocks.Interfaces.RealInput phi annotation(
|
||||
Placement(transformation(origin = {-98, -42}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 60}, extent = {{-10, -10}, {10, 10}})));
|
||||
Modelica.Blocks.Interfaces.RealInput y annotation(
|
||||
Placement(transformation(origin = {-98, 0}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, -60}, extent = {{-10, -10}, {10, 10}})));
|
||||
Modelica.Blocks.Interfaces.RealInput x annotation(
|
||||
Placement(transformation(origin = {-98, 40}, extent = {{-20, -20}, {20, 20}}), iconTransformation(origin = {-94, 0}, extent = {{-10, -10}, {10, 10}})));
|
||||
protected
|
||||
Real Rz[3,3];
|
||||
Real r[3];
|
||||
|
||||
Visualizers.Advanced.Shape link(
|
||||
shapeType = "cylinder",
|
||||
length = L,
|
||||
width = w,
|
||||
height = w,
|
||||
r_shape = {-L / 2, 0, 0},
|
||||
lengthDirection = {1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = r,
|
||||
R = Frames.from_T(Rz, zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape orb(
|
||||
shapeType = "sphere",
|
||||
length = 2 * orbRadius,
|
||||
width = 2 * orbRadius,
|
||||
height = 2 * orbRadius,
|
||||
r_shape = {L / 2 - orbRadius, 0, 0},
|
||||
lengthDirection = {1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = r,
|
||||
R = Frames.from_T(Rz, zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape xAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {255, 0, 0},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {axisLength / 2, 0, 0},
|
||||
lengthDirection = {1, 0, 0},
|
||||
widthDirection = {0, 1, 0},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape yAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {0, 200, 0},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {0, axisLength / 2, 0},
|
||||
lengthDirection = {0, 1, 0},
|
||||
widthDirection = {0, 0, 1},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
|
||||
Visualizers.Advanced.Shape zAxis(
|
||||
shapeType = "cylinder",
|
||||
length = axisLength,
|
||||
width = axisWidth,
|
||||
height = axisWidth,
|
||||
color = {0, 90, 255},
|
||||
specularCoefficient = 0.1,
|
||||
r_shape = {0, 0, axisLength / 2},
|
||||
lengthDirection = {0, 0, 1},
|
||||
widthDirection = {1, 0, 0},
|
||||
r = zeros(3),
|
||||
R = Frames.from_T([1, 0, 0;
|
||||
0, 1, 0;
|
||||
0, 0, 1], zeros(3)));
|
||||
equation
|
||||
r = {x, y, 0};
|
||||
// Negate phi here since shapes use inverse frame-mapping
|
||||
Rz = [cos(-phi), -sin(-phi), 0;
|
||||
sin(-phi), cos(-phi), 0;
|
||||
0, 0, 1];
|
||||
|
||||
annotation(
|
||||
uses(Modelica(version = "4.1.0")),
|
||||
Icon(graphics = {Rectangle(lineColor = {204, 204, 204}, fillColor = {255, 255, 255}, fillPattern = FillPattern.Sphere, extent = {{-100, 100}, {100, -100}}, radius = 10), Text(extent = {{-100, 100}, {100, -100}}, textString = "VIS")}));
|
||||
end vis2d;
|
||||
Reference in New Issue
Block a user