/
ControlledCraneCrab.mo
105 lines (103 loc) · 4.08 KB
/
ControlledCraneCrab.mo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
within PlanarMechanics.Examples;
model ControlledCraneCrab
extends Modelica.Icons.Example;
Parts.Body body(
m=70,
I=0) annotation (Placement(transformation(
extent={{-10,-10},{10,10}},
rotation=270,
origin={-10,-70})));
Parts.FixedTranslation fixedTranslation(r={0,2.5})
annotation (
Placement(transformation(
extent={{-10,-10},{10,10}},
rotation=270,
origin={-10,-30})));
Parts.Fixed fixed annotation (Placement(transformation(
extent={{-10,-10},{10,10}},
rotation=180,
origin={-70,30})));
Parts.Body body1(
m=250,
I=0,
a(start={1,0}))
annotation (Placement(transformation(extent={{0,20},{20,40}})));
Modelica.Mechanics.Rotational.Sensors.AngleSensor angleSensor
annotation (Placement(transformation(extent={{20,-10},{40,10}})));
Modelica.Mechanics.Translational.Sources.Force force annotation (
Placement(transformation(
extent={{-10,-10},{10,10}},
rotation=270,
origin={-30,60})));
Modelica.Blocks.Continuous.PID PID(
Td=0.2,
k=-320*9.81*5,
initType=Modelica.Blocks.Types.InitPID.InitialState,
xi_start=0,
Ti=1e9) annotation (Placement(transformation(extent={{40,70},{20,90}})));
inner PlanarWorld planarWorld
annotation (Placement(transformation(extent={{-80,-20},{-60,0}})));
Joints.Prismatic prismatic(r={1,0}, useFlange=true,
s(fixed=true),
v(fixed=true))
annotation (Placement(transformation(extent={{-40,20},{-20,40}})));
Joints.Revolute revolute(useFlange=true,
w(fixed=true),
phi(fixed=true, start=-0.34906585039887))
annotation (Placement(transformation(
extent={{-10,-10},{10,10}},
rotation=270,
origin={-10,0})));
equation
connect(fixedTranslation.frame_b, body.frame_a) annotation (Line(
points={{-10,-40},{-10,-45},{-10,-45},{-10,-50},{-10,-60},{-10,-60}},
color={95,95,95},
thickness=0.5));
connect(force.f, PID.y) annotation (Line(
points={{-30,72},{-30,80},{19,80}},
color={0,0,127}));
connect(angleSensor.phi, PID.u) annotation (Line(
points={{41,0},{70,0},{70,80},{42,80}},
color={0,0,127}));
connect(fixed.frame_a, prismatic.frame_a) annotation (Line(
points={{-60,30},{-40,30}},
color={95,95,95},
thickness=0.5));
connect(prismatic.frame_b, body1.frame_a) annotation (Line(
points={{-20,30},{0,30}},
color={95,95,95},
thickness=0.5));
connect(prismatic.flange_a, force.flange) annotation (Line(
points={{-30,39},{-30,50}},
color={0,127,0}));
connect(prismatic.frame_b, revolute.frame_a) annotation (Line(
points={{-20,30},{-10,30},{-10,10}},
color={95,95,95},
thickness=0.5));
connect(revolute.frame_b, fixedTranslation.frame_a) annotation (Line(
points={{-10,-10},{-10,-20}},
color={95,95,95},
thickness=0.5));
connect(revolute.flange_a, angleSensor.flange) annotation (Line(
points={{0,0},{10,0},{10,0},{20,0}}));
annotation (experiment(StopTime=3),
Documentation(revisions="<html><p><img src=\"modelica://PlanarMechanics/Resources/Images/dlr_logo.png\"/> <b>Developed 2010-2014 at the DLR Institute of System Dynamics and Control</b></p></html>", info="<html>
<h4><font color=\"#008000\">A controlled crane crab.</font></h4>
<p>A simple PID (actually PD) controlles the pendulum into upright position.</p>
<p><br/><img src=\"modelica://PlanarMechanics/Resources/Images/ControlledCraneCrab_1.png\"/></p>
<p><img src=\"modelica://PlanarMechanics/Resources/Images/ControlledCraneCrab_2.png\"/></p>
<p><br/>SELECTED CONTINUOUS TIME STATES</p>
<p> actuatedPrismatic.s</p>
<p> actuatedPrismatic.v</p>
<p> actuatedRevolute.phi</p>
<p> actuatedRevolute.w</p>
<p> PID.D.x</p>
<p> PID.I.y</p>
<p> </p>
</html>",
revisions="<html>
<p>(c) Copyright by Dirk Zimmer</p>
<p>The library was created and is owned by Dr. Dirk Zimmer.</p>
<p>dirk.zimmer@dlr.de</p>
</html>"));
end ControlledCraneCrab;