From 741265aad49ce78c41459b891cee74ac16de0d83 Mon Sep 17 00:00:00 2001
From: Wouter Horlings <wouter@horlin.gs>
Date: Tue, 14 Jul 2020 18:34:30 +0200
Subject: [PATCH] fixup! fixup! fixup! Add physics model

---
 .../SCARA/02_physics/scara_physics.emx        | 768 ++++++++++++++----
 1 file changed, 601 insertions(+), 167 deletions(-)

diff --git a/implementation/SCARA/02_physics/scara_physics.emx b/implementation/SCARA/02_physics/scara_physics.emx
index 0745ec1..6bdf850 100644
--- a/implementation/SCARA/02_physics/scara_physics.emx
+++ b/implementation/SCARA/02_physics/scara_physics.emx
@@ -8,7 +8,7 @@
 <IsMainModel>0</IsMainModel>
   <KeepParameterValues>False</KeepParameterValues>
     <LibraryPath>C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\02_physics\scara_physics.emx</LibraryPath>
-  <TimeStamp>2020-7-14 17:29:22</TimeStamp>
+  <TimeStamp>2020-7-14 18:34:19</TimeStamp>
 </Description>
  <Attributes>
   <GlobalRelations>parameters
@@ -30,7 +30,7 @@ initialequations
  end;
  implementation bg
   submodels
-   COM1 304 520
+   COM1 424 584
     description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel></Description><Attributes><GlobalRelations>parameters
 	real mass = 1 {kg};</GlobalRelations></Attributes></Information>';
     type Submodel
@@ -40,9 +40,9 @@ initialequations
     end;
     icon bg ellipse bottom
      figures
-      ellipse 296 512 312 528 color 0 fill 15132390;
-      line 304 512 304 528 color 0 fill 15132390;
-      line 296 520 312 520 color 0 fill 15132390;
+      ellipse 416 576 432 592 color 0 fill 15132390;
+      line 424 576 424 592 color 0 fill 15132390;
+      line 416 584 432 584 color 0 fill 15132390;
     end;
     implementation bg
      submodels
@@ -168,7 +168,7 @@ implementation_end;
       Splitter1\output -> I\mass;
      end;
     implementation_end;
-   COM2 704 520
+   COM2 824 584
     description '<Information><Description><Version>4.8</Version><IsMainModel>1</IsMainModel></Description><Attributes><GlobalRelations>parameters
 	real mass = 1 {kg};</GlobalRelations></Attributes></Information>';
     type Submodel
@@ -178,9 +178,9 @@ implementation_end;
     end;
     icon bg ellipse
      figures
-      ellipse 696 512 712 528 color 0 fill 15132390;
-      line 704 512 704 528 color 0 fill 15132390;
-      line 696 520 712 520 color 0 fill 15132390;
+      ellipse 816 576 832 592 color 0 fill 15132390;
+      line 824 576 824 592 color 0 fill 15132390;
+      line 816 584 832 584 color 0 fill 15132390;
     end;
     implementation bg
      submodels
@@ -306,29 +306,7 @@ implementation_end;
       Splitter1\output -> I\mass;
      end;
     implementation_end;
-   Constant 80 96
-    description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
-   <LibraryPath>Signal\Sources\Constant.emx</LibraryPath>
-  <TimeStamp>2007-10-19 14:48:44</TimeStamp>
-</Description>';
-    type Constant
-     ports
-      signal out output;
-    end;
-    icon bg bottom
-     figures
-      rectangle 64.1 80 95.9 112 color 0 fill 15132390;
-      line 68.9 91.6 90.9 91.6 color 16711680 width 2;
-      line 66.2 102.7 91.9 102.7 color 0;
-      line 68.5 86.7 68.5 105 color 0;
-    end;
-    implementation eq
-parameters
-	real C = 0.0;		// output value
-equations
-	output = C;
-     implementation_end;
-   I 312 112
+   I 432 176
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -348,14 +326,14 @@ equations
     end;
     icon bg bottom
      figures
-      text 'I' 312 112 color 0 18 bold;
+      text 'I' 432 176 color 0 18 bold;
     end;
     implementation eq
 equations
     state = int(p.e);
     p.f = state / i;
 implementation_end;
-   I1 704 112
+   I1 824 176
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -375,14 +353,58 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'I' 704 112 color 0 18 bold;
+      text 'I' 824 176 color 0 18 bold;
     end;
     implementation eq
 equations
     state = int(p.e);
     p.f = state / i;
 implementation_end;
-   MSe 504 96
+   inverse_kinematics 312 32
+    description '<Information>
+ <Description>
+    <Version>4.8</Version>
+  <LibraryPath>SCARA\inverse_kinematics_v1.emx</LibraryPath>
+<IsMainModel>1</IsMainModel>
+  <KeepParameterValues>False</KeepParameterValues>
+    <TimeStamp>2020-7-10 12:26:18</TimeStamp>
+<AllowLibraryUpdate>False</AllowLibraryUpdate>
+ </Description>
+</Information>';
+    type 'Submodel-Equation'
+     ports
+      signal in input [2,1] {m} ;
+      signal out angle1 {rad} ;
+      signal out angle2 {rad} ;
+      signal out a {rad} ;
+      signal out b {rad} ;
+      signal out c {rad} ;
+      signal out phi {rad} ;
+    end;
+    implementation eq
+parameters
+	real A_length = 0.065 {m}; // length of first arm
+	real B_length = 0.05 {m}; // length of second arm
+	real to_rad = 1 {rad};
+variables
+	real x {m}, y {m};
+	real C_length {m}; // length to x and y.
+	real angle_sum {rad}; //  test angle
+equations
+	x = input[1];
+	y = input[2];
+	phi = atan2(y, x);
+	C_length = sqrt(x^2 + y^2);	
+ 	a = arccos ((B_length^2 + C_length^2 - A_length^2) / (2 * B_length * C_length));
+	b = arccos ((A_length^2 + C_length^2 - B_length^2) / (2 * A_length * C_length));
+	c = arccos ((A_length^2 + B_length^2 - C_length^2) / (2 * A_length * B_length));
+	angle1 = b + phi;
+	angle2 =  angle1 - pi * to_rad + c;
+	
+	
+
+ implementation_end;
+   MSe1 624 192
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -401,7 +423,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'MSe' 504 96 color 0 18 bold;
+      text 'MSe' 624 192 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -410,7 +432,7 @@ equations
 	p.e = effort;
 	flow = p.f;
 implementation_end;
-   MSe1 168 168
+   MSe2 248 176
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -429,7 +451,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'MSe' 168 168 color 0 18 bold;
+      text 'MSe' 248 176 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -438,7 +460,7 @@ equations
 	p.e = effort;
 	flow = p.f;
 implementation_end;
-   MXTF 232.3 224
+   MXTF 352.3 288
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -461,7 +483,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'MXTF' 232.3 224 color 0 18 bold;
+      text 'MXTF' 352.3 288 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -470,7 +492,7 @@ equations
 	r = [-p[2];p[1]];
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;implementation_end;
-   OneJunction 304 304
+   OneJunction 424 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -489,7 +511,7 @@ equations
     end;
     icon bg
      figures
-      text '1' 304 304 color 0 18 bold;
+      text '1' 424 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -497,7 +519,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction1 304 456
+   OneJunction1 424 520
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -516,7 +538,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 304 456 color 0 18 bold;
+      text '1' 424 520 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -524,7 +546,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction10 704 168
+   OneJunction10 824 232
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -543,7 +565,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 704 168 color 0 18 bold;
+      text '1' 824 232 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -551,7 +573,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction11 864 448
+   OneJunction11 984 512
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -570,7 +592,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 864 448 color 0 18 bold;
+      text '1' 984 512 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -578,7 +600,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction12 584 304
+   OneJunction12 704 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -597,7 +619,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 584 304 color 0 18 bold;
+      text '1' 704 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -605,7 +627,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction3 312 168
+   OneJunction3 432 232
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -624,7 +646,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 312 168 color 0 18 bold;
+      text '1' 432 232 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -632,7 +654,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction5 152.3 304
+   OneJunction5 272.3 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -651,7 +673,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 152.3 304 color 0 18 bold;
+      text '1' 272.3 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -659,7 +681,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction6 152 456
+   OneJunction6 272 520
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -678,7 +700,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 152 456 color 0 18 bold;
+      text '1' 272 520 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -686,7 +708,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction7 704 304
+   OneJunction7 824 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -705,7 +727,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 704 304 color 0 18 bold;
+      text '1' 824 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -713,7 +735,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction8 704 456
+   OneJunction8 824 520
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -732,7 +754,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 704 456 color 0 18 bold;
+      text '1' 824 520 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -740,7 +762,7 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   OneJunction9 864 304
+   OneJunction9 984 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -759,7 +781,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '1' 864 304 color 0 18 bold;
+      text '1' 984 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -767,7 +789,77 @@ equations
 	equal (collect (p.f));
 	flow = first (p.f);
 implementation_end;
-   PlusMinus1 512 336
+   PID 248 112
+    description '<Description>
+ <Version>4.0</Version>
+ <IsMainModel>1</IsMainModel>
+ <KeepParameterValues>False</KeepParameterValues>
+   <LibraryPath>Signal\Control\PID Control\Continuous\PID.emx</LibraryPath>
+  <TimeStamp>2008-1-17 10:49:30</TimeStamp>
+</Description>';
+    type PID
+     ports
+      signal in error;
+      signal out output;
+    end;
+    icon bg bottom
+     figures
+      rectangle 232 96 264 128 color 0 fill 15132390;
+      text 'PID' 248 111.5 color 16711680 18 bold;
+    end;
+    implementation eq
+parameters
+	real kp = 0.05 {};			// Proportional gain
+	real tauD = 1.0 {s};		// Derivative time constant: tauD > 0
+	real beta = 0.001 {};		// Tameness constant: 0 < beta << 1
+	real tauI = 1.0 {s};		// Integral time constant: tauI > 0
+variables
+	real pdout, pdrate, pdstate;
+	real pirate, pistate;
+equations
+	pdrate = (kp * error - pdout) / (beta * tauD);
+	pdstate = int (pdrate);
+	pdout = pdstate + (kp * error / beta);
+	pirate = pdout / tauI;
+	pistate = int (pirate);
+	output = pistate + pdout;
+      implementation_end;
+   PID1 624 136
+    description '<Description>
+ <Version>4.0</Version>
+ <IsMainModel>1</IsMainModel>
+ <KeepParameterValues>False</KeepParameterValues>
+   <LibraryPath>Signal\Control\PID Control\Continuous\PID.emx</LibraryPath>
+  <TimeStamp>2008-1-17 10:49:30</TimeStamp>
+</Description>';
+    type PID
+     ports
+      signal in error;
+      signal out output;
+    end;
+    icon bg bottom
+     figures
+      rectangle 608 120 640 152 color 0 fill 15132390;
+      text 'PID' 624 135.5 color 16711680 18 bold;
+    end;
+    implementation eq
+parameters
+	real kp = 0.02 {};			// Proportional gain
+	real tauD = 1.0 {s};		// Derivative time constant: tauD > 0
+	real beta = 0.001 {};		// Tameness constant: 0 < beta << 1
+	real tauI = 0.05 {s};		// Integral time constant: tauI > 0
+variables
+	real pdout, pdrate, pdstate;
+	real pirate, pistate;
+equations
+	pdrate = (kp * error - pdout) / (beta * tauD);
+	pdstate = int (pdrate);
+	pdout = pdstate + (kp * error / beta);
+	pirate = pdout / tauI;
+	pistate = int (pirate);
+	output = pistate + pdout;
+      implementation_end;
+   PlusMinus1 632 400
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Signal\Block Diagram\PlusMinus.emx</LibraryPath>
   <TimeStamp>2007-9-27 10:15:13</TimeStamp>
@@ -780,13 +872,51 @@ implementation_end;
     end;
     icon bg ellipse
      figures
-      ellipse 504 328 520 344 color 0 fill 16777215;
+      ellipse 624 392 640 408 color 0 fill 16777215;
     end;
     implementation eq
 equations
     output = sum (collect (plus)) - sum (collect (minus));
     implementation_end;
-   R 368 120
+   PlusMinus2 304 112
+    description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
+   <LibraryPath>Signal\Block Diagram\PlusMinus.emx</LibraryPath>
+  <TimeStamp>2007-9-27 10:15:13</TimeStamp>
+</Description>';
+    knot PlusMinus
+     ports
+      signal knot duplicatable in plus [1];
+      signal knot duplicatable in minus [1];
+      signal knot out output [1];
+    end;
+    icon bg ellipse
+     figures
+      ellipse 296 104 312 120 color 0 fill 16777215;
+    end;
+    implementation eq
+equations
+    output = sum (collect (plus)) - sum (collect (minus));
+    implementation_end;
+   PlusMinus3 568 136
+    description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
+   <LibraryPath>Signal\Block Diagram\PlusMinus.emx</LibraryPath>
+  <TimeStamp>2007-9-27 10:15:13</TimeStamp>
+</Description>';
+    knot PlusMinus
+     ports
+      signal knot duplicatable in plus [1];
+      signal knot duplicatable in minus [1];
+      signal knot out output [1];
+    end;
+    icon bg ellipse
+     figures
+      ellipse 560 128 576 144 color 0 fill 16777215;
+    end;
+    implementation eq
+equations
+    output = sum (collect (plus)) - sum (collect (minus));
+    implementation_end;
+   R1 624 272
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -802,15 +932,15 @@ equations
     end;
     icon bg bottom
      figures
-      text 'R' 368 120 color 0 18 bold;
+      text 'R' 624 272 color 0 18 bold;
     end;
     implementation eq
 parameters
-	real r = 0.00000001;
+	real r = 0.001;
 equations
 	p.e = r * p.f;
 implementation_end;
-   R1 504 216
+   R2 248 280
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -826,15 +956,54 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'R' 504 216 color 0 18 bold;
+      text 'R' 248 280 color 0 18 bold;
     end;
     implementation eq
 parameters
-	real r = 0.00001;
+	real r = 0.001;
 equations
 	p.e = r * p.f;
 implementation_end;
-   RTF 304 392
+   rectanglepath 64 32
+    description '<Information>
+ <Description>
+    <Version>4.8</Version>
+  <LibraryPath>setpoint\rectanglepath.emx</LibraryPath>
+<IsMainModel>1</IsMainModel>
+  <KeepParameterValues>False</KeepParameterValues>
+    <TimeStamp>2020-7-10 12:30:38</TimeStamp>
+<AllowLibraryUpdate>False</AllowLibraryUpdate>
+ </Description>
+</Information>';
+    type 'Submodel-Equation'
+     ports
+      signal out output [2,1];
+    end;
+    implementation eq
+/* 
+This will generate coordinates for the box that has to be drawn
+*/
+parameters
+	real w = 0.07{m} ;
+	real h = 0.05 {m};
+	real t  = 1 {s};
+	real origin[2] = [0.01,0.03]{m};
+variables
+	real v {m/s};
+	real t_w {s};
+	real t_h {s};
+	real period {s};
+	real test1, test2, test3;
+initialequations
+	v = (2*w + 2*h)/t;
+	t_w = w / v;
+	t_h = h / v;
+equations
+	period = floor(time / t) * t;
+	output[1] = v*(ramp(period+t_h) - ramp(period + t_w+t_h) - ramp(period + t_w + t_h + t_h)) + origin[1];
+	output[2] = v*(ramp(period) - ramp(period + t_h) - ramp(period + t_h + t_w) + ramp(period + 2 * t_h + t_w))+ origin[2];
+implementation_end;
+   RTF 424 456
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -854,7 +1023,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'RTF' 304 392 color 0 18 bold;
+      text 'RTF' 424 456 color 0 18 bold;
     end;
     implementation eq
 /*
@@ -867,7 +1036,7 @@ equations
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;
 implementation_end;
-   RTF1 512 304
+   RTF1 632 368
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -887,7 +1056,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'RTF' 512 304 color 0 18 bold;
+      text 'RTF' 632 368 color 0 18 bold;
     end;
     implementation eq
 /*
@@ -900,7 +1069,7 @@ equations
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;
 implementation_end;
-   RTF2 152 368
+   RTF2 272 432
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -920,7 +1089,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'RTF' 152 368 color 0 18 bold;
+      text 'RTF' 272 432 color 0 18 bold;
     end;
     implementation eq
 /*
@@ -933,7 +1102,7 @@ equations
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;
 implementation_end;
-   RTF3 704 368
+   RTF3 824 432
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -953,7 +1122,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'RTF' 704 368 color 0 18 bold;
+      text 'RTF' 824 432 color 0 18 bold;
     end;
     implementation eq
 /*
@@ -966,7 +1135,7 @@ equations
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;
 implementation_end;
-   RTF4 864 360
+   RTF4 984 424
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -986,7 +1155,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'RTF' 864 360 color 0 18 bold;
+      text 'RTF' 984 424 color 0 18 bold;
     end;
     implementation eq
 /*
@@ -999,7 +1168,7 @@ equations
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;
 implementation_end;
-   Sf 72 456
+   Sf 192 520
     description '<Description><Version>4.0</Version><IsMainModel>1</IsMainModel><KeepParameterValues>False</KeepParameterValues>
    <LibraryPath>Bond Graph\2D\Sf-2.emx</LibraryPath>
   <TimeStamp>2007-9-25 12:7:5</TimeStamp>
@@ -1012,7 +1181,7 @@ implementation_end;
     end;
     icon bg bottom
      figures
-      text 'Sf' 72 456 color 0 18 bold;
+      text 'Sf' 192 520 color 0 18 bold;
     end;
     implementation eq
 parameters
@@ -1023,7 +1192,36 @@ equations
 	p.f = flow;
 	effort = p.e;
 implementation_end;
-   Splitter1 264 336
+   Sf1 152 232
+    description '<Information>
+ <Description>
+    <Version>4.2</Version>
+<IsMainModel>1</IsMainModel>
+  <KeepParameterValues>False</KeepParameterValues>
+    <LibraryPath>Bond Graph\Sf.emx</LibraryPath>
+  <TimeStamp>2011-11-29 16:38:03</TimeStamp>
+</Description>
+</Information>';
+    type Sf
+     ports
+      power out p;
+     restrictions
+      causality fixed in p;
+    end;
+    icon bg bottom
+     figures
+      text 'Sf' 152 232 color 0 18 bold;
+    end;
+    implementation eq
+parameters
+	real flow = 0;
+variables
+	real effort;
+equations
+	p.f = flow;
+	effort = p.e;
+implementation_end;
+   Splitter1 384 400
     description '<Description><Version>4.0</Version>
    <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
   <TimeStamp>2008-01-17 11:28:29</TimeStamp>
@@ -1037,16 +1235,16 @@ implementation_end;
     end;
     icon bg ellipse
      figures
-      ellipse 260.8 332.8 267.2 339.2 color -1 fill 0;
-      ellipse 259.7 331.7 268.3 340.3 color -1;
+      ellipse 380.8 396.8 387.2 403.2 color -1 fill 0;
+      ellipse 379.7 395.7 388.3 404.3 color -1;
      terminals
-      input 264 336 fixed;
+      input 384 400 fixed;
     end;
     implementation eq
 equations
     collect (output) = input;
 implementation_end;
-   Splitter2 680 336
+   Splitter2 800 400
     description '<Description><Version>4.0</Version>
    <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
   <TimeStamp>2008-01-17 11:28:29</TimeStamp>
@@ -1060,16 +1258,16 @@ implementation_end;
     end;
     icon bg ellipse
      figures
-      ellipse 676.8 332.8 683.2 339.2 color -1 fill 0;
-      ellipse 675.7 331.7 684.3 340.3 color -1;
+      ellipse 796.8 396.8 803.2 403.2 color -1 fill 0;
+      ellipse 795.7 395.7 804.3 404.3 color -1;
      terminals
-      input 680 336 fixed;
+      input 800 400 fixed;
     end;
     implementation eq
 equations
     collect (output) = input;
 implementation_end;
-   Splitter3 168 96
+   Splitter3 304 400
     description '<Description><Version>4.0</Version>
    <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
   <TimeStamp>2008-01-17 11:28:29</TimeStamp>
@@ -1083,16 +1281,39 @@ implementation_end;
     end;
     icon bg ellipse
      figures
-      ellipse 164.8 92.8 171.2 99.2 color -1 fill 0;
-      ellipse 163.7 91.7 172.3 100.3 color -1;
+      ellipse 300.8 396.8 307.2 403.2 color -1 fill 0;
+      ellipse 299.7 395.7 308.3 404.3 color -1;
      terminals
-      input 168 96 fixed;
+      input 304 400 fixed;
     end;
     implementation eq
 equations
     collect (output) = input;
 implementation_end;
-   Submodel3 224 416
+   Splitter4 704 400
+    description '<Description><Version>4.0</Version>
+   <LibraryPath>Signal\Block Diagram\Splitter.emx</LibraryPath>
+  <TimeStamp>2008-01-17 11:28:29</TimeStamp>
+<IsMainModel>1</IsMainModel>
+ <KeepParameterValues>False</KeepParameterValues>
+</Description>';
+    knot Splitter
+     ports
+      signal knot duplicatable out output [1];
+      signal knot in input [1];
+    end;
+    icon bg ellipse
+     figures
+      ellipse 700.8 396.8 707.2 403.2 color -1 fill 0;
+      ellipse 699.7 395.7 708.3 404.3 color -1;
+     terminals
+      input 704 400 fixed;
+    end;
+    implementation eq
+equations
+    collect (output) = input;
+implementation_end;
+   Submodel3 344 480
     description '<Description><Version>4.0</Version>
    <LibraryPath>Template\Submodel-Equation.emx</LibraryPath>
 <IsMainModel>1</IsMainModel>
@@ -1119,7 +1340,7 @@ parameters
 	real density = 1400 {kg/m3};
 	real dimension[3] = [0.005;0.01;0.065] {m};
 	real initial_position[2] = [0;0.0325] {m};
-	real initial_angle = 0.00001 {rad};
+	real initial_angle = 0 {rad};
 variables
 	real body_angle[3];
 	real body_position[3];
@@ -1140,7 +1361,7 @@ equations
 	joint2=[0;dimension[3]/2];
 	
  implementation_end;
-   Submodel4 624 416
+   Submodel4 744 480
     description '<Description><Version>4.0</Version>
    <LibraryPath>Template\Submodel-Equation.emx</LibraryPath>
 <IsMainModel>1</IsMainModel>
@@ -1161,8 +1382,8 @@ equations
     end;
     icon bg
      figures
-      rectangle 584 400 664 432 color 0 fill 15132390;
-      text 'name' 624 416 color 0 16;
+      rectangle 704 464 784 496 color 0 fill 15132390;
+      text 'name' 744 480 color 0 16;
     end;
     implementation eq
 /* Equation Submodel
@@ -1193,7 +1414,7 @@ equations
 	joint2=[0;dimension[3]/2];
 	
  implementation_end;
-   TF 384 224
+   TF 504 288
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1216,7 +1437,7 @@ equations
     end;
     icon bg bottom
      figures
-      text 'MXTF' 384 224 color 0 18 bold;
+      text 'MXTF' 504 288 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -1225,7 +1446,7 @@ equations
 	r = [-p[2];p[1]];
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;implementation_end;
-   TF2 784 224
+   TF2 904 288
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1248,7 +1469,7 @@ equations
     end;
     icon bg bottom
      figures
-      text 'MXTF' 784 224 color 0 18 bold;
+      text 'MXTF' 904 288 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -1257,7 +1478,7 @@ equations
 	r = [-p[2];p[1]];
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;implementation_end;
-   TF3 632.3 224
+   TF3 808 288
     description '<Information>
  <Description>
     <Version>4.8</Version>
@@ -1280,7 +1501,7 @@ equations
     end;
     icon bg bottom
      figures
-      text 'MXTF' 632.3 224 color 0 18 bold;
+      text 'MXTF' 808 288 color 0 18 bold;
     end;
     implementation eq
 variables
@@ -1289,7 +1510,7 @@ equations
 	r = [-p[2];p[1]];
 	p1.e = transpose (r) * p2.e;
 	p2.f = r * p1.f;implementation_end;
-   ZeroJunction 384 304
+   ZeroJunction 504 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1308,7 +1529,7 @@ equations
     end;
     icon bg
      figures
-      text '0' 384 304 color 0 18 bold;
+      text '0' 504 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1316,7 +1537,7 @@ equations
 	equal (collect (p.e));
 	effort = first (p.e);
 implementation_end;
-   ZeroJunction1 232.3 304
+   ZeroJunction1 352.3 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1335,7 +1556,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '0' 232.3 304 color 0 18 bold;
+      text '0' 352.3 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1343,7 +1564,7 @@ equations
 	equal (collect (p.e));
 	effort = first (p.e);
 implementation_end;
-   ZeroJunction2 784 304
+   ZeroJunction2 904 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1362,7 +1583,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '0' 784 304 color 0 18 bold;
+      text '0' 904 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1370,7 +1591,7 @@ equations
 	equal (collect (p.e));
 	effort = first (p.e);
 implementation_end;
-   ZeroJunction3 632.3 304
+   ZeroJunction3 752.3 368
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1389,7 +1610,7 @@ implementation_end;
     end;
     icon bg
      figures
-      text '0' 632.3 304 color 0 18 bold;
+      text '0' 752.3 368 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1397,7 +1618,7 @@ equations
 	equal (collect (p.e));
 	effort = first (p.e);
 implementation_end;
-   ZeroJunction4 504 168
+   ZeroJunction4 624 232
     description '<Information>
  <Description>
     <Version>4.2</Version>
@@ -1416,7 +1637,34 @@ implementation_end;
     end;
     icon bg
      figures
-      text '0' 504 168 color 0 18 bold;
+      text '0' 624 232 color 0 18 bold;
+    end;
+    implementation eq
+equations
+	sum (direct (p.f)) = 0;
+	equal (collect (p.e));
+	effort = first (p.e);
+implementation_end;
+   ZeroJunction5 248 232
+    description '<Information>
+ <Description>
+    <Version>4.2</Version>
+<IsMainModel>1</IsMainModel>
+  <KeepParameterValues>False</KeepParameterValues>
+    <LibraryPath>Bond Graph\ZeroJunction.emx</LibraryPath>
+  <TimeStamp>2011-11-29 16:45:16</TimeStamp>
+</Description>
+</Information>';
+    knot ZeroJunction
+     ports
+      power knot duplicatable none p [1];
+      signal knot out effort [1];
+     restrictions
+      causality constraint one_in p;
+    end;
+    icon bg
+     figures
+      text '0' 248 232 color 0 18 bold;
     end;
     implementation eq
 equations
@@ -1426,56 +1674,64 @@ equations
 implementation_end;
   end;
   connections
-   Constant\output -> Splitter3\input;
    I\p <= OneJunction3\p;
    I1\p <= OneJunction10\p;
-   MSe1\p => OneJunction3\p;
+   inverse_kinematics\angle1 -> PlusMinus2\plus;
+   inverse_kinematics\angle2 -> PlusMinus3\plus;
+   inverse_kinematics\input <- rectanglepath\output;
+   MSe1\effort <- PID1\output;
+   MSe1\p => ZeroJunction4\p;
+   MSe2\effort <- PID\output;
    MXTF\p2 => ZeroJunction1\p;
    OneJunction\p => RTF\p1;
    OneJunction\p => ZeroJunction\p;
    OneJunction\p => ZeroJunction1\p;
-   OneJunction1\flow -> Submodel3\velocity 224 456;
+   OneJunction1\flow -> Submodel3\velocity 344 520;
    OneJunction1\p => COM1\p;
-   OneJunction10\flow -> Submodel4\angle_velocity 608 224 608 360;
+   OneJunction10\flow -> Submodel4\angle_velocity 728 288 728 424;
    OneJunction10\p => TF2\p1;
    OneJunction10\p => TF3\p1;
-   OneJunction3\flow -> Submodel3\angle_velocity 208 200 208 376;
+   OneJunction3\flow -> Submodel3\angle_velocity 328 264 328 440;
    OneJunction3\p => MXTF\p1;
-   OneJunction3\p => R\p;
    OneJunction3\p => TF\p1;
    OneJunction3\p => ZeroJunction4\p;
    OneJunction5\p => RTF2\p1;
    OneJunction7\p => RTF3\p1;
    OneJunction7\p => ZeroJunction2\p;
    OneJunction7\p => ZeroJunction3\p;
-   OneJunction8\flow -> Submodel4\velocity 624 456;
+   OneJunction8\flow -> Submodel4\velocity 744 520;
    OneJunction8\p => COM2\p;
    OneJunction9\p => RTF4\p1;
    PlusMinus1\output -> RTF1\angle;
+   PlusMinus2\output -> PID\error;
+   PlusMinus3\output -> PID1\error;
    RTF\p2 => OneJunction1\p;
    RTF1\p2 => OneJunction12\p;
    RTF2\p2 => OneJunction6\p;
    RTF3\p2 => OneJunction8\p;
    RTF4\p2 => OneJunction11\p;
    Sf\p => OneJunction6\p;
+   Sf1\p => ZeroJunction5\p;
    Splitter1\output -> PlusMinus1\plus;
-   Splitter1\output -> RTF\angle 280 352 280 368;
-   Splitter1\output -> RTF2\angle 176 336;
-   Splitter2\output -> PlusMinus1\minus;
-   Splitter2\output -> RTF3\angle 680 344;
-   Splitter2\output -> RTF4\angle 832 336;
-   Splitter3\output -> MSe\effort;
-   Splitter3\output -> MSe1\effort;
-   Submodel3\angle -> Splitter1\input 248 384 248 352;
-   Submodel3\angular_momentum -> I\i 200 376 200 200;
-   Submodel3\joint1 -> MXTF\p 272 256;
+   Splitter1\output -> RTF\angle 400 416 400 432;
+   Splitter1\output -> Splitter3\input;
+   Splitter2\output -> RTF3\angle 800 408;
+   Splitter2\output -> RTF4\angle 952 400;
+   Splitter2\output -> Splitter4\input;
+   Splitter3\output -> PlusMinus2\minus 304 160;
+   Splitter3\output -> RTF2\angle 296 400;
+   Splitter4\output -> PlusMinus1\minus;
+   Splitter4\output -> PlusMinus3\minus 568 208;
+   Submodel3\angle -> Splitter1\input 368 448 368 416;
+   Submodel3\angular_momentum -> I\i 320 440 320 264;
+   Submodel3\joint1 -> MXTF\p 392 320;
    Submodel3\joint2 -> TF\p;
-   Submodel3\mass -> COM1\mass 216 440 216 464 296 464 296 504;
+   Submodel3\mass -> COM1\mass 336 528 416 528 416 568;
    Submodel4\angle -> Splitter2\input;
-   Submodel4\angular_momentum -> I1\i 600 360 600 224;
+   Submodel4\angular_momentum -> I1\i 720 424 720 288;
    Submodel4\joint1 -> TF3\p;
    Submodel4\joint2 -> TF2\p;
-   Submodel4\mass -> COM2\mass 616 464 616 520;
+   Submodel4\mass -> COM2\mass 736 528 736 584;
    TF\p2 => ZeroJunction\p;
    TF2\p2 => ZeroJunction2\p;
    TF3\p2 => ZeroJunction3\p;
@@ -1483,9 +1739,11 @@ implementation_end;
    ZeroJunction1\p => OneJunction5\p;
    ZeroJunction2\p => OneJunction9\p;
    ZeroJunction3\p => OneJunction12\p;
-   ZeroJunction4\p <= MSe\p;
    ZeroJunction4\p => OneJunction10\p;
    ZeroJunction4\p => R1\p;
+   ZeroJunction5\p <= MSe2\p;
+   ZeroJunction5\p => OneJunction3\p;
+   ZeroJunction5\p => R2\p;
   end;
  implementation_end;
 ]]>
@@ -1516,6 +1774,22 @@ implementation_end;
      <Name>I\state_initial</Name>
      <Value>0</Value>
     </Variable>
+    <Variable>
+     <Name>PID1\pdstate_initial</Name>
+     <Value>0</Value>
+    </Variable>
+    <Variable>
+     <Name>PID1\pistate_initial</Name>
+     <Value>0</Value>
+    </Variable>
+    <Variable>
+     <Name>PID\pdstate_initial</Name>
+     <Value>0</Value>
+    </Variable>
+    <Variable>
+     <Name>PID\pistate_initial</Name>
+     <Value>0</Value>
+    </Variable>
     <Variable>
      <Name>COM1\I\p.e_initial</Name>
      <Rows>2</Rows>
@@ -1553,6 +1827,8 @@ implementation_end;
     <VarName>Submodel4\body_position[1]</VarName>
     <VarName>Submodel4\body_position[2]</VarName>
     <VarName>Submodel4\body_position[3]</VarName>
+    <VarName>PlusMinus2\output</VarName>
+    <VarName>PlusMinus3\output</VarName>
    </VarNames>
    <Plots>
     <Plot>
@@ -1625,8 +1901,8 @@ implementation_end;
      <SharedZAxis>false</SharedZAxis>
      <XAxes>
       <Axis>
-       <Minimum>-0.09018649661710011</Minimum>
-       <Maximum>0.0899326084824357</Maximum>
+       <Minimum>-0.07410853798627151</Minimum>
+       <Maximum>0.0</Maximum>
        <Linear>true</Linear>
        <Scaling>3</Scaling>
        <Label>position[1]</Label>
@@ -1634,8 +1910,8 @@ implementation_end;
      </XAxes>
      <YAxes>
       <Axis>
-       <Minimum>-0.1</Minimum>
-       <Maximum>0.1</Maximum>
+       <Minimum>-0.08</Minimum>
+       <Maximum>0.12000000000000001</Maximum>
        <Linear>true</Linear>
        <Scaling>1</Scaling>
        <Label>position[2]</Label>
@@ -1664,6 +1940,151 @@ implementation_end;
        </YCurveData>
       </Curve>
      </Curves>
+     <Legenda>
+      <ShowLegenda>true</ShowLegenda>
+      <Position>
+       <X>0.9024193548387097</X>
+       <Y>0.0308411214953271</Y>
+      </Position>
+      <BorderPenColor>0</BorderPenColor>
+      <BackgroundColor>16777215</BackgroundColor>
+     </Legenda>
+    </Plot>
+    <Plot>
+     <PlotType>GraphPlot</PlotType>
+     <BasePlot>
+       <PlotId>3</PlotId>
+       <UseWindowsBGColor>false</UseWindowsBGColor>
+       <BGColor>16777215</BGColor>
+       <PlotIsVisible>true</PlotIsVisible>
+     </BasePlot>
+     <Grid>
+      <DrawGrid>true</DrawGrid>
+      <GridColor>15780518</GridColor>
+      <GridBorderLineColor>12624260</GridBorderLineColor>
+      <GridZeroLineColor>0</GridZeroLineColor>
+      <XTicks>10</XTicks>
+      <YTicks>10</YTicks>
+      <ZTicks>10</ZTicks>
+      <Use3DLook>false</Use3DLook>
+     </Grid>
+     <PlotBGColor>16777215</PlotBGColor>
+     <ShowPlotTitle>true</ShowPlotTitle>
+     <TitlePosition>1</TitlePosition>
+     <PlotTitle>model(1)</PlotTitle>
+     <ShowXValues>true</ShowXValues>
+     <Fonts>
+      <TitleFont>
+       <Name>Arial</Name>
+       <Height>12</Height>
+       <PitchFamily>34</PitchFamily>
+       <Weight>400</Weight>
+       <Italic>0</Italic>
+       <UnderLine>0</UnderLine>
+       <StrikeOut>0</StrikeOut>
+       <Color>0</Color>
+      </TitleFont>
+      <LabelFont>
+       <Name>Arial</Name>
+       <Height>12</Height>
+       <PitchFamily>34</PitchFamily>
+       <Weight>400</Weight>
+       <Italic>0</Italic>
+       <UnderLine>0</UnderLine>
+       <StrikeOut>0</StrikeOut>
+       <Color>0</Color>
+      </LabelFont>
+      <ValuesFont>
+       <Name>Arial</Name>
+       <Height>10</Height>
+       <PitchFamily>34</PitchFamily>
+       <Weight>400</Weight>
+       <Italic>0</Italic>
+       <UnderLine>0</UnderLine>
+       <StrikeOut>0</StrikeOut>
+       <Color>0</Color>
+      </ValuesFont>
+      <LegendFont>
+       <Name>Arial</Name>
+       <Height>12</Height>
+       <PitchFamily>34</PitchFamily>
+       <Weight>400</Weight>
+       <Italic>0</Italic>
+       <UnderLine>0</UnderLine>
+       <StrikeOut>0</StrikeOut>
+       <Color>0</Color>
+      </LegendFont>
+     </Fonts>
+     <SharedXAxis>true</SharedXAxis>
+     <SharedYAxis>true</SharedYAxis>
+     <SharedZAxis>false</SharedZAxis>
+     <XAxes>
+      <Axis>
+       <Minimum>0.0</Minimum>
+       <Maximum>20.0</Maximum>
+       <Linear>true</Linear>
+       <Scaling>3</Scaling>
+       <Label>time</Label>
+      </Axis>
+     </XAxes>
+     <YAxes>
+      <Axis>
+       <Minimum>-2.5</Minimum>
+       <Maximum>2.5</Maximum>
+       <Linear>true</Linear>
+       <Scaling>2</Scaling>
+       <Label>error Joint 1</Label>
+      </Axis>
+      <Axis>
+       <Minimum>0.0</Minimum>
+       <Maximum>235.918450873204</Maximum>
+       <Linear>true</Linear>
+       <Scaling>2</Scaling>
+       <Label>error joint 2</Label>
+      </Axis>
+     </YAxes>
+     <ZAxes>
+     </ZAxes>
+     <Curves>
+      <Curve>
+       <LineColor>3355111</LineColor>
+       <LineStyle>1</LineStyle>
+       <TickColor>3355111</TickColor>
+       <TickStyle>0</TickStyle>
+       <CurveVisible>true</CurveVisible>
+       <PixelThresshold>1</PixelThresshold>
+       <LineThickness>1</LineThickness>
+       <LineOrder>1</LineOrder>
+       <ShowYValues>true</ShowYValues>
+       <XCurveData>
+        <ShowUnit>true</ShowUnit>
+        <VarName>time</VarName>
+       </XCurveData>
+       <YCurveData>
+        <ShowUnit>true</ShowUnit>
+        <VarName>PlusMinus2\output</VarName>
+       </YCurveData>
+      </Curve>
+      <Curve>
+       <LineColor>6076255</LineColor>
+       <LineStyle>1</LineStyle>
+       <TickColor>6076255</TickColor>
+       <TickStyle>0</TickStyle>
+       <CurveVisible>true</CurveVisible>
+       <PixelThresshold>1</PixelThresshold>
+       <LineThickness>1</LineThickness>
+       <LineOrder>1</LineOrder>
+       <ShowYValues>true</ShowYValues>
+       <XCurveData>
+        <ShowUnit>true</ShowUnit>
+        <VarName>time</VarName>
+       </XCurveData>
+       <YCurveData>
+        <ShowUnit>true</ShowUnit>
+        <VarName>PlusMinus3\output</VarName>
+       </YCurveData>
+      </Curve>
+     </Curves>
      <Legenda>
       <ShowLegenda>true</ShowLegenda>
       <BorderPenColor>0</BorderPenColor>
@@ -2280,36 +2701,36 @@ implementation_end;
       <Name>Camera Looking at Origin</Name>
       <Position>
        <X>
-        <Value>2.76127713468209</Value>
+        <Value>3.40085013905783</Value>
        </X>
        <Y>
-        <Value>0.182591634017352</Value>
+        <Value>0.342236040003864</Value>
        </Y>
        <Z>
-        <Value>2.3434295155844</Value>
+        <Value>1.21109282816646</Value>
        </Z>
       </Position>
       <Orientation>
        <ZAxis>
         <X>
-         <Value>-0.761470091577479</Value>
+         <Value>-0.937843447259703</Value>
         </X>
         <Y>
-         <Value>-0.0503528119398566</Value>
+         <Value>-0.0943775275033589</Value>
         </Y>
         <Z>
-         <Value>-0.646241358907595</Value>
+         <Value>-0.33397986576198</Value>
         </Z>
        </ZAxis>
        <YAxis>
         <X>
-         <Value>-0.644833091581157</Value>
+         <Value>-0.332301512200796</Value>
         </X>
         <Y>
-         <Value>-0.0426400980841132</Value>
+         <Value>-0.033440330791642</Value>
         </Y>
         <Z>
-         <Value>0.763133085403368</Value>
+         <Value>0.942580208399057</Value>
         </Z>
        </YAxis>
        <OrientationType>Direct3D</OrientationType>
@@ -2636,18 +3057,18 @@ implementation_end;
          </X>
          <Y>
      <VariableName>Submodel3\body_position[2]</VariableName>
-     <Value>0.01353094798615309</Value>
+     <Value>-0.03102478766698046</Value>
          </Y>
          <Z>
      <VariableName>Submodel3\body_position[3]</VariableName>
-     <Value>0.029525982328835566</Value>
+     <Value>-0.009583285900524256</Value>
          </Z>
         </Position>
         <Orientation>
          <Angles>
      <X>
       <VariableName>Submodel3\body_angle[1]</VariableName>
-      <Value>12.13887841310187</Value>
+      <Value>1.8698917335338603</Value>
      </X>
      <Y>
       <VariableName>Submodel3\body_angle[2]</VariableName>
@@ -2741,18 +3162,18 @@ implementation_end;
          </X>
          <Y>
      <VariableName>Submodel4\body_position[2]</VariableName>
-     <Value>0.022454019332912738</Value>
+     <Value>-0.070814832000755</Value>
          </Y>
          <Z>
      <VariableName>Submodel4\body_position[3]</VariableName>
-     <Value>0.03555033739412045</Value>
+     <Value>0.004242842205841144</Value>
          </Z>
         </Position>
         <Orientation>
          <Angles>
      <X>
       <VariableName>Submodel4\body_angle[1]</VariableName>
-      <Value>97.21743686567467</Value>
+      <Value>0.3583520801537476</Value>
      </X>
      <Y>
       <VariableName>Submodel4\body_angle[2]</VariableName>
@@ -2791,6 +3212,16 @@ implementation_end;
      <Tiling>0</Tiling>
      <PlotIds>
        <PlotId>1</PlotId>
+       <PlotId>3</PlotId>
+     </PlotIds>
+     <ToggleState>Base</ToggleState>
+    </PlotPanel>
+    <PlotPanel>
+     <PlotPanelId>2</PlotPanelId>
+     <PlotPanelVisible>true</PlotPanelVisible>
+     <Name>Window 2</Name>
+     <Tiling>0</Tiling>
+     <PlotIds>
        <PlotId>2</PlotId>
      </PlotIds>
      <ToggleState>Base</ToggleState>
@@ -2798,7 +3229,10 @@ implementation_end;
    </PlotPanels>
    <WindowRectangles virtualDesktopWidth="5760" virtualDesktopHeight="1080">
      <WindowRectangle monitor="1" left="-1920" top="0" right="0" bottom="1080" plotpanelid="1">
-0.126562 0.0935185 0.819792 0.916667
+0.126562 0.0935185 0.909896 0.916667
+     </WindowRectangle>
+     <WindowRectangle monitor="2" left="1920" top="0" right="3840" bottom="1080" plotpanelid="2">
+0.205729 0.141667 0.845833 0.835185
      </WindowRectangle>
    </WindowRectangles>
   </PlotSpecs>
@@ -2849,9 +3283,9 @@ implementation_end;
     <IntegrationMethod>
      <Name>RungeKutta8</Name>
      <UseInitialStepSize>false</UseInitialStepSize>
-     <InitialStepSize>0.001</InitialStepSize>
+     <InitialStepSize>0.0</InitialStepSize>
      <UseMaximumStepSize>false</UseMaximumStepSize>
-     <MaximumStepSize>1.0</MaximumStepSize>
+     <MaximumStepSize>0.0</MaximumStepSize>
      <AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
      <RelativeTolerance>1.0e-6</RelativeTolerance>
      <SafetyFactor>0.9</SafetyFactor>
@@ -2866,18 +3300,18 @@ implementation_end;
     <IntegrationMethod>
      <Name>RungeKuttaFehlberg</Name>
      <UseInitialStepSize>false</UseInitialStepSize>
-     <InitialStepSize>0.001</InitialStepSize>
+     <InitialStepSize>0.0</InitialStepSize>
      <UseMaximumStepSize>false</UseMaximumStepSize>
-     <MaximumStepSize>1.0</MaximumStepSize>
+     <MaximumStepSize>0.0</MaximumStepSize>
      <AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
      <RelativeTolerance>1.0e-6</RelativeTolerance>
     </IntegrationMethod>
     <IntegrationMethod>
      <Name>VodeAdams</Name>
      <UseInitialStepSize>false</UseInitialStepSize>
-     <InitialStepSize>0.001</InitialStepSize>
+     <InitialStepSize>0.0</InitialStepSize>
      <UseMaximumStepSize>false</UseMaximumStepSize>
-     <MaximumStepSize>1.0</MaximumStepSize>
+     <MaximumStepSize>0.0</MaximumStepSize>
      <AbsoluteTolerance>1.0e-6</AbsoluteTolerance>
      <RelativeTolerance>1.0e-6</RelativeTolerance>
      <VodeUseBDF>true</VodeUseBDF>
@@ -2890,9 +3324,9 @@ implementation_end;
      <AlgebraicAbsoluteTolerance>1.0e-5</AlgebraicAbsoluteTolerance>
      <AlgebraicRelativeTolerance>1.0e-5</AlgebraicRelativeTolerance>
      <UseInitialStepSize>false</UseInitialStepSize>
-     <InitialStepSize>0.001</InitialStepSize>
+     <InitialStepSize>0.0</InitialStepSize>
      <UseMaximumStepSize>false</UseMaximumStepSize>
-     <MaximumStepSize>1.0</MaximumStepSize>
+     <MaximumStepSize>0.0</MaximumStepSize>
     </IntegrationMethod>
     <IntegrationMethod>
      <Name>MeBDFiMethod</Name>
@@ -2901,9 +3335,9 @@ implementation_end;
      <AlgebraicAbsoluteTolerance>1.0e-5</AlgebraicAbsoluteTolerance>
      <AlgebraicRelativeTolerance>1.0e-5</AlgebraicRelativeTolerance>
      <UseInitialStepSize>false</UseInitialStepSize>
-     <InitialStepSize>0.001</InitialStepSize>
+     <InitialStepSize>0.0</InitialStepSize>
      <UseMaximumStepSize>false</UseMaximumStepSize>
-     <MaximumStepSize>1.0</MaximumStepSize>
+     <MaximumStepSize>0.0</MaximumStepSize>
     </IntegrationMethod>
     <SelectedIntegrationMethod>8</SelectedIntegrationMethod>
    </IntegrationMethods>