4.8 0 False C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\03_motor\motor_physics2.emx 2020-7-20 15:58:21 True parameters real A = 0.05 {m}; real B = 0.05 {m}; variables real J0_1_BF1[2]; real J1_2_BF1[2]; real J1_2_BF2[2]; real EE1_BF2[2]; initialequations J0_1_BF1 = [A/2;0]; J1_2_BF1 = [-A/2;0]; J1_2_BF2 = [B/2;0]; EE1_BF2 = [-B/2;0]; '; type Mainmodel end; implementation bg submodels Base1 720 184 description '4.81FalseTrueBond Graph\MR\center_of_mass_v2.emx2020-7-20 15:56:10Baseparameters real I [3,1] = [7.583333333333335e-7; 3.645833333333334e-8; 7.364583333333335e-7] {N.m.s}; real m = 0.0035 {kg};'; type Submodel ports power out p [6,1]; signal in Hin [4,4]; signal out Hout [4,4]; end; icon bg ellipse bottom figures ellipse 696 160 744 208 color 0 fill 16777215 width 2; line 696 184 744 184 color 0 fill 16777215; line 720 160 720 208 color 0 fill 16777215; terminals p 704 160 fixed; Hin 736 160 fixed; Hout 744 216 fixed; end; implementation bg submodels AdHi0 544 400 description '4.01False2007-9-25 12:3:3True'; type MTF ports power in p1 [6,1]; power out p2 [6,1]; signal in H [4,4]; restrictions causality constraint not_equal p1 p2; end; icon bg left figures text 'MTF' 544 400 color 0 18 bold; end; implementation eq variables real onlyRotH[4,4]; code //Only rotations with respect to the inertial system matter! onlyRotH = H; onlyRotH[1,4] = 0; onlyRotH[2,4] = 0; onlyRotH[3,4] = 0; p2.e = transpose(Adjoint(onlyRotH)) * p1.e; p1.f = Adjoint(onlyRotH) * p2.f; implementation_end; EJS 616 336 description '4.01False2007-10-31 11:43:6True'; type MGY ports power in p1 [6,1]; end; icon bg top figures text 'MGY' 616 336 color 0 18 bold; end; implementation eq //EJS / Gyroscopic effects parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Q[6,6]; real QI[6,6]; real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations Q = transpose(adjoint(p1.f)); QI = Q*II; p1.e = QI*p1.f;implementation_end; Gravity 616 400 description '4.01False2007-9-25 12:3:26True'; type Se ports power out p [6,1]; restrictions causality fixed out p; end; icon bg bottom figures text 'Se' 616 400 color 0 18 bold; end; implementation eq parameters real global m; variables real effort[6]; equations effort = [0;0;0;0;0;-g_n*m]; p.e = effort; implementation_end; InertialTensor 544 288 description '4.01False2007-9-25 12:2:12True'; type I ports power in p [6,1]; signal out state [6,1]; restrictions causality preferred in p; end; icon bg top figures text 'I' 544 288 color 0 18 bold; end; implementation eq parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations state = int(p.e); //state = generalized momentum p.f = inverse(II)*state; implementation_end; plug p 472 336; plug Hin 472 496; plug Hout 640 496; Splitter1 544 496 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; ellipse 539.7 491.7 548.3 500.3 color -1; terminals input 544 496 fixed; end; implementation eq equations collect (output) = input; implementation_end; Ta0j 544 336 description ' 4.0 1 False Bond Graph\OneJunction.emx 2007-9-27 9:51:18 True '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg bottom figures text '1' 544 336 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; end; connections AdHi0\p2 => Ta0j\p; Gravity\p => AdHi0\p1; Hin -> Splitter1\input; InertialTensor\p <= Ta0j\p; p <= Ta0j\p; Splitter1\output -> AdHi0\H; Splitter1\output -> Hout; Ta0j\p => EJS\p1; end; parameterrelations InertialTensor\I = I; InterialTensor\m = m; EJS\I = I; EJS\m = m; AdHik\COMdim = COMdim; AdHik1\COMdim = COMdim; Hij\dim = dim; Gravity\m=m;parameterrelations_end; figures text 'b = current link (body) a = previous link (body) i = Body fixed frame, fixed in joint with previous link j = Body fixed frame, fixed in joint with next link k = Body fixed frame, principal inertial frame 0 = inertial system ' 288 280 color 0; implementation_end; Base2 912 120 description '4.81FalseTrueBond Graph\MR\center_of_mass_v2.emx2020-7-20 15:56:10Baseparameters real I [3,1] = [0;0;0] {N.m.s}; real m = 0.015 {kg};'; type Submodel ports power out p [6,1]; signal in Hin [4,4]; signal out Hout [4,4]; end; icon bg ellipse bottom figures ellipse 888 96 936 144 color 0 fill 16777215 width 2; line 888 120 936 120 color 0 fill 16777215; line 912 96 912 144 color 0 fill 16777215; terminals p 896 96 fixed; Hin 928 96 fixed; Hout 936 152 fixed; end; implementation bg submodels AdHi0 544 400 description '4.01False2007-9-25 12:3:3True'; type MTF ports power in p1 [6,1]; power out p2 [6,1]; signal in H [4,4]; restrictions causality constraint not_equal p1 p2; end; icon bg left figures text 'MTF' 544 400 color 0 18 bold; end; implementation eq variables real onlyRotH[4,4]; code //Only rotations with respect to the inertial system matter! onlyRotH = H; onlyRotH[1,4] = 0; onlyRotH[2,4] = 0; onlyRotH[3,4] = 0; p2.e = transpose(Adjoint(onlyRotH)) * p1.e; p1.f = Adjoint(onlyRotH) * p2.f; implementation_end; EJS 616 336 description '4.01False2007-10-31 11:43:6True'; type MGY ports power in p1 [6,1]; end; icon bg top figures text 'MGY' 616 336 color 0 18 bold; end; implementation eq //EJS / Gyroscopic effects parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Q[6,6]; real QI[6,6]; real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations Q = transpose(adjoint(p1.f)); QI = Q*II; p1.e = QI*p1.f;implementation_end; Gravity 616 400 description '4.01False2007-9-25 12:3:26True'; type Se ports power out p [6,1]; restrictions causality fixed out p; end; icon bg bottom figures text 'Se' 616 400 color 0 18 bold; end; implementation eq parameters real global m; variables real effort[6]; equations effort = [0;0;0;0;0;-g_n*m]; p.e = effort; implementation_end; InertialTensor 544 288 description '4.01False2007-9-25 12:2:12True'; type I ports power in p [6,1]; signal out state [6,1]; restrictions causality preferred in p; end; icon bg top figures text 'I' 544 288 color 0 18 bold; end; implementation eq parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations state = int(p.e); //state = generalized momentum p.f = inverse(II)*state; implementation_end; plug p 472 336; plug Hin 472 496; plug Hout 640 496; Splitter1 544 496 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; ellipse 539.7 491.7 548.3 500.3 color -1; terminals input 544 496 fixed; end; implementation eq equations collect (output) = input; implementation_end; Ta0j 544 336 description ' 4.0 1 False Bond Graph\OneJunction.emx 2007-9-27 9:51:18 True '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg bottom figures text '1' 544 336 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; end; connections AdHi0\p2 => Ta0j\p; Gravity\p => AdHi0\p1; Hin -> Splitter1\input; InertialTensor\p <= Ta0j\p; p <= Ta0j\p; Splitter1\output -> AdHi0\H; Splitter1\output -> Hout; Ta0j\p => EJS\p1; end; parameterrelations InertialTensor\I = I; InterialTensor\m = m; EJS\I = I; EJS\m = m; AdHik\COMdim = COMdim; AdHik1\COMdim = COMdim; Hij\dim = dim; Gravity\m=m;parameterrelations_end; figures text 'b = current link (body) a = previous link (body) i = Body fixed frame, fixed in joint with previous link j = Body fixed frame, fixed in joint with next link k = Body fixed frame, principal inertial frame 0 = inertial system ' 288 280 color 0; implementation_end; Base3 376 184 description ' 4.8 1 False True Bond Graph\MR\center_of_mass_v2.emx 2020-7-20 14:44:13 Base parameters real I [3,1] = [1.6399999999999998e-6; 4.7e-8; 1.61e-6] {N.m.s}; real m = 0.00455 {kg}; '; type Submodel ports power out p [6,1]; signal in Hin [4,4]; signal out Hout [4,4]; end; icon bg ellipse bottom figures ellipse 352 160 400 208 color 0 fill 16777215 width 2; line 352 184 400 184 color 0 fill 16777215; line 376 160 376 208 color 0 fill 16777215; terminals p 360 160 fixed; Hin 392 160 fixed; end; implementation bg submodels AdHi0 544 400 description ' 4.0 1 False Bond Graph\MTF.emx 2007-9-25 12:3:3 True '; type MTF ports power in p1 [6,1]; power out p2 [6,1]; signal in H [4,4]; restrictions causality constraint not_equal p1 p2; end; icon bg left figures text 'MTF' 544 400 color 0 18 bold; end; implementation eq variables real onlyRotH[4,4]; code //Only rotations with respect to the inertial system matter! onlyRotH = H; onlyRotH[1,4] = 0; onlyRotH[2,4] = 0; onlyRotH[3,4] = 0; p2.e = transpose(Adjoint(onlyRotH)) * p1.e; p1.f = Adjoint(onlyRotH) * p2.f; implementation_end; EJS 616 336 description ' 4.0 1 False Bond Graph\MGY.emx 2007-10-31 11:43:6 True '; type MGY ports power in p1 [6,1]; end; icon bg top figures text 'MGY' 616 336 color 0 18 bold; end; implementation eq //EJS / Gyroscopic effects parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Q[6,6]; real QI[6,6]; real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations Q = transpose(adjoint(p1.f)); QI = Q*II; p1.e = QI*p1.f;implementation_end; Gravity 616 400 description ' 4.0 1 False Bond Graph\Se.emx 2007-9-25 12:3:26 True '; type Se ports power out p [6,1]; restrictions causality fixed out p; end; icon bg bottom figures text 'Se' 616 400 color 0 18 bold; end; implementation eq parameters real global m; variables real effort[6]; equations effort = [0;0;0;0;0;-g_n*m]; p.e = effort; implementation_end; InertialTensor 544 288 description ' 4.0 1 False Bond Graph\I.emx 2007-9-25 12:2:12 True '; type I ports power in p [6,1]; signal out state [6,1]; restrictions causality preferred in p; end; icon bg top figures text 'I' 544 288 color 0 18 bold; end; implementation eq parameters real global I[3]; real global m; variables real II[6,6]; //Inertial tensor real Ia[6]; initialequations Ia[1:3] = I; Ia[4:6] = m; II = diag(Ia); equations state = int(p.e); //state = generalized momentum p.f = inverse(II)*state; implementation_end; plug p 472 336; plug Hin 472 496; plug Hout 640 496; Splitter1 544 496 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 540.8 492.8 547.2 499.2 color -1 fill 0; ellipse 539.7 491.7 548.3 500.3 color -1; terminals input 544 496 fixed; end; implementation eq equations collect (output) = input; implementation_end; Ta0j 544 336 description ' 4.0 1 False Bond Graph\OneJunction.emx 2007-9-27 9:51:18 True '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg bottom figures text '1' 544 336 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; end; connections AdHi0\p2 => Ta0j\p; Gravity\p => AdHi0\p1; Hin -> Splitter1\input; InertialTensor\p <= Ta0j\p; p <= Ta0j\p; Splitter1\output -> AdHi0\H; Splitter1\output -> Hout; Ta0j\p => EJS\p1; end; parameterrelations InertialTensor\I = I; InterialTensor\m = m; EJS\I = I; EJS\m = m; AdHik\COMdim = COMdim; AdHik1\COMdim = COMdim; Hij\dim = dim; Gravity\m=m;parameterrelations_end; figures text 'b = current link (body) a = previous link (body) i = Body fixed frame, fixed in joint with previous link j = Body fixed frame, fixed in joint with next link k = Body fixed frame, principal inertial frame 0 = inertial system ' 288 280 color 0; implementation_end; C1 640 264 description '4.01False Bond Graph\3D\C-3.emx 2007-9-25 12:12:8 '; type 'C-3' ports power in p [3,1]; signal out state [3,1]; restrictions causality preferred out p; end; icon bg bottom figures text 'C' 640 264 color 0 18 bold; end; implementation eq parameters real c[3,3] = [0.1, 0.0, 0.0; 0.0, 0.1, 0.0; 0.0, 0.0, 0.1] {N/m}; equations state = int(p.f); p.e = inverse(c) *state; implementation_end; C2 304 272 description '4.01False Bond Graph\3D\C-3.emx 2007-9-25 12:12:8 '; type 'C-3' ports power in p [3,1]; signal out state [3,1]; restrictions causality preferred out p; end; icon bg bottom figures text 'C' 304 272 color 0 18 bold; end; implementation eq parameters real c[3,3] = [0.1, 0.0, 0.0; 0.0, 0.1, 0.0; 0.0, 0.0, 0.1] {N/m}; equations state = int(p.f); p.e = inverse(c) *state; implementation_end; C3 304 224 description '4.01False Bond Graph\2D\C-2.emx 2007-9-25 12:7:27 '; type 'C-2' ports power in p [2,1]; signal out state [2,1]; restrictions causality preferred out p; end; icon bg bottom figures text 'C' 304 224 color 0 18 bold; end; implementation eq parameters real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; implementation_end; C4 640 216 description '4.01False Bond Graph\2D\C-2.emx 2007-9-25 12:7:27 '; type 'C-2' ports power in p [2,1]; signal out state [2,1]; restrictions causality preferred out p; end; icon bg bottom figures text 'C' 640 216 color 0 18 bold; end; implementation eq parameters real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; implementation_end; Integrate 176 288 description ' 4.3 1 False Signal\Block Diagram\Integrate.emx 2013-3-8 14:47:42 '; type Integrate ports signal in input; signal out output; end; icon bg bottom figures rectangle 160 272 192 304 color 0 fill 15132390; text '∫' 176 288.3 color 16711680 'Lucida Sans' 21 italic; end; implementation eq parameters real initial = 0; // initial value equations output = int (input, initial); implementation_end; Integrate1 520 352 description ' 4.3 1 False Signal\Block Diagram\Integrate.emx 2013-3-8 14:47:42 '; type Integrate ports signal in input; signal out output; end; icon bg bottom figures rectangle 504 336 536 368 color 0 fill 15132390; text '∫' 520 352.3 color 16711680 'Lucida Sans' 21 italic; end; implementation eq parameters real initial = 0; // initial value equations output = int (input, initial); implementation_end; inverse_kinematics1 272 520 description ' 4.8 SCARA\inverse_kinematics_v1.emx 1 False 2020-7-10 12:26:18 False '; 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} ; signal out abs_angle2; 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; abs_angle2 = angle1 - pi * to_rad + c; angle2 = c - pi * to_rad; implementation_end; Joint1 200 120 description ' 4.8 1 Bond Graph\MR\joint-v3.emx 2020-7-20 14:25:50 '; type 'Submodel-v3' ports power in Pin [6,1]; signal in Hin [4,4]; power in Pdiff [6,1]; signal out Hout [4,4]; power out Pout [6,1]; end; icon bg bottom figures rectangle 176 88 224 152 color 0 fill 14745599; text 'Joint' 200 120 color 0 'Clear Sans' 16; terminals Pin 224 104 fixed; Hin 176 136 fixed; Hout 224 136 fixed; Pout 176 104 fixed; end; implementation bg submodels AdHji 424 424 description ' 4.0 1 False Bond Graph\MTF.emx 2007-9-25 12:3:3 True '; type MTF ports power out p1 [6,1]; power in p2 [6,1]; signal in H [4,4]; restrictions causality constraint not_equal p1 p2; end; icon bg bottom figures text 'MTF' 424 424 color 0 18 bold; end; implementation eq equations p2.e = transpose(Adjoint(H)) * p1.e; p1.f = Adjoint(H) * p2.f;implementation_end; FlowSensor2 240 271.9 description ' 4.2 1 False Bond Graph\FlowSensor.emx 2011-11-29 15:50:53 '; knot FlowSensor ports power knot in p1 [6,1]; power knot out p2 [6,1]; signal knot out flow [6,1]; restrictions causality constraint not_equal p1 p2; end; icon bg ellipse figures ellipse 233.1 264.8 246.9 279.1 color 0 fill 16777215; text 'f' 240 271.2 color 0; end; implementation eq equations p2.f = p1.f; p1.e = p2.e; flow = p1.f; implementation_end; Hmatrix 320 272 description '4.0Template\Submodel-Equation.emx1False2007-11-1 22:32:1False'; type 'Submodel-Equation' ports signal in flow [6,1]; signal out H [4,4]; end; implementation eq parameters real init[4] = [1;0;0;0]; variables real q[4]; //quaternions real W[3,4]; //Quaternion Rates Matrix real R[3,3]; //Rotation Matrix real p[3]; //Position Vector equations ddt(q,init) = transpose(W) * flow[1:3] ./ 2; p = int(flow[4:6]); W = [-q[2], q[1], -q[4], q[3]; -q[3], q[4], q[1], -q[2]; -q[4], -q[3], q[2], q[1]]; R = [q[1]^2+q[2]^2-q[3]^2-q[4]^2, 2*(q[2]*q[3]+q[1]*q[4]), 2*(q[2]*q[4]-q[1]*q[3]); 2*(q[2]*q[3]-q[1]*q[4]), q[1]^2-q[2]^2+q[3]^2-q[4]^2, 2*(q[3]*q[4]+q[1]*q[2]); 2*(q[2]*q[4]+q[1]*q[3]), 2*(q[3]*q[4]-q[1]*q[2]), q[1]^2-q[2]^2-q[3]^2+q[4]^2]; H = homogeneous(R,p); implementation_end; MatrixMul 320 576 description ' 4.0 1 False Signal\Block Diagram\Gain.emx 2007-9-26 12:15:12 True '; type Gain ports signal in input1 [4,4]; signal out output [4,4]; signal in input2 [4,4]; end; icon bg bottom figures rectangle 304.1 560 335.9 592 color 0 fill 15132390; text 'X' 320 576 color 16711680 16 bold; end; implementation eq equations output = input2*input1; implementation_end; plug Hin 121.4 576; plug Pdiff 240 220; plug Hout 491.2 576; plug Pout 119.4 424; plug Pin 485.6 424; Splitter2 320 312 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 316.8 308.8 323.2 315.2 color -1 fill 0; ellipse 315.7 307.7 324.3 316.3 color -1; terminals input 320 312 fixed; end; implementation eq equations collect (output) = input; implementation_end; Wbai 240 424 description ' 4.0 1 False Bond Graph\ZeroJunction.emx 2007-9-27 9:51:43 True '; knot ZeroJunction ports power knot duplicatable none p [6,1]; signal knot out effort [6,1]; restrictions causality constraint one_in p; end; icon bg bottom figures text '0' 240 424 color 0 18 bold; end; implementation eq equations sum (direct (p.f)) = 0; equal (collect (p.e)); effort = first (p.e); implementation_end; end; connections FlowSensor2\flow -> Hmatrix\flow; FlowSensor2\p2 => Wbai\p; Hin -> MatrixMul\input2; Hmatrix\H -> Splitter2\input; MatrixMul\output -> Hout; Pdiff => FlowSensor2\p1; Pin => AdHji\p2; Splitter2\output -> AdHji\H 424 312; Splitter2\output -> MatrixMul\input1; Wbai\p <= AdHji\p1; Wbai\p => Pout; end; parameterrelations EndstopMin\Rendstop = Rendstop; EndstopMin\Cendstop = Cendstop; EndstopMin\InitialPos = InitialPos; EndstopMin\EndstopPos = MinEndstopPos; EndstopMax\Rendstop = Rendstop; EndstopMax\Cendstop = Cendstop; EndstopMax\InitialPos = InitialPos; EndstopMax\EndstopPos = MaxEndstopPos; Rjoint\Rjoint= Rjoint; Integrate\init = InitialPos; uTbai\Rconstraint = Rconstraint; uTbai\Cconstraint = Cconstraint; parameterrelations_end; implementation_end; Joint2 536 120 description ' 4.8 1 Bond Graph\MR\joint-v3.emx 2020-7-20 14:25:50 '; type 'Submodel-v3' ports power in Pin [6,1]; signal in Hin [4,4]; power in Pdiff [6,1]; signal out Hout [4,4]; power out Pout [6,1]; end; icon bg bottom figures rectangle 512 88 560 152 color 0 fill 14745599; text 'Joint' 536 120 color 0 'Clear Sans' 16; terminals Pin 560 104 fixed; Hin 512 136 fixed; Hout 560 136 fixed; Pout 512 104 fixed; end; implementation bg submodels AdHji 424 424 description ' 4.0 1 False Bond Graph\MTF.emx 2007-9-25 12:3:3 True '; type MTF ports power out p1 [6,1]; power in p2 [6,1]; signal in H [4,4]; restrictions causality constraint not_equal p1 p2; end; icon bg bottom figures text 'MTF' 424 424 color 0 18 bold; end; implementation eq equations p2.e = transpose(Adjoint(H)) * p1.e; p1.f = Adjoint(H) * p2.f;implementation_end; FlowSensor2 240 271.9 description ' 4.2 1 False Bond Graph\FlowSensor.emx 2011-11-29 15:50:53 '; knot FlowSensor ports power knot in p1 [6,1]; power knot out p2 [6,1]; signal knot out flow [6,1]; restrictions causality constraint not_equal p1 p2; end; icon bg ellipse figures ellipse 233.1 264.8 246.9 279.1 color 0 fill 16777215; text 'f' 240 271.2 color 0; end; implementation eq equations p2.f = p1.f; p1.e = p2.e; flow = p1.f; implementation_end; Hmatrix 320 272 description '4.0Template\Submodel-Equation.emx1False2007-11-1 22:32:1False'; type 'Submodel-Equation' ports signal in flow [6,1]; signal out H [4,4]; end; implementation eq parameters real init[4] = [1;0;0;0]; variables real q[4]; //quaternions real W[3,4]; //Quaternion Rates Matrix real R[3,3]; //Rotation Matrix real p[3]; //Position Vector equations ddt(q,init) = transpose(W) * flow[1:3] ./ 2; p = int(flow[4:6]); W = [-q[2], q[1], -q[4], q[3]; -q[3], q[4], q[1], -q[2]; -q[4], -q[3], q[2], q[1]]; R = [q[1]^2+q[2]^2-q[3]^2-q[4]^2, 2*(q[2]*q[3]+q[1]*q[4]), 2*(q[2]*q[4]-q[1]*q[3]); 2*(q[2]*q[3]-q[1]*q[4]), q[1]^2-q[2]^2+q[3]^2-q[4]^2, 2*(q[3]*q[4]+q[1]*q[2]); 2*(q[2]*q[4]+q[1]*q[3]), 2*(q[3]*q[4]-q[1]*q[2]), q[1]^2-q[2]^2-q[3]^2+q[4]^2]; H = homogeneous(R,p); implementation_end; MatrixMul 320 576 description ' 4.0 1 False Signal\Block Diagram\Gain.emx 2007-9-26 12:15:12 True '; type Gain ports signal in input1 [4,4]; signal out output [4,4]; signal in input2 [4,4]; end; icon bg bottom figures rectangle 304.1 560 335.9 592 color 0 fill 15132390; text 'X' 320 576 color 16711680 16 bold; end; implementation eq equations output = input2*input1; implementation_end; plug Hin 121.4 576; plug Pdiff 240 220; plug Hout 491.2 576; plug Pout 119.4 424; plug Pin 485.6 424; Splitter2 320 312 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 316.8 308.8 323.2 315.2 color -1 fill 0; ellipse 315.7 307.7 324.3 316.3 color -1; terminals input 320 312 fixed; end; implementation eq equations collect (output) = input; implementation_end; Wbai 240 424 description ' 4.0 1 False Bond Graph\ZeroJunction.emx 2007-9-27 9:51:43 True '; knot ZeroJunction ports power knot duplicatable none p [6,1]; signal knot out effort [6,1]; restrictions causality constraint one_in p; end; icon bg bottom figures text '0' 240 424 color 0 18 bold; end; implementation eq equations sum (direct (p.f)) = 0; equal (collect (p.e)); effort = first (p.e); implementation_end; end; connections FlowSensor2\flow -> Hmatrix\flow; FlowSensor2\p2 => Wbai\p; Hin -> MatrixMul\input2; Hmatrix\H -> Splitter2\input; MatrixMul\output -> Hout; Pdiff => FlowSensor2\p1; Pin => AdHji\p2; Splitter2\output -> AdHji\H 424 312; Splitter2\output -> MatrixMul\input1; Wbai\p <= AdHji\p1; Wbai\p => Pout; end; parameterrelations EndstopMin\Rendstop = Rendstop; EndstopMin\Cendstop = Cendstop; EndstopMin\InitialPos = InitialPos; EndstopMin\EndstopPos = MinEndstopPos; EndstopMax\Rendstop = Rendstop; EndstopMax\Cendstop = Cendstop; EndstopMax\InitialPos = InitialPos; EndstopMax\EndstopPos = MaxEndstopPos; Rjoint\Rjoint= Rjoint; Integrate\init = InitialPos; uTbai\Rconstraint = Rconstraint; uTbai\Cconstraint = Cconstraint; parameterrelations_end; implementation_end; Link 280 120 description ' 4.8 Bond Graph\MR\link-v3.emx 1 False 2020-7-19 15:22:34 False '; type 'Submodel-Equation' ports signal in Hin [4,4]; signal out Hout [4,4]; power in Pin [6,1]; power out Pout [6,1]; restrictions causality constraint not_equal Pin Pout; end; icon bg bottom figures rectangle 248 96 312 144 color 0 fill 8454041; text 'Link' 280 120 color 0 'Clear Sans' 16; terminals Hin 248 136 fixed; Hout 312 136 fixed; Pin 312 104 fixed; Pout 248 104 fixed; end; implementation eq parameters real offset[3]= [0;0.0325;0]; //coordinates of joint_1 variables real Hab[4,4]; real AdHab[6,6]; equations Hab = homogeneous(eye(3),offset); AdHab = Adjoint(eye(3),offset); Hout = Hin * Hab; Pout.e = transpose(AdHab) * Pin.e; Pin.f = AdHab * Pout.f; implementation_end; Link1 456 120 description ' 4.8 Bond Graph\MR\link-v3.emx 1 False 2020-7-19 15:22:34 False '; type 'Submodel-Equation' ports signal in Hin [4,4]; signal out Hout [4,4]; power in Pin [6,1]; power out Pout [6,1]; restrictions causality constraint not_equal Pin Pout; end; icon bg bottom figures rectangle 424 96 488 144 color 0 fill 8454041; text 'Link' 456 120 color 0 'Clear Sans' 16; terminals Hin 424 136 fixed; Hout 488 136 fixed; Pin 488 104 fixed; Pout 424 104 fixed; end; implementation eq parameters real offset[3]= [0;0.0325;0]; //coordinates of joint_1 variables real Hab[4,4]; real AdHab[6,6]; equations Hab = homogeneous(eye(3),offset); AdHab = Adjoint(eye(3),offset); Hout = Hin * Hab; Pout.e = transpose(AdHab) * Pin.e; Pin.f = AdHab * Pout.f; implementation_end; Link2 624 120 description ' 4.8 Bond Graph\MR\link-v3.emx 1 False 2020-7-19 15:22:34 False '; type 'Submodel-Equation' ports signal in Hin [4,4]; signal out Hout [4,4]; power in Pin [6,1]; power out Pout [6,1]; restrictions causality constraint not_equal Pin Pout; end; icon bg bottom figures rectangle 592 96 656 144 color 0 fill 8454041; text 'Link' 624 120 color 0 'Clear Sans' 16; terminals Hin 592 136 fixed; Hout 656 136 fixed; Pin 656 104 fixed; Pout 592 104 fixed; end; implementation eq parameters real offset[3]= [0;0.025;0]; //coordinates of joint_1 variables real Hab[4,4]; real AdHab[6,6]; equations Hab = homogeneous(eye(3),offset); AdHab = Adjoint(eye(3),offset); Hout = Hin * Hab; Pout.e = transpose(AdHab) * Pin.e; Pin.f = AdHab * Pout.f; implementation_end; Link3 800 120 description ' 4.8 Bond Graph\MR\link-v3.emx 1 False 2020-7-19 15:22:34 False '; type 'Submodel-Equation' ports signal in Hin [4,4]; signal out Hout [4,4]; power in Pin [6,1]; power out Pout [6,1]; restrictions causality constraint not_equal Pin Pout; end; icon bg bottom figures rectangle 768 96 832 144 color 0 fill 8454041; text 'Link' 800 120 color 0 'Clear Sans' 16; terminals Hin 768 136 fixed; Hout 832 136 fixed; Pin 832 104 fixed; Pout 768 104 fixed; end; implementation eq parameters real offset[3]= [0;0.025;0]; //coordinates of joint_1 variables real Hab[4,4]; real AdHab[6,6]; equations Hab = homogeneous(eye(3),offset); AdHab = Adjoint(eye(3),offset); Hout = Hin * Hab; Pout.e = transpose(AdHab) * Pin.e; Pin.f = AdHab * Pout.f; implementation_end; Negate1 272 448 description '4.01False Signal\Block Diagram\Negate.emx 2007-9-26 12:14:11 '; type Negate ports signal in input; signal out output; end; icon bg bottom figures rectangle 256 432 288 464 color 0 fill 15132390; text '-1' 272 448 color 16711680 16 bold; end; implementation eq equations output = - input; implementation_end; Negate2 520 520 description '4.01False Signal\Block Diagram\Negate.emx 2007-9-26 12:14:11 '; type Negate ports signal in input; signal out output; end; icon bg bottom figures rectangle 504 504 536 536 color 0 fill 15132390; text '-1' 520 520 color 16711680 16 bold; end; implementation eq equations output = - input; implementation_end; new_joint1 376 256 description '4.0 Template\Submodel-Equation.emx 1 False 2007-11-1 22:32:1 False '; type 'Submodel-Equation' ports signal in input [4,4]; signal out output; end; implementation eq variables real position[3]; real rotation[3]; real R[3,3]; equations position = input[1:3,4]; rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); //[input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; new_joint2 824 288 description '4.0 Template\Submodel-Equation.emx 1 False 2007-11-1 22:32:1 False '; type 'Submodel-Equation' ports signal in input [4,4]; signal out output; end; implementation eq variables real position[3]; real rotation[3]; real R[3,3]; equations // start typing here position = input[1:3,4]; rotation = [input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; new_joint3 1008 152 description '4.0 Template\Submodel-Equation.emx 1 False 2007-11-1 22:32:1 False '; type 'Submodel-Equation' ports signal in input [4,4]; signal out output; end; implementation eq variables real position[3]; real rotation[3]; real R[3,3]; equations position = input[1:3,4]; rotation = [input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; OneJunction1 360 104 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 360 104 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction17 520 288 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [1]; signal knot out flow [1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 520 288 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction2 120 104 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 120 104 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction3 704 104 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [6,1]; signal knot out flow [6,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 704 104 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction4 593 232 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [2,1]; signal knot out flow [2,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 593 232 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction5 257 240 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [2,1]; signal knot out flow [2,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 257 240 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction6 257 256 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [3,1]; signal knot out flow [3,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 257 256 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; OneJunction7 593 248 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [3,1]; signal knot out flow [3,1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 593 248 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; PID2 200 384 description ' 4.0 1 False Signal\Control\PID Control\Continuous\PID.emx 2008-1-17 10:49:30 '; type PID ports signal in error; signal out output; end; icon bg bottom figures rectangle 184 368 216 400 color 0 fill 15132390; text 'PID' 200 383.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 = 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; PID3 576 432 description ' 4.0 1 False Signal\Control\PID Control\Continuous\PID.emx 2008-1-17 10:49:30 '; type PID ports signal in error; signal out output; end; icon bg bottom figures rectangle 560 416 592 448 color 0 fill 15132390; text 'PID' 576 431.5 color 16711680 18 bold; end; implementation eq parameters real kp = 0.02 {}; // Proportional gain real tauD = 2 {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; PlusMinus4 272 384 description '4.01False Signal\Block Diagram\PlusMinus.emx 2007-9-27 10:15:13 '; 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 264 376 280 392 color 0 fill 16777215; end; implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; PlusMinus5 520 432 description '4.01False Signal\Block Diagram\PlusMinus.emx 2007-9-27 10:15:13 '; 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 512 424 528 440 color 0 fill 16777215; end; implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; PowerMux 200 214 specifications active 'rot_x' specification 'rot_x' description ' 4.8 Bond Graph\MR\PowerMux-Rotation.emx 2020-7-20 14:35:23 1 False True '; type 'PowerMux-Rotation' ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; causality constraint not_equal input_rot2 output; causality constraint not_equal input_pos3 output; end; icon bg bottom figures line 176 216 224 216 color 0 width 2; rectangle 176 208 224 220 color -1; text '1' 194 211 color 8421504 8; terminals input 184 216 fixed; output 200 216 fixed; input_rot2 216 216 fixed; input_pos3 200 216 fixed; end; implementation eq equations output.e[1] = input.e; output.e[2:3] = input_rot2.e; output.e[4:6] = input_pos3.e; output.f[1] = input.f; output.f[2:3] = input_rot2.f; output.f[4:6] = input_pos3.f;implementation_end; specification_end; specification 'rot_y' description ' 4.1 Bond Graph\PowerMux.emx 2011-3-4 15:12:50 1 False True '; type PowerMux ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; end; icon bg bottom figures line 808 216 808 264 color 0 width 2; rectangle 804 216 816 264 color -1; text '1' 813 234 color 8421504 8; terminals input 808 224 fixed; output 808 240 fixed; input_rot2 808 256 fixed; input_pos3 808 240 fixed; end; implementation eq equations output.e[1] = input_rot2.e[1]; output.e[2] = input.e; output.e[3] = input_rot2.e[2]; output.e[4:6] = input_pos3.e; output.f[1] = input_rot2.f[1]; output.f[2] = input.f; output.f[3] = input_rot2.f[2]; output.f[4:6] = input_pos3.f;implementation_end; specification_end; specification 'rot_z' description ' 4.1 Bond Graph\PowerMux.emx 2011-3-4 15:12:50 1 False True '; type PowerMux ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; end; icon bg bottom figures line 808 216 808 264 color 0 width 2; rectangle 804 216 816 264 color -1; text '1' 813 234 color 8421504 8; terminals input 808 224 fixed; output 808 240 fixed; input_rot2 808 256 fixed; input_pos3 808 240 fixed; end; implementation eq equations output.e[3] = input.e; output.e[1:2] = input_rot2.e; output.e[4:6] = input_pos3.e; output.f[3] = input.f; output.f[1:2] = input_rot2.f; output.f[4:6] = input_pos3.f;implementation_end; specification_end; end; PowerMux1 536 206 specifications active 'rot_x' specification 'rot_x' description ' 4.8 Bond Graph\MR\PowerMux-Rotation.emx 2020-7-20 14:35:23 1 False True '; type 'PowerMux-Rotation' ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; causality constraint not_equal input_rot2 output; causality constraint not_equal input_pos3 output; end; icon bg bottom figures line 512 208 560 208 color 0 width 2; rectangle 512 200 560 212 color -1; text '1' 530 203 color 8421504 8; terminals input 520 208 fixed; output 536 208 fixed; input_rot2 552 208 fixed; input_pos3 536 208 fixed; end; implementation eq equations output.e[1] = input.e; output.e[2:3] = input_rot2.e; output.e[4:6] = input_pos3.e; output.f[1] = input.f; output.f[2:3] = input_rot2.f; output.f[4:6] = input_pos3.f;implementation_end; specification_end; specification 'rot_y' description ' 4.1 Bond Graph\PowerMux.emx 2011-3-4 15:12:50 1 False True '; type PowerMux ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; end; icon bg bottom figures line 808 216 808 264 color 0 width 2; rectangle 804 216 816 264 color -1; text '1' 813 234 color 8421504 8; terminals input 808 224 fixed; output 808 240 fixed; input_rot2 808 256 fixed; input_pos3 808 240 fixed; end; implementation eq equations output.e[1] = input_rot2.e[1]; output.e[2] = input.e; output.e[3] = input_rot2.e[2]; output.e[4:6] = input_pos3.e; output.f[1] = input_rot2.f[1]; output.f[2] = input.f; output.f[3] = input_rot2.f[2]; output.f[4:6] = input_pos3.f;implementation_end; specification_end; specification 'rot_z' description ' 4.1 Bond Graph\PowerMux.emx 2011-3-4 15:12:50 1 False True '; type PowerMux ports power in input; power out output [6,1]; power in input_rot2 [2,1]; power in input_pos3 [3,1]; restrictions causality constraint not_equal input output; end; icon bg bottom figures line 808 216 808 264 color 0 width 2; rectangle 804 216 816 264 color -1; text '1' 813 234 color 8421504 8; terminals input 808 224 fixed; output 808 240 fixed; input_rot2 808 256 fixed; input_pos3 808 240 fixed; end; implementation eq equations output.e[3] = input.e; output.e[1:2] = input_rot2.e; output.e[4:6] = input_pos3.e; output.f[3] = input.f; output.f[1:2] = input_rot2.f; output.f[4:6] = input_pos3.f;implementation_end; specification_end; end; R 64 288 description ' 4.2 1 False Bond Graph\R.emx 2011-11-29 16:35:37 '; type R ports power in p; end; icon bg bottom figures text 'R' 64 288 color 0 18 bold; end; implementation eq parameters real r = 0.01; equations p.e = r * p.f; implementation_end; R1 464 288 description ' 4.2 1 False Bond Graph\R.emx 2011-11-29 16:35:37 '; type R ports power in p; end; icon bg bottom figures text 'R' 464 288 color 0 18 bold; end; implementation eq parameters real r = 0.01; equations p.e = r * p.f; implementation_end; R2 257 184 description '4.01False Bond Graph\2D\R-2.emx 2007-9-25 12:6:54 '; type 'R-2' ports power in p [2,1]; end; icon bg bottom figures text 'R' 257 184 color 0 18 bold; end; implementation eq parameters real r[2,2] = [1.0, 0.0; 0.0, 1.0] {kN.m.s/rad}; equations p.e = r * p.f; implementation_end; R3 257 312 description '4.01False Bond Graph\3D\R-3.emx 2007-9-25 12:11:54 '; type 'R-3' ports power in p [3,1]; end; icon bg bottom figures text 'R' 257 312 color 0 18 bold; end; implementation eq parameters real r[3,3] = [1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0] {kN.s/m}; equations p.e = r * p.f; implementation_end; R4 593 176 description '4.01False Bond Graph\2D\R-2.emx 2007-9-25 12:6:54 '; type 'R-2' ports power in p [2,1]; end; icon bg bottom figures text 'R' 593 176 color 0 18 bold; end; implementation eq parameters real r[2,2] = [1.0, 0.0; 0.0, 1.0] {kN.m.s/rad}; equations p.e = r * p.f; implementation_end; R5 593 304 description '4.01False Bond Graph\3D\R-3.emx 2007-9-25 12:11:54 '; type 'R-3' ports power in p [3,1]; end; icon bg bottom figures text 'R' 593 304 color 0 18 bold; end; implementation eq parameters real r[3,3] = [1.0, 0.0, 0.0; 0.0, 1.0, 0.0; 0.0, 0.0, 1.0] {kN.s/m}; equations p.e = r * p.f; implementation_end; rectanglepath 272 584 description ' 4.8 setpoint\rectanglepath.emx 1 False 2020-7-10 12:30:38 False '; 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 = 0.5 {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; Sf2 64 104 description '4.01False Bond Graph\2D\Sf-2.emx 2007-9-25 12:7:5 '; type 'Sf-2' ports power out p [6,1]; restrictions causality fixed in p; end; icon bg bottom figures text 'Sf' 64 104 color 0 18 bold; end; implementation eq parameters real flow[6,1] = 0; variables real effort [6]; equations p.f = flow; effort = p.e; implementation_end; Splitter1 392 136 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 388.8 132.8 395.2 139.2 color -1 fill 0; ellipse 387.7 131.7 396.3 140.3 color -1; terminals input 392 136 fixed; end; implementation eq equations collect (output) = input; implementation_end; Splitter2 736 136 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; knot Splitter ports signal knot duplicatable out output [4,4]; signal knot in input [4,4]; end; icon bg ellipse figures ellipse 732.8 132.8 739.2 139.2 color -1 fill 0; ellipse 731.7 131.7 740.3 140.3 color -1; terminals input 736 136 fixed; end; implementation eq equations collect (output) = input; implementation_end; Submodel2 112 384 description ' 4.8 1 '; type Submodel ports signal in input; power out p2; end; implementation bg submodels Gain1 552 88 description '4.01False Signal\Block Diagram\Gain.emx 2007-9-26 12:15:12 '; type Gain ports signal in input; signal out output; end; icon bg bottom figures rectangle 536.1 72 567.9 104 color 0 fill 15132390; text 'K' 552 88 color 16711680 16 bold; end; implementation eq parameters real K = 12.0; // gain equations output = K * input; implementation_end; GY 624 184 description ' 4.2 1 False Bond Graph\GY.emx 2011-11-29 15:53:45 '; type GY ports power in p1; power out p2; restrictions causality constraint equal p1 p2; end; icon bg bottom figures text 'GY' 624 184 color 0 18 bold; end; implementation eq parameters real r = 0.127; equations p1.e = r * p2.f; p2.e = r * p1.f; implementation_end; MSe 624 88 description ' 4.2 1 False Bond Graph\MSe.emx 2011-11-29 16:12:33 '; type MSe ports power out p; signal in effort; restrictions causality fixed out p; end; icon bg bottom figures text 'MSe' 624 88 color 0 18 bold; end; implementation eq variables real flow; equations p.e = effort; flow = p.f; implementation_end; plug input 424 88; plug p2 624 220; OneJunction2 624 136 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [1]; signal knot out flow [1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 624 136 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; R 664 136 description ' 4.2 1 False Bond Graph\R.emx 2011-11-29 16:35:37 '; type R ports power in p; end; icon bg bottom figures text 'R' 664 136 color 0 18 bold; end; implementation eq parameters real r = 0.08; equations p.e = r * p.f; implementation_end; SignalLimiter2 488 88 description '4.01False Signal\Block Diagram Non-Linear\SignalLimiter-Limit.emx 2007-9-26 12:47:40 '; type 'SignalLimiter-Limit' ports signal in input; signal out output; end; icon bg bottom figures group rectangle 472 72 504 104 color 0 fill 15132390; line 487.9 76.5 487.9 101 color 0 fill 15132390; line 475 88.2 500.7 88.2 color 0 fill 15132390; spline 481.9 95.1 493.9 81.4 color 16711680 fill 15132390 width 2; spline 493.3 82.2 501.9 81.8 color 16711680 fill 15132390 width 2; spline 475 95.6 481.9 95.1 color 16711680 fill 15132390 width 2; end; end; implementation eq parameters real maximum = 1; real minimum = -1; equations output = limit (input, minimum, maximum); implementation_end; end; connections Gain1\output -> MSe\effort; GY\p2 => p2; input -> SignalLimiter2\input; MSe\p => OneJunction2\p; OneJunction2\p => GY\p1; R\p <= OneJunction2\p; SignalLimiter2\output -> Gain1\input; end; implementation_end; Submodel5 664 432 description ' 4.8 1 '; type Submodel ports signal in input; power out p2; end; implementation bg submodels Gain1 552 88 description '4.01False Signal\Block Diagram\Gain.emx 2007-9-26 12:15:12 '; type Gain ports signal in input; signal out output; end; icon bg bottom figures rectangle 536.1 72 567.9 104 color 0 fill 15132390; text 'K' 552 88 color 16711680 16 bold; end; implementation eq parameters real K = 12.0; // gain equations output = K * input; implementation_end; GY 624 184 description ' 4.2 1 False Bond Graph\GY.emx 2011-11-29 15:53:45 '; type GY ports power in p1; power out p2; restrictions causality constraint equal p1 p2; end; icon bg bottom figures text 'GY' 624 184 color 0 18 bold; end; implementation eq parameters real r = 0.127; equations p1.e = r * p2.f; p2.e = r * p1.f; implementation_end; MSe 624 88 description ' 4.2 1 False Bond Graph\MSe.emx 2011-11-29 16:12:33 '; type MSe ports power out p; signal in effort; restrictions causality fixed out p; end; icon bg bottom figures text 'MSe' 624 88 color 0 18 bold; end; implementation eq variables real flow; equations p.e = effort; flow = p.f; implementation_end; plug input 424 88; plug p2 624 220; OneJunction2 624 136 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [1]; signal knot out flow [1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 624 136 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; R 664 136 description ' 4.2 1 False Bond Graph\R.emx 2011-11-29 16:35:37 '; type R ports power in p; end; icon bg bottom figures text 'R' 664 136 color 0 18 bold; end; implementation eq parameters real r = 0.08; equations p.e = r * p.f; implementation_end; SignalLimiter2 488 88 description '4.01False Signal\Block Diagram Non-Linear\SignalLimiter-Limit.emx 2007-9-26 12:47:40 '; type 'SignalLimiter-Limit' ports signal in input; signal out output; end; icon bg bottom figures group rectangle 472 72 504 104 color 0 fill 15132390; line 487.9 76.5 487.9 101 color 0 fill 15132390; line 475 88.2 500.7 88.2 color 0 fill 15132390; spline 481.9 95.1 493.9 81.4 color 16711680 fill 15132390 width 2; spline 493.3 82.2 501.9 81.8 color 16711680 fill 15132390 width 2; spline 475 95.6 481.9 95.1 color 16711680 fill 15132390 width 2; end; end; implementation eq parameters real maximum = 1; real minimum = -1; equations output = limit (input, minimum, maximum); implementation_end; end; connections Gain1\output -> MSe\effort; GY\p2 => p2; input -> SignalLimiter2\input; MSe\p => OneJunction2\p; OneJunction2\p => GY\p1; R\p <= OneJunction2\p; SignalLimiter2\output -> Gain1\input; end; implementation_end; Zero 112 136 description '4.01False Signal\Sources\Zero.emx 2007-9-27 15:54:36 '; type Zero ports signal out output [4,4]; end; icon bg bottom figures rectangle 96.1 120 127.9 152 color 0 fill 15132390; text '0' 112 136 color 16711680 18 bold; end; implementation eq equations output = eye(4);implementation_end; ZeroJunction7 112 288 description ' 4.2 1 False Bond Graph\OneJunction.emx 2011-11-29 16:17:51 '; knot OneJunction ports power knot duplicatable none p [1]; signal knot out flow [1]; restrictions causality constraint one_out p; end; icon bg figures text '1' 112 288 color 0 18 bold; end; implementation eq equations sum (direct (p.e)) = 0; equal (collect (p.f)); flow = first (p.f); implementation_end; end; connections Base1\Hout -> new_joint2\input; Base2\Hout -> new_joint3\input; Base3\Hin <- Splitter1\output; Base3\Hout -> new_joint1\input; C1\p <= OneJunction7\p; C2\p <= OneJunction6\p; Integrate\input <- ZeroJunction7\flow; Integrate\output -> PlusMinus4\minus; Integrate1\output -> PlusMinus5\minus; inverse_kinematics1\angle1 -> Negate1\input; inverse_kinematics1\angle2 -> Negate2\input; Joint1\Hout -> Link\Hin; Joint1\Pdiff <= PowerMux\output; Joint1\Pin <= Link\Pout; Joint1\Pout => OneJunction2\p; Joint2\Hout -> Link2\Hin; Joint2\Pdiff <= PowerMux1\output; Joint2\Pin <= Link2\Pout; Link\Pin <= OneJunction1\p; Link1\Hout -> Joint2\Hin; Link1\Pin <= Joint2\Pout; Link1\Pout => OneJunction1\p; Link2\Hout -> Splitter2\input; Link2\Pin <= OneJunction3\p; Link3\Hout -> Base2\Hin; Link3\Pin <= Base2\p; Link3\Pout => OneJunction3\p; Negate1\output -> PlusMinus4\plus; Negate2\output -> PlusMinus5\plus; OneJunction1\p <= Base3\p; OneJunction17\flow -> Integrate1\input; OneJunction3\p <= Base1\p; OneJunction4\p => C4\p; OneJunction4\p => R4\p; OneJunction5\p => C3\p; OneJunction5\p => R2\p; OneJunction6\p => R3\p; OneJunction7\p => R5\p; PID2\output -> Submodel2\input; PID3\output -> Submodel5\input; PlusMinus4\output -> PID2\error; PlusMinus5\output -> PID3\error; PowerMux\input_pos3 <= OneJunction6\p; PowerMux\input_rot2 <= OneJunction5\p; PowerMux1\input <= OneJunction17\p; PowerMux1\input_pos3 <= OneJunction7\p; PowerMux1\input_rot2 <= OneJunction4\p; R\p <= ZeroJunction7\p; R1\p <= OneJunction17\p; rectanglepath\output -> inverse_kinematics1\input; Sf2\p => OneJunction2\p; Splitter1\input <- Link\Hout; Splitter1\output -> Link1\Hin; Splitter2\output -> Base1\Hin; Splitter2\output -> Link3\Hin; Submodel2\p2 => ZeroJunction7\p; Submodel5\p2 => OneJunction17\p; Zero\output -> Joint1\Hin; ZeroJunction7\p => PowerMux\input; end; implementation_end; ]]> Experiment 1 4.8 Base1\InertialTensor\state_initial 6 1 0 0 0 0 0 0 Base3\InertialTensor\state_initial 6 1 0 0 0 0 0 0 C1\state_initial 3 1 0 0 0 C2\state_initial 3 1 0 0 0 C3\state_initial 2 1 0 0 C4\state_initial 2 1 0 0 Joint1\Hmatrix\p_initial 3 1 0 0 0 Joint2\Hmatrix\p_initial 3 1 0 0 0 PID2\pdstate_initial 0 PID2\pistate_initial 0 PID3\pdstate_initial 0 PID3\pistate_initial 0 Base2\InertialTensor\p.e_initial 6 1 0 0 0 0 0 0 time new_joint1\position[1] new_joint1\position[2] new_joint1\position[3] new_joint1\R[1,3] new_joint1\R[2,3] new_joint1\R[3,3] new_joint1\R[1,2] new_joint1\R[2,2] new_joint1\R[3,2] new_joint2\R[1,3] new_joint2\R[2,3] new_joint2\R[3,3] new_joint2\R[1,2] new_joint2\R[2,2] new_joint2\R[3,2] new_joint2\position[1] new_joint2\position[2] new_joint2\position[3] PID2\error PID3\error new_joint3\position[3] new_joint3\position[2] rectanglepath\output[2] rectanglepath\output[1] Submodel3\body_position[1] Submodel3\body_position[2] Submodel3\body_position[3] Submodel3\body_angle[1] Submodel3\body_angle[2] Submodel3\body_angle[3] Submodel3\dimension[1] Submodel3\dimension[2] Submodel3\dimension[3] Submodel4\body_position[1] Submodel4\body_position[2] Submodel4\body_position[3] Submodel4\body_angle[1] Submodel4\body_angle[2] Submodel4\body_angle[3] Submodel4\dimension[1] Submodel4\dimension[2] Submodel4\dimension[3] D3DPlot 1 false 16777215 true 3D Animation 137 true false 4294967295 Gradients\BlueWhite.png true 1.0 1.0 1 1 1 true Reference Frame Bryant false false false false false false 1 1 1 false Default Lights and Cameras Bryant false false false false false false false Ambient 1 1 Direct3D false false false false false false 0 0.3 0.3 0.3 1 1 1 true 0.3 0.3 0.3 1 1 1 true true false false false Parallel -3 5 3 0.457495710997814 -0.762492851663023 -0.457495710997814 0.235379601434674 -0.392299335724456 0.889211827642101 Direct3D false false false false false false 3 0.5 0.5 0.5 1 1 1 true 1.0 0.0 0.0 0.5 0.5 0.5 1 1 1 true 0.5 0.5 0.5 1 1 1 true false true true false Spot Light 1 -3 -5 1 0.50709255283711 0.845154254728517 -0.169030850945703 0.0869656553478673 0.144942758913112 0.985610760609162 Direct3D false false false false false false 2 0.5 0.5 0.5 1 1 1 true 1.0 0.05 0.05 1.0471975511965976 1.0471975511965976 0.0 0.5 0.5 0.5 1 1 1 true 0.5 0.5 0.5 1 1 1 true false true true false Spot Light 2 2 -3 -1 -0.534522483824849 0.801783725737273 0.267261241912424 0.14824986333222 -0.22237479499833 0.963624111659432 Direct3D false false false false false false 2 1 1 1 1 1 1 true 1.0 0.05 0.05 1.5707963267948966 1.5707963267948966 0.0 1 1 1 1 1 1 true 0.5 0.5 0.5 1 1 1 true false true true false Camera Looking at Origin 3.45165288156629 0.132202570606362 1.10375172903332 -0.951853185182919 -0.0364571532071126 -0.304378694781954 -0.304155680683107 -0.0116495384184135 0.952551106325972 Direct3D false false false false false false 0.01 100.0 true 0.003926990816987242 45.0 -10.0 10.0 10.0 -10.0 true -9.99200722162641e-016 1.76247905159244e-015 -3.05678199886544e-015 true 1 true true true false Front(XY)-Camera 10 -1 1 Direct3D false false false false false false 0.01 100.0 true 9.999999999999998 45.0 -9.999999999999998 9.999999999999998 9.999999999999998 -9.999999999999998 true true 1 true false false false Side(YZ)-Camera 10 -1 1 Direct3D false false false false false false 0.01 100.0 true 9.999999999999998 45.0 -9.999999999999998 9.999999999999998 9.999999999999998 -9.999999999999998 true true 1 true false false false Top(XZ)-Camera 10 -1 1 Direct3D false false false false false false 0.01 100.0 true 9.999999999999998 45.0 -9.999999999999998 9.999999999999998 9.999999999999998 -9.999999999999998 true true 1 true false false 1 1 1 false Scenery Bryant false false false false false false 10 10 10 false Reference Frame Bryant false false false false false false
Center
1.0 true 4 Submodel3\dimension[1] 0.005 Submodel3\dimension[2] 0.065 Submodel3\dimension[3] 0.01 true 1 1 1 1.0 1 1 1 true 0.498039215686275 0.498039215686275 0.498039215686275 14.298713684082 false false Block Submodel3\body_position[1] 0.0 Submodel3\body_position[2] -0.009158353632383561 Submodel3\body_position[3] 0.03118053280824738 Submodel3\body_angle[1] 1.8564889888314844 Submodel3\body_angle[2] 0.0 Submodel3\body_angle[3] 0.0 Euler false false false false false false
Center
1.0 true 4 Submodel4\dimension[1] 0.005 Submodel4\dimension[2] 0.05 Submodel4\dimension[3] 0.01 true 1 1 1 1.0 1 1 1 true 0.5 0.5 0.5 15 false false Block Submodel4\body_position[1] 0.0 Submodel4\body_position[2] 0.0013903741642251554 Submodel4\body_position[3] 0.07774345829579168 Submodel4\body_angle[1] 0.66277054304191 Submodel4\body_angle[2] 0.0 Submodel4\body_angle[3] 0.0 Euler false false false false false false
10 10 10 false Reference Frame 0.5 Bryant false false false false false false
Center
1.0 true 4 Submodel3\dimension[1] 0.005 Submodel3\dimension[2] 0.065 Submodel3\dimension[3] 0.01 true 1 1 1 1.0 1 1 1 true 0.498039215686275 0.498039215686275 0.498039215686275 14.298713684082 false false Block new_joint1\position[1] 0.0 new_joint1\position[2] -0.013912384738414135 new_joint1\position[3] 0.03060231153051999 new_joint1\R[1,3] 0.0 new_joint1\R[2,3] -0.906373184815434 new_joint1\R[3,3] -0.4224779874118024 new_joint1\R[1,2] 0.0 new_joint1\R[2,2] -0.4224779874118024 new_joint1\R[3,2] 0.906373184815434 Matrix false false false false false false
Center
1.0 true 4 Submodel4\dimension[1] 0.005 Submodel4\dimension[2] 0.05 Submodel4\dimension[3] 0.01 true 1 1 1 1.0 1 1 1 true 0.5 0.5 0.5 15 false false Block new_joint2\position[1] 0.0 new_joint2\position[2] -0.007082947566976128 new_joint2\position[3] 0.0476258741006878 new_joint2\R[1,3] 0.0 new_joint2\R[2,3] 0.5291132430096096 new_joint2\R[3,3] 0.8485512218315721 new_joint2\R[1,2] 0.0 new_joint2\R[2,2] 0.8485512218315721 new_joint2\R[3,2] -0.5291132430096096 Matrix false false false false false false
GraphPlot 2 false 16777215 true true 15780518 12624260 0 10 10 10 false 16777215 true 1 model(2) true Arial 12 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 Arial 10 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 true true false -0.04000000000000001 0.15999999999999998 true 1 -0.010000000000000009 0.09 true 1 -0.01 0.09 true 1 3355111 1 3355111 0 true 1 1 1 true true new_joint3\position[2] true new_joint3\position[3] 6076255 1 6076255 0 true 1 1 1 true true rectanglepath\output[1] true rectanglepath\output[2] true 0 16777215 GraphPlot 3 false 16777215 true true 15780518 12624260 0 10 10 10 false 16777215 true 1 Plot true Arial 12 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 Arial 10 34 400 0 0 0 0 Arial 12 34 400 0 0 0 0 true true false 0.0 10.0 true 3 -5.0 5.0 true 2 -1885.20460252739 0.0 true 2 3355111 1 3355111 0 true 1 1 1 true true time true PID2\error 6076255 1 6076255 0 true 1 1 1 true true time true PID3\error true 0 16777215
1 true Window 1 0 2 Base 2 true Window 2 0 1 Base 3 true Window 3 0 3 Base -0.00208333 0.00462963 1.00208 1.0037 0.138021 0.169444 0.777604 0.862963 0.526042 0.287963 0.922396 0.725926
0.0 10.0 false false false false 0.1 1.0e-6 1.0e-7 false true Euler 0.01 false BackwardEuler 1.0e-5 1.0e-5 1.0e-5 1.0e-5 0.01 1.0 AdamsBashforth 0.01 false RungeKutta2 0.01 false RungeKutta4 0.01 false RungeKutta8 false 0.0 false 0.0 1.0e-6 1.0e-6 0.9 0.33 6.0 0.0 false 100000 false 1000 RungeKuttaFehlberg false 0.0 false 0.0 1.0e-6 1.0e-6 VodeAdams false 0.0 false 0.0 1.0e-6 1.0e-6 true true BDFMethod 1.0e-5 1.0e-5 1.0e-5 1.0e-5 false 0.0 false 0.0 MeBDFiMethod 1.0e-5 1.0e-5 1.0e-5 1.0e-5 false 0.0 false 0.0 8 10 false true true false true 0 0.0 true MultipleRun true UseEndValue 0.001 BroydonFletcherGoldfarbShanno true true true true false