From fd5313703590445b6fc00d000c9954a83c8acb4d Mon Sep 17 00:00:00 2001 From: Wouter Horlings Date: Thu, 23 Jul 2020 16:01:31 +0200 Subject: [PATCH] fixup! Add complex arm model version 2 --- implementation/SCARA/06_arm/arm_model-v2.emx | 3077 ++++++++++++------ 1 file changed, 2102 insertions(+), 975 deletions(-) diff --git a/implementation/SCARA/06_arm/arm_model-v2.emx b/implementation/SCARA/06_arm/arm_model-v2.emx index 0a4b528..f6f2826 100644 --- a/implementation/SCARA/06_arm/arm_model-v2.emx +++ b/implementation/SCARA/06_arm/arm_model-v2.emx @@ -8,14 +8,14 @@ 0 False C:\users\wouter\My Documents\studie\ma\mahd\implementation\SCARA\06_arm\arm_model-v2.emx - 2020-7-23 13:55:45 + 2020-7-23 16:01:17 '; - type Mainmodel + type 'Mainmodel-v2' end; implementation bg submodels - Body1 720 464 + Body1 472 336 description '4.0 Template\Submodel-Equation.emx 1 @@ -29,7 +29,7 @@ end; implementation eq parameters - real dimension[3] = [0.005;0.01;0.065] {m}; + real dimension[3] = [0.0048;0.009;0.065] {m}; variables real position[3]; real rotation[3]; @@ -63,7 +63,7 @@ equations rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); //[input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; - Body3 272 448 + Body3 104 312 description '4.0 Template\Submodel-Equation.emx 1 @@ -77,7 +77,7 @@ equations end; implementation eq parameters - real dimension[3] = [0.005;0.01;0.065] {m}; + real dimension[3] = [0.0045;0.01;0.065] {m}; variables real position[3]; real rotation[3]; @@ -87,7 +87,7 @@ equations rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); //[input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; - Body4 680 192 + Body4 480 192 description '4.0 Template\Submodel-Equation.emx 1 @@ -111,7 +111,7 @@ equations rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); //[input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; - Body5 848 224 + Body5 992 224 description '4.0 Template\Submodel-Equation.emx 1 @@ -135,7 +135,7 @@ equations rotation = dll('EulerAngles.dll','EulXYZsFromHMatrix',input); //[input[3,2];input[1,3];input[2,1]]; R = input[1:3,1:3];implementation_end; - Body6 960 224 + Body6 1104 224 description '4.0 Template\Submodel-Equation.emx 1 @@ -824,10 +824,18 @@ implementation_end; ZeroJunction1\p => Ta0j\p; end; implementation_end; - COM_body4 560 48 + COM_body4 712 48 description '4.81Bond Graph\MR\center_of_mass_v2.emx2020-7-21 12:24:38parameters - real I [3,1] = [7.583333333333335e-7; 3.645833333333334e-8; 7.364583333333335e-7] {N.m.s}; - real m = 0.0035 {kg};'; + real density = 1400 {kg/m3}; + real dimension[3] = [0.005;0.075;0.01] {m}; +variables + real I[3,1] {N.m.s}; + real m {kg}; +initialequations + m = dimension[1] * dimension[2] * dimension[3] * density; + I[1] = m/12 * ( dimension[2]^2 + dimension[3]^2); + I[2] = m/12 * ( dimension[1]^2 + dimension[3]^2); + I[3] = m/12 * ( dimension[1]^2 + dimension[2]^2);'; type Submodel ports signal in Hin [4,4]; @@ -836,11 +844,11 @@ implementation_end; end; icon bg bottom figures - rectangle 528 32 592 64 color 0 fill 139; - text 'COM' 560 48 color 0 'Clear Sans' 16; + rectangle 680 32 744 64 color 0 fill 139; + text 'COM' 712 48 color 0 'Clear Sans' 16; terminals - Hin 576 64 fixed; - p 544 64 fixed; + Hin 728 64 fixed; + p 696 64 fixed; end; implementation bg submodels @@ -871,7 +879,7 @@ code p2.e = transpose(Adjoint(onlyRotH)) * p1.e; p1.f = Adjoint(onlyRotH) * p2.f; implementation_end; - EJS 616 336 + EJS 624 336 description '4.01False2007-10-31 11:43:6True'; type MGY ports @@ -879,14 +887,13 @@ implementation_end; end; icon bg top figures - text 'MGY' 616 336 color 0 18 bold; + text 'MGY' 624 336 color 0 18 bold; end; implementation eq //EJS / Gyroscopic effects -parameters +variables real global I[3]; real global m; -variables real II[6,6]; //Inertial tensor real Q[6,6]; real QI[6,6]; @@ -912,15 +919,14 @@ equations text 'Se' 624 400 color 0 18 bold; end; implementation eq -parameters - real global m; variables + real global m; real effort[6]; equations effort = [0;0;0;0;0;-g_n*m]; p.e = effort; implementation_end; - InertialTensor 544 272 + InertialTensor 544 256 description '4.01False Bond Graph\3D\I-3.emx 2007-9-25 12:12:14 @@ -934,13 +940,12 @@ implementation_end; end; icon bg bottom figures - text 'I' 544 272 color 0 18 bold; + text 'I' 544 256 color 0 18 bold; end; implementation eq -parameters +variables real global I[3]; real global m; -variables real II[6,6]; //Inertial tensor real Ia[6]; initialequations @@ -1044,7 +1049,7 @@ implementation_end; ZeroJunction1\p => Ta0j\p; end; implementation_end; - COM_body5 992 128 + COM_body5 1184 128 description '4.81Bond Graph\MR\center_of_mass_v2.emx2020-7-21 12:24:38parameters real I [3,1] = [0.018;0.159;0.159] {mN.m.s}; real m = 0.015 {kg};'; @@ -1056,11 +1061,11 @@ implementation_end; end; icon bg bottom figures - rectangle 976 96 1008 160 color 0 fill 139; - text 'COM' 992 128 color 0 'Clear Sans' 16; + rectangle 1168 96 1200 160 color 0 fill 139; + text 'COM' 1184 128 color 0 'Clear Sans' 16; terminals - Hin 976 144 fixed; - p 976 112 fixed; + Hin 1168 144 fixed; + p 1168 112 fixed; end; implementation bg submodels @@ -1264,7 +1269,7 @@ implementation_end; ZeroJunction1\p => Ta0j\p; end; implementation_end; - Ground1 620 727.5 + Ground1 620 687.5 description ' 4.8 @@ -1280,22 +1285,22 @@ implementation_end; end; icon bg bottom figures - line 584 720 600 736 color 0 fill 15132390; - line 592 720 608 736 color 0 fill 15132390; - line 576 720 592 736 color 0 fill 15132390; - line 616 736 600 720 color 0 fill 15132390; - line 624 736 608 720 color 0 fill 15132390; - line 632 736 616 720 color 0 fill 15132390; - line 648 736 632 720 color 0 fill 15132390; - line 640 736 624 720 color 0 fill 15132390; - line 656 736 640 720 color 0 fill 15132390; - line 664 736 648 720 color 0 fill 15132390; - line 576 720 664 720 color 0 fill 15132390 width 2; - line 656 720 664 728 color 0 fill 15132390; - line 576 728 584 736 color 0 fill 15132390; + line 584 680 600 696 color 0 fill 15132390; + line 592 680 608 696 color 0 fill 15132390; + line 576 680 592 696 color 0 fill 15132390; + line 616 696 600 680 color 0 fill 15132390; + line 624 696 608 680 color 0 fill 15132390; + line 632 696 616 680 color 0 fill 15132390; + line 648 696 632 680 color 0 fill 15132390; + line 640 696 624 680 color 0 fill 15132390; + line 656 696 640 680 color 0 fill 15132390; + line 664 696 648 680 color 0 fill 15132390; + line 576 680 664 680 color 0 fill 15132390 width 2; + line 656 680 664 688 color 0 fill 15132390; + line 576 688 584 696 color 0 fill 15132390; terminals - p 600 720 fixed; - output 632 720 fixed; + p 600 680 fixed; + output 632 680 fixed; end; implementation bg submodels @@ -1377,7 +1382,7 @@ equations Zero\output -> output; end; implementation_end; - inverse_kinematics 784 784 + inverse_kinematics 712 768 description ' 4.8 @@ -2495,7 +2500,7 @@ implementation_end; Wbai\p => Pout; end; implementation_end; - JointF 720 128 + JointF 864 128 description ' 4.8 @@ -2515,13 +2520,13 @@ implementation_end; end; icon bg bottom figures - rectangle 696 96 744 160 color 0 fill 14745599; - text 'Joint' 720 128 color 0 'Clear Sans' 16; + rectangle 840 96 888 160 color 0 fill 14745599; + text 'Joint' 864 128 color 0 'Clear Sans' 16; terminals - Pin 744 112 fixed; - Hin 696 144 fixed; - Hout 744 144 fixed; - Pout 696 112 fixed; + Pin 888 112 fixed; + Hin 840 144 fixed; + Hout 888 144 fixed; + Pout 840 112 fixed; end; implementation bg submodels @@ -2710,7 +2715,7 @@ implementation_end; Wbai\p => Pout; end; implementation_end; - Joint_dynA 704 512 + Joint_dynA 712 512 description ' 4.8 1 @@ -3532,7 +3537,7 @@ variables real global p; real angle {rad}; equations - angle = int(omega,-1.57); + angle = int(omega,1.570796); output_a = -p * fluxLinkage * sin(p * angle); output_b = p * fluxLinkage * sin(p * angle - pi / 2); output_d = detentTorque * sin(2 * p * angle); @@ -3630,13 +3635,13 @@ implementation_end; OneJunction7\p => PowerMux\input_pos3; OneJunction7\p => R5\p; OneJunction8\p => R\p; + OneJunction8\p => ZeroJunction1\p; PowerMux\input <= ZeroJunction1\p; PowerMux\output => output; - ZeroJunction1\p <= OneJunction8\p; ZeroJunction1\p <= stepper_model\p; end; implementation_end; - Joint_dynB 456 192 + Joint_dynB 672 192 description ' 4.8 1 @@ -3645,6 +3650,11 @@ implementation_end; ports power out output [6,1]; end; + icon bg + figures + rectangle 632 176 712 208 color 0 fill 15132390; + text 'name' 672 192 color 0 'Clear Sans' 16; + end; implementation bg submodels C1 191.8 216.2 @@ -3665,7 +3675,7 @@ implementation_end; 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] {mN/m}; + real c[3,3] = [1, 0.0, 0.0; 0.0, 1, 0.0; 0.0, 0.0, 1] {uN/m}; equations state = int(p.f); p.e = inverse(c) *state; @@ -3688,7 +3698,7 @@ implementation_end; end; implementation eq parameters - real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; + real c[2,2] = [1, 0.0; 0.0, 1] {uN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; @@ -4904,9 +4914,9 @@ implementation_end; OneJunction7\p => PowerMux\input_pos3; OneJunction7\p => R5\p; OneJunction8\p => R\p; + OneJunction8\p => ZeroJunction1\p; PowerMux\output => output; stepper_model\p => ZeroJunction1\p; - ZeroJunction1\p => OneJunction8\p; ZeroJunction1\p => PowerMux\input; end; implementation_end; @@ -4939,7 +4949,7 @@ implementation_end; 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] {mN/m}; + real c[3,3] = [1, 0.0, 0.0; 0.0, 1, 0.0; 0.0, 0.0, 1] {uN/m}; equations state = int(p.f); p.e = inverse(c) *state; @@ -4962,7 +4972,7 @@ implementation_end; end; implementation eq parameters - real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; + real c[2,2] = [1, 0.0; 0.0, 1] {uN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; @@ -5262,6 +5272,11 @@ implementation_end; ports power out output [6,1]; end; + icon bg + figures + rectangle 48 152 128 184 color 0 fill 15132390; + text 'name' 88 168 color 0 'Clear Sans' 16; + end; implementation bg submodels C1 191.8 216.2 @@ -5282,7 +5297,7 @@ implementation_end; 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] {mN/m}; + real c[3,3] = [1, 0.0, 0.0; 0.0, 1, 0.0; 0.0, 0.0, 1] {uN/m}; equations state = int(p.f); p.e = inverse(c) *state; @@ -5305,7 +5320,7 @@ implementation_end; end; implementation eq parameters - real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; + real c[2,2] = [1, 0.0; 0.0, 1] {uN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; @@ -5596,7 +5611,7 @@ implementation_end; R\p <= OneJunction8\p; end; implementation_end; - Joint_dynF 720 48 + Joint_dynF 864 48 description ' 4.8 1 @@ -5605,36 +5620,13 @@ implementation_end; ports power out output [6,1]; end; + icon bg + figures + rectangle 824 32 904 64 color 0 fill 15132390; + text 'name' 864 48 color 0 'Clear Sans' 16; + end; implementation bg submodels - C 264 200 - description ' - - 4.2 -1 - False - Bond Graph\C.emx - 2011-11-29 15:58:35 - -'; - type C - ports - power in p; - signal out state; - restrictions - causality preferred out p; - end; - icon bg bottom - figures - text 'C' 264 200 color 0 18 bold; - end; - implementation eq -parameters - real c = 0.001; -equations - state = int(p.f); - p.e = state / c; -implementation_end; C1 191.8 216.2 description '4.01False Bond Graph\3D\C-3.emx @@ -5653,7 +5645,7 @@ implementation_end; 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] {mN/m}; + real c[3,3] = [1, 0.0, 0.0; 0.0, 1, 0.0; 0.0, 0.0, 1] {uN/m}; equations state = int(p.f); p.e = inverse(c) *state; @@ -5676,39 +5668,12 @@ implementation_end; end; implementation eq parameters - real c[2,2] = [0.1, 0.0; 0.0, 0.1] {mN.m/rad}; + real c[2,2] = [1, 0.0; 0.0, 1] {uN.m/rad}; equations state = int(p.f); p.e = inverse(c) *state; implementation_end; plug output 396 216; - OneJunction 328 200 - 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' 328 200 color 0 18 bold; - end; - implementation eq -equations - sum (direct (p.e)) = 0; - equal (collect (p.f)); - flow = first (p.f); -implementation_end; OneJunction5 239.8 232.2 description ' @@ -5763,7 +5728,7 @@ equations equal (collect (p.f)); flow = first (p.f); implementation_end; - OneJunction8 296 200 + OneJunction8 288 200 description ' 4.2 @@ -5782,7 +5747,7 @@ implementation_end; end; icon bg figures - text '1' 296 200 color 0 18 bold; + text '1' 288 200 color 0 18 bold; end; implementation eq equations @@ -5790,10 +5755,10 @@ equations equal (collect (p.f)); flow = first (p.f); implementation_end; - PowerMux 362 216 + PowerMux 330 216 specifications active 'rot_z' specification 'rot_x' - description ' +description ' 4.8 Bond Graph\MR\PowerMux-Rotation.emx @@ -5816,14 +5781,14 @@ implementation_end; end; icon bg bottom figures - line 360 192 360 240 color 0 width 2; - rectangle 356 192 368 240 color -1; - text '1' 365 210 color 8421504 8; + line 328 192 328 240 color 0 width 2; + rectangle 324 192 336 240 color -1; + text '1' 333 210 color 8421504 8; terminals - input 360 200 fixed; - output 360 216 fixed; - input_rot2 360 232 fixed; - input_pos3 360 216 fixed; + input 328 200 fixed; + output 328 216 fixed; + input_rot2 328 232 fixed; + input_pos3 328 216 fixed; end; implementation eq equations @@ -5900,14 +5865,14 @@ specification_end; end; icon bg bottom figures - line 360 192 360 240 color 0 width 2; - rectangle 356 192 368 240 color -1; - text '1' 365 210 color 8421504 8; + line 328 192 328 240 color 0 width 2; + rectangle 324 192 336 240 color -1; + text '1' 333 210 color 8421504 8; terminals - input 360 200 fixed; - output 360 216 fixed; - input_rot2 360 232 fixed; - input_pos3 360 216 fixed; + input 328 200 fixed; + output 328 216 fixed; + input_rot2 328 232 fixed; + input_pos3 328 216 fixed; end; implementation eq equations @@ -5919,7 +5884,7 @@ equations output.f[4:6] = input_pos3.f;implementation_end; specification_end; end; - R 296 120 + R 288 152 description ' 4.2 @@ -5935,11 +5900,11 @@ specification_end; end; icon bg top figures - text 'R' 296 120 color 0 18 bold; + text 'R' 288 152 color 0 18 bold; end; implementation eq parameters - real r = 1000; + real r = 0.0001; equations p.e = r * p.f; implementation_end; @@ -5983,15 +5948,13 @@ equations implementation_end; end; connections - C\p <= OneJunction8\p; C1\p <= OneJunction7\p; - OneJunction\p => PowerMux\input; OneJunction5\p => C4\p; OneJunction5\p => PowerMux\input_rot2; OneJunction5\p => R4\p; OneJunction7\p => PowerMux\input_pos3; OneJunction7\p => R5\p; - OneJunction8\p => OneJunction\p; + OneJunction8\p => PowerMux\input; PowerMux\output => output; R\p <= OneJunction8\p; end; @@ -6028,7 +5991,56 @@ implementation_end; end; implementation eq parameters - real offset[6]= [0;0;0;0;0.025;0]; //coordinates of joint_1 + real offset[6]= [0;0;0;0;0.0125;0]; //coordinates of joint_1 +variables + real Hab[4,4]; + real AdHab[6,6]; + real R[3,3]; + real omega[3]; +initialequations + omega = offset[1:3]; + R = dll('EulerAngles.dll','RotationMatrixFromEulXYZs',omega); + Hab = homogeneous(R,offset[4:6]); + AdHab = Adjoint(Hab); +equations + Hout = Hin * Hab; + Pout.e = transpose(AdHab) * Pin.e; + Pin.f = AdHab * Pout.f; + + implementation_end; + Link10 944 128 + description ' + + 4.8 + Bond Graph\MR\link-v3.emx +1 + False + 2020-7-21 11:14:41 +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 912 104 976 152 color 0 fill 8454041; + text 'Link' 944 128 color 0 'Clear Sans' 16; + terminals + Hin 912 144 fixed; + Hout 976 144 fixed; + Pin 976 112 fixed; + Pout 912 112 fixed; + end; + implementation eq +parameters + real offset[6]= [0;0;0;0;0.0125;0]; //coordinates of joint_1 variables real Hab[4,4]; real AdHab[6,6]; @@ -6045,7 +6057,7 @@ equations Pin.f = AdHab * Pout.f; implementation_end; - Link10 800 128 + Link11 784 128 description ' 4.8 @@ -6067,13 +6079,13 @@ equations end; icon bg bottom figures - rectangle 768 104 832 152 color 0 fill 8454041; - text 'Link' 800 128 color 0 'Clear Sans' 16; + rectangle 752 104 816 152 color 0 fill 8454041; + text 'Link' 784 128 color 0 'Clear Sans' 16; terminals - Hin 768 144 fixed; - Hout 832 144 fixed; - Pin 832 112 fixed; - Pout 768 112 fixed; + Hin 752 144 fixed; + Hout 816 144 fixed; + Pin 816 112 fixed; + Pout 752 112 fixed; end; implementation eq parameters @@ -6388,7 +6400,7 @@ equations Pin.f = AdHab * Pout.f; implementation_end; - Link8 904 128 + Link8 1048 128 description ' 4.8 @@ -6410,13 +6422,13 @@ equations end; icon bg bottom figures - rectangle 872 104 936 152 color 0 fill 8454041; - text 'Link' 904 128 color 0 'Clear Sans' 16; + rectangle 1016 104 1080 152 color 0 fill 8454041; + text 'Link' 1048 128 color 0 'Clear Sans' 16; terminals - Hin 872 144 fixed; - Hout 936 144 fixed; - Pin 936 112 fixed; - Pout 872 112 fixed; + Hin 1016 144 fixed; + Hout 1080 144 fixed; + Pin 1080 112 fixed; + Pout 1016 112 fixed; end; implementation eq parameters @@ -6486,26 +6498,7 @@ equations Pin.f = AdHab * Pout.f; implementation_end; - Negate1 704 696 - 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 688 680 720 712 color 0 fill 15132390; - text '-1' 704 696 color 16711680 16 bold; - end; - implementation eq -equations - output = - input; - implementation_end; - Negate2 624 784 + Negate2 608 768 description '4.01False Signal\Block Diagram\Negate.emx 2007-9-26 12:14:11 @@ -6517,8 +6510,8 @@ equations end; icon bg bottom figures - rectangle 608 768 640 800 color 0 fill 15132390; - text '-1' 624 784 color 16711680 16 bold; + rectangle 592 752 624 784 color 0 fill 15132390; + text '-1' 608 768 color 16711680 16 bold; end; implementation eq equations @@ -6659,7 +6652,34 @@ equations equal (collect (p.f)); flow = first (p.f); implementation_end; - rectanglepath 1024 784 + OneJunction6 696 112 + 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' 696 112 color 0 18 bold; + end; + implementation eq +equations + sum (direct (p.e)) = 0; + equal (collect (p.f)); + flow = first (p.f); +implementation_end; + rectanglepath 856 768 description ' 4.8 @@ -6676,8 +6696,8 @@ implementation_end; end; icon bg figures - rectangle 968 768 1080 800 color 0 fill 15132390; - text 'name' 1024 784 color 0 'Clear Sans' 16; + rectangle 800 752 912 784 color 0 fill 15132390; + text 'name' 856 768 color 0 'Clear Sans' 16; end; implementation eq /* @@ -6686,7 +6706,7 @@ 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 = 2.0 {s}; + real t = 1 {s}; real origin[2] = [0.01;0.03] {m}; variables real v {m/s}; @@ -6700,9 +6720,9 @@ initialequations t_h = h / v; code y = frequencyevent (t,0); -period = time + floor(time / t) * t; +period = t - (time - floor(time / t) * t); -if period < t_w or time < 4 then +if period < t_w or time < t then output = origin; else if period < t_w + t_h then @@ -6744,7 +6764,30 @@ implementation_end; equations collect (output) = input; implementation_end; - Splitter10 960 144 + Splitter10 1104 144 + 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 1100.8 140.8 1107.2 147.2 color -1 fill 0; + ellipse 1099.7 139.7 1108.3 148.3 color -1; + terminals + input 1104 144 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Splitter11 480 144 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 @@ -6758,10 +6801,10 @@ implementation_end; end; icon bg ellipse figures - ellipse 956.8 140.8 963.2 147.2 color -1 fill 0; - ellipse 955.7 139.7 964.3 148.3 color -1; + ellipse 476.8 140.8 483.2 147.2 color -1 fill 0; + ellipse 475.7 139.7 484.3 148.3 color -1; terminals - input 960 144 fixed; + input 480 144 fixed; end; implementation eq equations @@ -6836,7 +6879,7 @@ implementation_end; equations collect (output) = input; implementation_end; - Splitter7 216 448 + Splitter6 728 144 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 @@ -6850,16 +6893,16 @@ implementation_end; end; icon bg ellipse figures - ellipse 212.8 444.8 219.2 451.2 color -1 fill 0; - ellipse 211.7 443.7 220.3 452.3 color -1; + ellipse 724.8 140.8 731.2 147.2 color -1 fill 0; + ellipse 723.7 139.7 732.3 148.3 color -1; terminals - input 216 448 fixed; + input 728 144 fixed; end; implementation eq equations collect (output) = input; implementation_end; - Splitter8 576 464 + Splitter8 576 472 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 @@ -6873,16 +6916,16 @@ implementation_end; end; icon bg ellipse figures - ellipse 572.8 460.8 579.2 467.2 color -1 fill 0; - ellipse 571.7 459.7 580.3 468.3 color -1; + ellipse 572.8 468.8 579.2 475.2 color -1 fill 0; + ellipse 571.7 467.7 580.3 476.3 color -1; terminals - input 576 464 fixed; + input 576 472 fixed; end; implementation eq equations collect (output) = input; implementation_end; - Splitter9 848 144 + Splitter9 992 144 description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 @@ -6896,35 +6939,37 @@ implementation_end; end; icon bg ellipse figures - ellipse 844.8 140.8 851.2 147.2 color -1 fill 0; - ellipse 843.7 139.7 852.3 148.3 color -1; + ellipse 988.8 140.8 995.2 147.2 color -1 fill 0; + ellipse 987.7 139.7 996.3 148.3 color -1; terminals - input 848 144 fixed; + input 992 144 fixed; end; implementation eq equations collect (output) = input; implementation_end; - stepper_control 704 592 - description '4.81Z:\home\wouter\Documents\studie\ma\mahd\implementation\library\Motor\stepper_control.emx2020-7-23 13:09:57parameters + stepper_control 712 592 + specifications active 'P_only' + specification 'default' +description '4.81Z:\home\wouter\Documents\studie\ma\mahd\implementation\library\Motor\stepper_control.emx2020-7-23 13:09:57parameters real max_acceleration = 250 {rad/s2}; real max_velocity = 25 {rad/s}; real initial_velocity = 0 {rad/s}; - real initial_angle = -1.57 {rad};'; - type Submodel - ports - signal in setpoint {rad} ; - signal out output {rad} ; - end; - icon bg bottom - figures - rectangle 648 576 760 608 color 0 fill 15132390; - text 'name' 704 592 color 0 'Clear Sans' 16; - end; - implementation bg - submodels - Acceleration_int 648 336 - description ' + real initial_angle = -1.570796 {rad};'; + type Submodel + ports + signal in setpoint {rad} ; + signal out output {rad} ; + end; + icon bg bottom + figures + rectangle 648 576 760 608 color 0 fill 15132390; + text 'name' 704 592 color 0 'Clear Sans' 16; + end; + implementation bg + submodels + Acceleration_int 648 336 + description ' 4.3 1 @@ -6933,108 +6978,108 @@ implementation_end; 2013-3-8 14:47:42 '; - type Integrate - ports - signal in input; - signal out output; - end; - icon bg bottom - figures - rectangle 632 320 664 352 color 0 fill 15132390; - text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; - end; - implementation eq + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 632 320 664 352 color 0 fill 15132390; + text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq parameters real global initial_velocity; // initial value equations output = int (input, initial_velocity); implementation_end; - Acceleration_limit 568 336 - description '4.01False + Acceleration_limit 568 336 + 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 552 320 584 352 color 0 fill 15132390; - line 567.9 324.5 567.9 349 color 0 fill 15132390; - line 555 336.2 580.7 336.2 color 0 fill 15132390; - spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; - spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; - spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; - end; - end; - implementation eq + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 552 320 584 352 color 0 fill 15132390; + line 567.9 324.5 567.9 349 color 0 fill 15132390; + line 555 336.2 580.7 336.2 color 0 fill 15132390; + spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; + spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; + spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq parameters real global max_acceleration {rad/s2}; equations output = limit (input, -max_acceleration, max_acceleration); implementation_end; - plug setpoint 136 336; - plug output 791 336; - PlusMinus1 240 336 - description '4.01False + plug setpoint 136 336; + plug output 791 336; + PlusMinus1 240 336 + 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 232 328 248 344 color 0 fill 16777215; - end; - implementation eq + 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 232 328 248 344 color 0 fill 16777215; + end; + implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; - PlusMinus2 432 336 - description '4.01False + PlusMinus2 432 336 + 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 424 328 440 344 color 0 fill 16777215; - end; - implementation eq + 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 424 328 440 344 color 0 fill 16777215; + end; + implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; - Position_control 296 336 - description ' + Position_control 296 336 + description ' 4.0 1 False Signal\Control\PID Control\Continuous\PD.emx 2008-1-17 10:49:7 '; - type PD - ports - signal in error; - signal out output; - end; - icon bg bottom - figures - rectangle 280 320 312 352 color 0 fill 15132390; - text 'PD' 296.5 336.1 color 16711680 18 bold; - end; - implementation eq + type PD + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 280 320 312 352 color 0 fill 15132390; + text 'PD' 296.5 336.1 color 16711680 18 bold; + end; + implementation eq parameters real kp = 4 {}; // Proportional gain real tauD = 100 {s}; // Derivative time constant: tauD > 0 @@ -7046,71 +7091,71 @@ equations state = int (rate); output = state + kp * error / beta; implementation_end; - Splitter3 688 336 - description '4.0 + Splitter3 688 336 + description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; - knot Splitter - ports - signal knot duplicatable out output [1]; - signal knot in input [1]; - end; - icon bg ellipse - figures - ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; - ellipse 683.7 331.7 692.3 340.3 color -1; - terminals - input 688 336 fixed; - end; - implementation eq + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; + ellipse 683.7 331.7 692.3 340.3 color -1; + terminals + input 688 336 fixed; + end; + implementation eq equations collect (output) = input; implementation_end; - Splitter4 752 336 - description '4.0 + Splitter4 752 336 + description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; - knot Splitter - ports - signal knot duplicatable out output [1]; - signal knot in input [1]; - end; - icon bg ellipse - figures - ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; - ellipse 747.7 331.7 756.3 340.3 color -1; - terminals - input 752 336 fixed; - end; - implementation eq + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; + ellipse 747.7 331.7 756.3 340.3 color -1; + terminals + input 752 336 fixed; + end; + implementation eq equations collect (output) = input; implementation_end; - Velocity_control 480 336 - description ' + Velocity_control 480 336 + description ' 4.0 1 False Signal\Control\PID Control\Continuous\PD.emx 2008-1-17 10:49:7 '; - type PD - ports - signal in error; - signal out output; - end; - icon bg bottom - figures - rectangle 464 320 496 352 color 0 fill 15132390; - text 'PD' 480.5 336.1 color 16711680 18 bold; - end; - implementation eq + type PD + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 464 320 496 352 color 0 fill 15132390; + text 'PD' 480.5 336.1 color 16711680 18 bold; + end; + implementation eq parameters real kp = 12 {}; // Proportional gain real tauD = 10 {s}; // Derivative time constant: tauD > 0 @@ -7122,8 +7167,8 @@ equations state = int (rate); output = state + kp * error / beta; implementation_end; - Velocity_int 720 336 - description ' + Velocity_int 720 336 + description ' 4.3 1 @@ -7132,82 +7177,87 @@ equations 2013-3-8 14:47:42 '; - type Integrate - ports - signal in input; - signal out output; - end; - icon bg bottom - figures - rectangle 704 320 736 352 color 0 fill 15132390; - text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; - end; - implementation eq + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 704 320 736 352 color 0 fill 15132390; + text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq parameters real global initial_angle; // initial value equations output = int (input, initial_angle); implementation_end; - Velocity_limit 376 336 - description '4.01False + Velocity_limit 376 336 + 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 360 320 392 352 color 0 fill 15132390; - line 375.9 324.5 375.9 349 color 0 fill 15132390; - line 363 336.2 388.7 336.2 color 0 fill 15132390; - spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; - spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; - spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; - end; - end; - implementation eq + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 360 320 392 352 color 0 fill 15132390; + line 375.9 324.5 375.9 349 color 0 fill 15132390; + line 363 336.2 388.7 336.2 color 0 fill 15132390; + spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; + spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; + spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq parameters real global max_velocity {rad/s}; equations output = limit (input, -max_velocity, max_velocity); implementation_end; - end; - connections - Acceleration_int\output -> Splitter3\input; - Acceleration_limit\output -> Acceleration_int\input; - PlusMinus1\output -> Position_control\error; - PlusMinus2\output -> Velocity_control\error; - Position_control\output -> Velocity_limit\input; - setpoint -> PlusMinus1\plus; - Splitter3\output -> PlusMinus2\minus 688 288 432 288; - Splitter3\output -> Velocity_int\input; - Splitter4\output -> output; - Splitter4\output -> PlusMinus1\minus 752 400 240 400; - Velocity_control\output -> Acceleration_limit\input; - Velocity_int\output -> Splitter4\input; - Velocity_limit\output -> PlusMinus2\plus; - end; - implementation_end; - stepper_control1 512 784 - description '4.81Motor\stepper_control.emx2020-7-22 15:25:26parameters - real max_acceleration = 250 {rad/s2}; + end; + connections + Acceleration_int\output -> Splitter3\input; + Acceleration_limit\output -> Acceleration_int\input; + PlusMinus1\output -> Position_control\error; + PlusMinus2\output -> Velocity_control\error; + Position_control\output -> Velocity_limit\input; + setpoint -> PlusMinus1\plus; + Splitter3\output -> PlusMinus2\minus 688 288 432 288; + Splitter3\output -> Velocity_int\input; + Splitter4\output -> output; + Splitter4\output -> PlusMinus1\minus 752 400 240 400; + Velocity_control\output -> Acceleration_limit\input; + Velocity_int\output -> Splitter4\input; + Velocity_limit\output -> PlusMinus2\plus; + end; + implementation_end; +specification_end; + specification 'P_only' + description '4.81Z:\home\wouter\Documents\studie\ma\mahd\implementation\library\Motor\stepper_control.emx2020-7-23 13:09:57parameters + real max_acceleration = 350 {rad/s2}; real max_velocity = 25 {rad/s}; real initial_velocity = 0 {rad/s}; - real initial_angle = 0 {rad}; - '; - type Submodel - ports - signal in setpoint {rad} ; - signal out output {rad} ; - end; - implementation bg - submodels - Acceleration_int 648 336 - description ' + real initial_angle = 1.570796 {rad};'; + type Submodel + ports + signal in setpoint {rad} ; + signal out output {rad} ; + end; + icon bg bottom + figures + rectangle 656 576 768 608 color 0 fill 15132390; + text 'name' 712 592 color 0 'Clear Sans' 16; + end; + implementation bg + submodels + Acceleration_int 648 336 + description ' 4.3 1 @@ -7216,197 +7266,185 @@ equations 2013-3-8 14:47:42 '; - type Integrate - ports - signal in input; - signal out output; - end; - icon bg bottom - figures - rectangle 632 320 664 352 color 0 fill 15132390; - text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; - end; - implementation eq + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 632 320 664 352 color 0 fill 15132390; + text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq parameters real global initial_velocity; // initial value equations output = int (input, initial_velocity); implementation_end; - Acceleration_limit 568 336 - description '4.01False + Acceleration_limit 568 336 + 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 552 320 584 352 color 0 fill 15132390; - line 567.9 324.5 567.9 349 color 0 fill 15132390; - line 555 336.2 580.7 336.2 color 0 fill 15132390; - spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; - spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; - spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; - end; - end; - implementation eq + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 552 320 584 352 color 0 fill 15132390; + line 567.9 324.5 567.9 349 color 0 fill 15132390; + line 555 336.2 580.7 336.2 color 0 fill 15132390; + spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; + spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; + spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq parameters real global max_acceleration {rad/s2}; equations output = limit (input, -max_acceleration, max_acceleration); implementation_end; - plug setpoint 136 336; - plug output 791 336; - PlusMinus1 240 336 - description '4.01False + plug setpoint 136 336; + plug output 791 336; + PlusMinus1 240 336 + 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 232 328 248 344 color 0 fill 16777215; - end; - implementation eq + 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 232 328 248 344 color 0 fill 16777215; + end; + implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; - PlusMinus2 432 336 - description '4.01False + PlusMinus2 432 336 + 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 424 328 440 344 color 0 fill 16777215; - end; - implementation eq + 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 424 328 440 344 color 0 fill 16777215; + end; + implementation eq equations output = sum (collect (plus)) - sum (collect (minus)); implementation_end; - Position_control 296 336 - description ' + Position_control 296 336 + description ' 4.0 1 False - Signal\Control\PID Control\Continuous\PD.emx - 2008-1-17 10:49:7 + Signal\Control\PID Control\Continuous\P.emx + 2008-1-17 10:48:8 '; - type PD - ports - signal in error; - signal out output; - end; - icon bg bottom - figures - rectangle 280 320 312 352 color 0 fill 15132390; - text 'PD' 296.5 336.1 color 16711680 18 bold; - end; - implementation eq + type P + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 280 320 312 352 color 0 fill 15132390; + text 'P' 296.5 336.1 color 16711680 18 bold; + end; + implementation eq parameters - real kp = 4 {}; // Proportional gain - real tauD = 100 {s}; // Derivative time constant: tauD > 0 - real beta = 0.4 {}; // Tameness constant: 0 < beta << 1 -variables - real state, rate; + real kp = 26.0 {}; // Proportional gain equations - rate = (kp * error - output) / (beta * tauD); - state = int (rate); - output = state + kp * error / beta; - implementation_end; - Splitter3 688 336 - description '4.0 + output = kp * error; + implementation_end; + Splitter3 688 336 + description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; - knot Splitter - ports - signal knot duplicatable out output [1]; - signal knot in input [1]; - end; - icon bg ellipse - figures - ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; - ellipse 683.7 331.7 692.3 340.3 color -1; - terminals - input 688 336 fixed; - end; - implementation eq + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; + ellipse 683.7 331.7 692.3 340.3 color -1; + terminals + input 688 336 fixed; + end; + implementation eq equations collect (output) = input; implementation_end; - Splitter4 752 336 - description '4.0 + Splitter4 752 336 + description '4.0 Signal\Block Diagram\Splitter.emx 2008-01-17 11:28:29 1 False '; - knot Splitter - ports - signal knot duplicatable out output [1]; - signal knot in input [1]; - end; - icon bg ellipse - figures - ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; - ellipse 747.7 331.7 756.3 340.3 color -1; - terminals - input 752 336 fixed; - end; - implementation eq + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; + ellipse 747.7 331.7 756.3 340.3 color -1; + terminals + input 752 336 fixed; + end; + implementation eq equations collect (output) = input; implementation_end; - Velocity_control 480 336 - description ' + Velocity_control 480 336 + description ' 4.0 1 False - Signal\Control\PID Control\Continuous\PD.emx - 2008-1-17 10:49:7 + Signal\Control\PID Control\Continuous\P.emx + 2008-1-17 10:48:8 '; - type PD - ports - signal in error; - signal out output; - end; - icon bg bottom - figures - rectangle 464 320 496 352 color 0 fill 15132390; - text 'PD' 480.5 336.1 color 16711680 18 bold; - end; - implementation eq + type P + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 464 320 496 352 color 0 fill 15132390; + text 'P' 480.5 336.1 color 16711680 18 bold; + end; + implementation eq parameters - real kp = 12 {}; // Proportional gain - real tauD = 10 {s}; // Derivative time constant: tauD > 0 - real beta = 0.4 {}; // Tameness constant: 0 < beta << 1 -variables - real state, rate; + real kp = 66.0 {}; // Proportional gain equations - rate = (kp * error - output) / (beta * tauD); - state = int (rate); - output = state + kp * error / beta; - implementation_end; - Velocity_int 720 336 - description ' + output = kp * error; + implementation_end; + Velocity_int 720 336 + description ' 4.3 1 @@ -7415,163 +7453,735 @@ equations 2013-3-8 14:47:42 '; - type Integrate - ports - signal in input; - signal out output; - end; - icon bg bottom - figures - rectangle 704 320 736 352 color 0 fill 15132390; - text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; - end; - implementation eq + type Integrate + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + rectangle 704 320 736 352 color 0 fill 15132390; + text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq parameters real global initial_angle; // initial value equations output = int (input, initial_angle); implementation_end; - Velocity_limit 376 336 - description '4.01False + Velocity_limit 376 336 + 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 360 320 392 352 color 0 fill 15132390; - line 375.9 324.5 375.9 349 color 0 fill 15132390; - line 363 336.2 388.7 336.2 color 0 fill 15132390; - spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; - spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; - spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; - end; - end; - implementation eq + type 'SignalLimiter-Limit' + ports + signal in input; + signal out output; + end; + icon bg bottom + figures + group + rectangle 360 320 392 352 color 0 fill 15132390; + line 375.9 324.5 375.9 349 color 0 fill 15132390; + line 363 336.2 388.7 336.2 color 0 fill 15132390; + spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; + spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; + spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq parameters real global max_velocity {rad/s}; equations output = limit (input, -max_velocity, max_velocity); implementation_end; - end; - connections - Acceleration_int\output -> Splitter3\input; - Acceleration_limit\output -> Acceleration_int\input; - PlusMinus1\output -> Position_control\error; - PlusMinus2\output -> Velocity_control\error; - Position_control\output -> Velocity_limit\input; - setpoint -> PlusMinus1\plus; - Splitter3\output -> PlusMinus2\minus 688 288 432 288; - Splitter3\output -> Velocity_int\input; - Splitter4\output -> output; - Splitter4\output -> PlusMinus1\minus 752 400 240 400; - Velocity_control\output -> Acceleration_limit\input; - Velocity_int\output -> Splitter4\input; - Velocity_limit\output -> PlusMinus2\plus; - end; + end; + connections + Acceleration_int\output -> Splitter3\input; + Acceleration_limit\output -> Acceleration_int\input; + PlusMinus1\output -> Position_control\error; + PlusMinus2\output -> Velocity_control\error; + Position_control\output -> Velocity_limit\input; + setpoint -> PlusMinus1\plus; + Splitter3\output -> PlusMinus2\minus 688 288 432 288; + Splitter3\output -> Velocity_int\input; + Splitter4\output -> output; + Splitter4\output -> PlusMinus1\minus 752 400 240 400; + Velocity_control\output -> Acceleration_limit\input; + Velocity_int\output -> Splitter4\input; + Velocity_limit\output -> PlusMinus2\plus; + end; + implementation_end; +specification_end; + end; + stepper_control1 512 768 + specifications active 'P_only' + specification 'default' +description '4.81Z:\home\wouter\Documents\studie\ma\mahd\implementation\library\Motor\stepper_control.emx2020-7-23 13:09:57parameters + real max_acceleration = 250 {rad/s2}; + real max_velocity = 25 {rad/s}; + real initial_velocity = 0 {rad/s}; + real initial_angle = -1.570796 {rad};'; + type Submodel + ports + signal in setpoint {rad} ; + signal out output {rad} ; + end; + icon bg bottom + figures + rectangle 648 576 760 608 color 0 fill 15132390; + text 'name' 704 592 color 0 'Clear Sans' 16; + end; + implementation bg + submodels + Acceleration_int 648 336 + 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 632 320 664 352 color 0 fill 15132390; + text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real global initial_velocity; // initial value +equations + output = int (input, initial_velocity); + implementation_end; + Acceleration_limit 568 336 + 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 552 320 584 352 color 0 fill 15132390; + line 567.9 324.5 567.9 349 color 0 fill 15132390; + line 555 336.2 580.7 336.2 color 0 fill 15132390; + spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; + spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; + spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real global max_acceleration {rad/s2}; +equations + output = limit (input, -max_acceleration, max_acceleration); + implementation_end; + plug setpoint 136 336; + plug output 791 336; + PlusMinus1 240 336 + 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 232 328 248 344 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); implementation_end; - Zero 808 536 + PlusMinus2 432 336 description '4.01False - Signal\Sources\Zero.emx - 2007-9-27 15:54:36 - -'; - type Zero + 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 424 328 440 344 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); + implementation_end; + Position_control 296 336 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\PD.emx + 2008-1-17 10:49:7 +'; + type PD ports + signal in error; signal out output; end; icon bg bottom figures - rectangle 792.1 520 823.9 552 color 0 fill 15132390; - text '0' 808 536 color 16711680 18 bold; + rectangle 280 320 312 352 color 0 fill 15132390; + text 'PD' 296.5 336.1 color 16711680 18 bold; end; implementation eq +parameters + real kp = 4 {}; // Proportional gain + real tauD = 100 {s}; // Derivative time constant: tauD > 0 + real beta = 0.4 {}; // Tameness constant: 0 < beta << 1 +variables + real state, rate; equations - output = 0;implementation_end; - end; - connections - COM_body3\p => OneJunction4\p; - COM_body4\p => OneJunction3\p; - COM_body5\p => Link8\Pin; - Ground1\output -> JointC\Hin 632 616; - Ground1\p <= OneJunction5\p; - inverse_kinematics\angle1 -> Negate1\input; - inverse_kinematics\angle2 -> Negate2\input; - inverse_kinematics\input <- rectanglepath\output; - Joint_dynB\output => JointB\Pdiff; - Joint_dynC\output => JointC\Pdiff; - Joint_dynD\output => JointD\Pdiff; - Joint_dynE\output => JointE\Pdiff; - Joint_dynF\output => JointF\Pdiff; - JointA\Pdiff <= Joint_dynA\output; - JointA\Pout => Link3\Pin; - JointB\Hin <- Splitter4\output; - JointB\Hout -> Link6\Hin; - JointB\Pin <= Link6\Pout; - JointB\Pout => OneJunction3\p; - JointC\Hout -> Link5\Hin; - JointC\Pin <= Link5\Pout; - JointD\Hin <- Link7\Hout 216 616; - JointD\Hout -> Splitter7\input; - JointD\Pin <= Link9\Pout; - JointD\Pout => Link7\Pin 184 648; - JointE\Pout => Link4\Pin; - JointF\Hout -> Link10\Hin; - JointF\Pout => Link1\Pin; - Link1\Hout -> JointF\Hin; - Link10\Hout -> Splitter9\input; - Link10\Pout => JointF\Pin; - Link2\Hin <- JointE\Hout; - Link2\Hout -> Splitter4\input; - Link2\Pin <= OneJunction3\p; - Link2\Pout => JointE\Pin 184 112; - Link3\Hout -> Splitter8\input; - Link4\Hout -> JointE\Hin; - Link5\Hout -> Splitter1\input; - Link5\Pin <= OneJunction1\p; - Link6\Hout -> Splitter2\input; - Link6\Pin <= OneJunction2\p; - Link8\Hout -> Splitter10\input; - Link8\Pout => Link10\Pin; - Link9\Hout -> Splitter3\input; - Link9\Pin <= OneJunction4\p; - Negate1\output -> stepper_control\setpoint; - Negate2\output -> stepper_control1\setpoint; - OneJunction1\p <= COM_body2\p; - OneJunction1\p <= Link7\Pout; - OneJunction2\p <= COM_body1\p; - OneJunction2\p <= Link3\Pout; - OneJunction3\p <= Link1\Pout; - OneJunction4\p <= Link4\Pout; - OneJunction5\p <= JointC\Pout; - OneJunction5\p => JointA\Pin; - Splitter1\output -> Body2\input; - Splitter1\output -> COM_body2\Hin; - Splitter1\output -> Link7\Hin; - Splitter10\output -> Body6\input; - Splitter10\output -> COM_body5\Hin; - Splitter2\output -> COM_body1\Hin; - Splitter2\output -> Link3\Hin; - Splitter3\output -> COM_body3\Hin; - Splitter3\output -> Link4\Hin; - Splitter4\output -> Body4\input; - Splitter4\output -> COM_body4\Hin; - Splitter4\output -> Link1\Hin; - Splitter7\output -> Body3\input; - Splitter7\output -> Link9\Hin; - Splitter8\output -> Body1\input; - Splitter8\output -> JointA\Hin; - Splitter9\output -> Body5\input; - Splitter9\output -> Link8\Hin; - stepper_control\output -> Joint_dynA\angle; - stepper_control1\output -> Joint_dynC\angle; - end; - implementation_end; + rate = (kp * error - output) / (beta * tauD); + state = int (rate); + output = state + kp * error / beta; + implementation_end; + Splitter3 688 336 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; + ellipse 683.7 331.7 692.3 340.3 color -1; + terminals + input 688 336 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Splitter4 752 336 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; + ellipse 747.7 331.7 756.3 340.3 color -1; + terminals + input 752 336 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Velocity_control 480 336 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\PD.emx + 2008-1-17 10:49:7 +'; + type PD + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 464 320 496 352 color 0 fill 15132390; + text 'PD' 480.5 336.1 color 16711680 18 bold; + end; + implementation eq +parameters + real kp = 12 {}; // Proportional gain + real tauD = 10 {s}; // Derivative time constant: tauD > 0 + real beta = 0.4 {}; // Tameness constant: 0 < beta << 1 +variables + real state, rate; +equations + rate = (kp * error - output) / (beta * tauD); + state = int (rate); + output = state + kp * error / beta; + implementation_end; + Velocity_int 720 336 + 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 704 320 736 352 color 0 fill 15132390; + text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real global initial_angle; // initial value +equations + output = int (input, initial_angle); + implementation_end; + Velocity_limit 376 336 + 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 360 320 392 352 color 0 fill 15132390; + line 375.9 324.5 375.9 349 color 0 fill 15132390; + line 363 336.2 388.7 336.2 color 0 fill 15132390; + spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; + spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; + spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real global max_velocity {rad/s}; +equations + output = limit (input, -max_velocity, max_velocity); + implementation_end; + end; + connections + Acceleration_int\output -> Splitter3\input; + Acceleration_limit\output -> Acceleration_int\input; + PlusMinus1\output -> Position_control\error; + PlusMinus2\output -> Velocity_control\error; + Position_control\output -> Velocity_limit\input; + setpoint -> PlusMinus1\plus; + Splitter3\output -> PlusMinus2\minus 688 288 432 288; + Splitter3\output -> Velocity_int\input; + Splitter4\output -> output; + Splitter4\output -> PlusMinus1\minus 752 400 240 400; + Velocity_control\output -> Acceleration_limit\input; + Velocity_int\output -> Splitter4\input; + Velocity_limit\output -> PlusMinus2\plus; + end; + implementation_end; +specification_end; + specification 'P_only' + description '4.81Z:\home\wouter\Documents\studie\ma\mahd\implementation\library\Motor\stepper_control.emx2020-7-23 13:09:57parameters + real max_acceleration = 350 {rad/s2}; + real max_velocity = 25 {rad/s}; + real initial_velocity = 0 {rad/s}; + real initial_angle = 0 {rad};'; + type Submodel + ports + signal in setpoint {rad} ; + signal out output {rad} ; + end; + icon bg + figures + rectangle 456 752 568 784 color 0 fill 15132390; + text 'name' 512 768 color 0 'Clear Sans' 16; + end; + implementation bg + submodels + Acceleration_int 648 336 + 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 632 320 664 352 color 0 fill 15132390; + text '∫' 648 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real global initial_velocity; // initial value +equations + output = int (input, initial_velocity); + implementation_end; + Acceleration_limit 568 336 + 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 552 320 584 352 color 0 fill 15132390; + line 567.9 324.5 567.9 349 color 0 fill 15132390; + line 555 336.2 580.7 336.2 color 0 fill 15132390; + spline 561.9 343.1 573.9 329.4 color 16711680 fill 15132390 width 2; + spline 573.3 330.2 581.9 329.8 color 16711680 fill 15132390 width 2; + spline 555 343.6 561.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real global max_acceleration {rad/s2}; +equations + output = limit (input, -max_acceleration, max_acceleration); + implementation_end; + plug setpoint 136 336; + plug output 791 336; + PlusMinus1 240 336 + 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 232 328 248 344 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); + implementation_end; + PlusMinus2 432 336 + 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 424 328 440 344 color 0 fill 16777215; + end; + implementation eq +equations + output = sum (collect (plus)) - sum (collect (minus)); + implementation_end; + Position_control 296 336 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\P.emx + 2008-1-17 10:48:8 +'; + type P + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 280 320 312 352 color 0 fill 15132390; + text 'P' 296.5 336.1 color 16711680 18 bold; + end; + implementation eq +parameters + real kp = 31.0 {}; // Proportional gain +equations + output = kp * error; + implementation_end; + Splitter3 688 336 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 684.8 332.8 691.2 339.2 color -1 fill 0; + ellipse 683.7 331.7 692.3 340.3 color -1; + terminals + input 688 336 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Splitter4 752 336 + description '4.0 + Signal\Block Diagram\Splitter.emx + 2008-01-17 11:28:29 +1 + False +'; + knot Splitter + ports + signal knot duplicatable out output [1]; + signal knot in input [1]; + end; + icon bg ellipse + figures + ellipse 748.8 332.8 755.2 339.2 color -1 fill 0; + ellipse 747.7 331.7 756.3 340.3 color -1; + terminals + input 752 336 fixed; + end; + implementation eq +equations + collect (output) = input; +implementation_end; + Velocity_control 480 336 + description ' + 4.0 + 1 + False + Signal\Control\PID Control\Continuous\P.emx + 2008-1-17 10:48:8 +'; + type P + ports + signal in error; + signal out output; + end; + icon bg bottom + figures + rectangle 464 320 496 352 color 0 fill 15132390; + text 'P' 480.5 336.1 color 16711680 18 bold; + end; + implementation eq +parameters + real kp = 69.0 {}; // Proportional gain +equations + output = kp * error; + implementation_end; + Velocity_int 720 336 + 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 704 320 736 352 color 0 fill 15132390; + text '∫' 720 336.3 color 16711680 'Lucida Sans' 21 italic; + end; + implementation eq +parameters + real global initial_angle; // initial value +equations + output = int (input, initial_angle); + implementation_end; + Velocity_limit 376 336 + 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 360 320 392 352 color 0 fill 15132390; + line 375.9 324.5 375.9 349 color 0 fill 15132390; + line 363 336.2 388.7 336.2 color 0 fill 15132390; + spline 369.9 343.1 381.9 329.4 color 16711680 fill 15132390 width 2; + spline 381.3 330.2 389.9 329.8 color 16711680 fill 15132390 width 2; + spline 363 343.6 369.9 343.1 color 16711680 fill 15132390 width 2; + end; + end; + implementation eq +parameters + real global max_velocity {rad/s}; +equations + output = limit (input, -max_velocity, max_velocity); + implementation_end; + end; + connections + Acceleration_int\output -> Splitter3\input; + Acceleration_limit\output -> Acceleration_int\input; + PlusMinus1\output -> Position_control\error; + PlusMinus2\output -> Velocity_control\error; + Position_control\output -> Velocity_limit\input; + setpoint -> PlusMinus1\plus; + Splitter3\output -> PlusMinus2\minus 688 288 432 288; + Splitter3\output -> Velocity_int\input; + Splitter4\output -> output; + Splitter4\output -> PlusMinus1\minus 752 400 240 400; + Velocity_control\output -> Acceleration_limit\input; + Velocity_int\output -> Splitter4\input; + Velocity_limit\output -> PlusMinus2\plus; + end; + implementation_end; +specification_end; + end; + Zero 816 592 + description '4.01False + Signal\Sources\Zero.emx + 2007-9-27 15:54:36 + +'; + type Zero + ports + signal out output; + end; + icon bg bottom + figures + rectangle 800.1 576 831.9 608 color 0 fill 15132390; + text '0' 816 592 color 16711680 18 bold; + end; + implementation eq +equations + output = 0;implementation_end; + end; + connections + Body1\input <- Splitter2\output; + Body3\input <- Splitter3\output; + COM_body3\p => OneJunction4\p; + COM_body4\p => OneJunction6\p; + COM_body5\p => Link8\Pin; + Ground1\output -> JointC\Hin 632 616; + Ground1\p <= OneJunction5\p; + inverse_kinematics\angle1 -> stepper_control\setpoint; + inverse_kinematics\angle2 -> Negate2\input; + inverse_kinematics\input <- rectanglepath\output; + Joint_dynB\output => JointB\Pdiff; + Joint_dynC\output => JointC\Pdiff; + Joint_dynD\output => JointD\Pdiff; + Joint_dynE\output => JointE\Pdiff; + Joint_dynF\output => JointF\Pdiff; + JointA\Pdiff <= Joint_dynA\output; + JointA\Pout => Link3\Pin; + JointB\Hin <- Splitter4\output; + JointB\Hout -> Link6\Hin; + JointB\Pin <= Link6\Pout; + JointB\Pout => OneJunction3\p; + JointC\Hout -> Link5\Hin; + JointC\Pin <= Link5\Pout; + JointD\Hin <- Link7\Hout 216 616; + JointD\Hout -> Link9\Hin; + JointD\Pin <= Link9\Pout; + JointD\Pout => Link7\Pin 184 648; + JointE\Pout => Link4\Pin; + JointF\Hout -> Link10\Hin; + JointF\Pout => Link11\Pin; + Link1\Hout -> Splitter6\input; + Link10\Hout -> Splitter9\input; + Link10\Pout => JointF\Pin; + Link11\Hout -> JointF\Hin; + Link11\Pout => OneJunction6\p; + Link2\Hin <- JointE\Hout; + Link2\Hout -> Splitter11\input; + Link2\Pin <= OneJunction3\p; + Link2\Pout => JointE\Pin 184 112; + Link3\Hout -> Splitter8\input; + Link4\Hout -> JointE\Hin; + Link5\Hout -> Splitter1\input; + Link5\Pin <= OneJunction1\p; + Link6\Hout -> Splitter2\input; + Link6\Pin <= OneJunction2\p; + Link8\Hout -> Splitter10\input; + Link8\Pout => Link10\Pin; + Link9\Hout -> Splitter3\input; + Link9\Pin <= OneJunction4\p; + Negate2\output -> stepper_control1\setpoint; + OneJunction1\p <= COM_body2\p; + OneJunction1\p <= Link7\Pout; + OneJunction2\p <= COM_body1\p; + OneJunction2\p <= Link3\Pout; + OneJunction3\p <= Link1\Pout; + OneJunction4\p <= Link4\Pout; + OneJunction5\p <= JointC\Pout; + OneJunction5\p => JointA\Pin; + OneJunction6\p => Link1\Pin; + Splitter1\output -> Body2\input; + Splitter1\output -> COM_body2\Hin; + Splitter1\output -> Link7\Hin; + Splitter10\output -> Body6\input; + Splitter10\output -> COM_body5\Hin; + Splitter11\output -> Body4\input; + Splitter11\output -> Splitter4\input; + Splitter2\output -> COM_body1\Hin; + Splitter2\output -> Link3\Hin; + Splitter3\output -> COM_body3\Hin; + Splitter3\output -> Link4\Hin; + Splitter4\output -> Link1\Hin; + Splitter6\output -> COM_body4\Hin; + Splitter6\output -> Link11\Hin; + Splitter8\output -> JointA\Hin; + Splitter9\output -> Body5\input; + Splitter9\output -> Link8\Hin; + stepper_control\output -> Joint_dynA\angle; + stepper_control1\output -> Joint_dynC\angle; + end; + implementation_end; ]]> @@ -7762,30 +8372,10 @@ equations 0 0 0 - Joint_dynF\C4\state_initial - 2 - 1 - 0 0 - - - Joint_dynF\C\state_initial - 0 - - - stepper_control1\Position_control\state_initial - 0 - - - stepper_control1\Velocity_control\state_initial - 0 - - - stepper_control\Position_control\state_initial - 0 - - - stepper_control\Velocity_control\state_initial - 0 + Joint_dynF\C4\state_initial + 2 + 1 + 0 0 @@ -7852,16 +8442,29 @@ equations Body5\R[1,2] Body5\R[2,2] Body5\R[3,2] - stepper_control\setpoint - Joint_dynA\stepper_model\angle Joint_dynA\stepper_model\RotorAngle\angle - stepper_control1\setpoint - Joint_dynC\angle Joint_dynC\stepper_model\RotorAngle\angle Body6\position[3] Body6\position[2] rectanglepath\output[2] rectanglepath\output[1] + stepper_control\Acceleration_int\input + stepper_control\Velocity_int\output + stepper_control\Velocity_int\input + stepper_control1\Acceleration_int\input + stepper_control1\Velocity_int\output + stepper_control1\Velocity_int\input + Body6\R[1,3] + Body6\R[2,3] + Body6\R[3,3] + Body6\R[1,2] + Body6\R[2,2] + Body6\R[3,2] + Body6\position[1] + stepper_control\setpoint + Joint_dynA\stepper_model\angle + stepper_control1\setpoint + Joint_dynC\angle @@ -7885,7 +8488,7 @@ equations 16777215 true 1 - StepperA + Stepper Motor A true @@ -7935,7 +8538,7 @@ equations 0.0 - 50.0 + 6.0 true 3 @@ -7943,25 +8546,25 @@ equations - -2.3 - -1.2999999999999998 + 0.6000000000000002 + 2.6000000000000005 true 2 - + - -4.08637499341765 - 0.0 + -2.6 + -0.6 true 2 - + - -4.08637499341765 - 0.0 + -2.6 + -0.6 true 2 - + @@ -8027,6 +8630,10 @@ equations true + + 0.12119013062409288 + 0.8741610738255033 + 0 16777215 @@ -8641,36 +9248,36 @@ equations Camera Looking at Origin - 0.145719696848087 + 0.211790609540361 - 0.00771906321726964 + -0.107655952853072 - 0.116837947253578 + 0.123737714793038 - -0.883129107704613 + -0.840897145443893 - -0.046561942628002 + 0.426188883453037 - -0.46681362943209 + -0.333549136418257 - -0.46616615439618 + -0.297518598807634 - -0.0245780617428944 + 0.150790284066671 - 0.884355717670463 + 0.942732715882203 Direct3D @@ -8882,7 +9489,7 @@ equations false - Scenery + Cylinder @@ -8898,18 +9505,18 @@ equations false -
Bottom
+
Center
1.0 true 4 Body1\dimension[1] - 0.005 + 0.0048 Body1\dimension[2] - 0.01 + 0.009 Body1\dimension[3] @@ -8969,11 +9576,11 @@ equations Body1\position[2] - -1.0544452114340241e-4 + -0.017071802542194314 Body1\position[3] - -3.650733400542561e-5 + 0.0265923779880002 @@ -8984,11 +9591,11 @@ equations Body1\R[2,3] - 0.5179032193456982 + -0.5021736062379976 Body1\R[3,3] - 0.8554392178240144 + 0.8647668293811487 @@ -8998,11 +9605,11 @@ equations Body1\R[2,2] - 0.8554392178240144 + 0.8647668293811487 Body1\R[3,2] - -0.5179032193456982 + 0.5021736062379976 Matrix @@ -9088,11 +9695,11 @@ equations Body2\position[2] - -0.010891116074686562 + -0.010639024121723569 Body2\position[3] - -0.006056910383998224 + 0.006594217889494994 @@ -9103,11 +9710,11 @@ equations Body2\R[2,3] - -0.4874093624920096 + 0.5249456675421719 Body2\R[3,3] - 0.8731735871836326 + 0.8511357389563098 @@ -9117,11 +9724,11 @@ equations Body2\R[2,2] - 0.8731735871836326 + 0.8511357389563098 Body2\R[3,2] - 0.4874093624920096 + -0.5249456675421719 Matrix @@ -9136,14 +9743,14 @@ equations -
Bottom
+
Center
1.0 true 4 Body3\dimension[1] - 0.005 + 0.0045 Body3\dimension[2] @@ -9207,11 +9814,11 @@ equations Body3\position[2] - -0.02178223215904808 + -0.03682020445746276 Body3\position[3] - -0.012112104568558652 + 0.041384334395577975 @@ -9222,11 +9829,11 @@ equations Body3\R[2,3] - 0.5107213704282335 + -0.48276125099189626 Body3\R[3,3] - 0.8597462891969393 + 0.875752005159417 @@ -9236,11 +9843,11 @@ equations Body3\R[2,2] - 0.8597462891969393 + 0.875752005159417 Body3\R[3,2] - -0.5107213704282335 + 0.48276125099189626 Matrix @@ -9326,11 +9933,11 @@ equations Body4\position[2] - 0.0335347846656958 + -0.033109209191929785 Body4\position[3] - 0.0555071396541279 + 0.0542093131266403 @@ -9341,11 +9948,11 @@ equations Body4\R[2,3] - -0.46775115900174147 + 0.6245890404745712 Body4\R[3,3] - 0.8838602000613716 + 0.7809536033075554 @@ -9355,11 +9962,11 @@ equations Body4\R[2,2] - 0.8838602000613716 + 0.7809536033075554 Body4\R[3,2] - 0.46775115900174147 + -0.6245890404745712 Matrix @@ -9439,69 +10046,666 @@ equations false Block - - Body5\position[1] - 0.0 - - - Body5\position[2] - 0.06667959735242092 - - - Body5\position[3] - 0.0730331226976036 - + + Body5\position[1] + 0.0 + + + Body5\position[2] + -0.004186627780447375 + + + Body5\position[3] + 0.0310775428306494 + + + + + + Body5\R[1,3] + 0.0 + + + Body5\R[2,3] + 0.6245890426846639 + + + Body5\R[3,3] + 0.7809536015399732 + + + + + Body5\R[1,2] + 0.0 + + + Body5\R[2,2] + 0.7809536015399732 + + + Body5\R[3,2] + -0.6245890426846639 + + + Matrix + + false + false + false + false + false + false + + + + + + + 1 + + + 1 + + + 1 + + + false + Reference Frame + + + Body6\position[1] + 0.0 + + + Body6\position[2] + 0.0054542326865783565 + + + Body6\position[3] + 0.02336700049128123 + + + + + + Body6\R[1,3] + 0.0 + + + Body6\R[2,3] + 0.6245890426846639 + + + Body6\R[3,3] + 0.7809536015399732 + + + + + Body6\R[1,2] + 0.0 + + + Body6\R[2,2] + 0.7809536015399732 + + + Body6\R[3,2] + -0.6245890426846639 + + + Matrix + + false + false + false + false + false + false + + + 16 + 0.0073 + 0.0073 + 0.135 + true + true + true + true + false + true + + + 1 + + + 1 + + + 1 + + + true + + + 1 + + + 1 + + + 1 + + + 1.0 + + + 1 + + + 1 + + + 1 + + + + + true + + + 0.5 + + + 0.5 + + + 0.5 + + + + + + 15 + + false + false + Cylinder + + + + + + 1.5707963267949 + + + Bryant + + false + false + false + false + false + false + + + + + + + + + + + + + GraphPlot + + 3 + false + 16777215 + true + + + true + 15780518 + 12624260 + 0 + 10 + 10 + 10 + false + + 16777215 + true + 1 + Stepper Motor D + 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 + 6.0 + true + 3 + + + + + + -1.0 + 1.0 + true + 2 + + + + -1.0 + 1.0 + true + 2 + + + + -1.0 + 1.0 + true + 2 + + + + + + + + 3355111 + 1 + 3355111 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + stepper_control1\setpoint + + + + 6076255 + 1 + 6076255 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + Joint_dynC\angle + + + + 12553035 + 1 + 12553035 + 0 + true + 1 + 1 + 1 + true + + true + time + + + true + Joint_dynC\stepper_model\RotorAngle\angle + + + + + true + + 0.11683599419448476 + 0.8741610738255033 - - - - Body5\R[1,3] - 0.0 - - - Body5\R[2,3] - -0.46775087171205454 - - - Body5\R[3,3] - 0.8838603520990255 - - - - - Body5\R[1,2] - 0.0 - - - Body5\R[2,2] - 0.8838603520990255 - - - Body5\R[3,2] - 0.46775087171205454 - - - Matrix - - false - false - false - false - false - false - - - - - - - - + 0 + 16777215 + + + + GraphPlot + + 4 + 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.041708787744804604 + 0.08638068231201962 + true + 3 + + + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + -0.06 + 0.14 + true + 2 + + + + + + + + 3355111 + 1 + 3355111 + 0 + true + 1 + 1 + 1 + true + + true + Body1\position[2] + + + true + Body1\position[3] + + + + 6076255 + 1 + 6076255 + 0 + true + 1 + 1 + 1 + true + + true + Body2\position[2] + + + true + Body2\position[3] + + + + 12553035 + 1 + 12553035 + 0 + true + 1 + 1 + 1 + true + + true + Body3\position[2] + + + true + Body3\position[3] + + + + 15086320 + 1 + 15086320 + 0 + true + 1 + 1 + 1 + true + + true + Body4\position[2] + + + true + Body4\position[3] + + + + 15790150 + 1 + 15790150 + 0 + true + 1 + 1 + 1 + true + + true + Body5\position[2] + + + true + Body5\position[3] + + + + 1696255 + 1 + 1696255 + 0 + true + 1 + 1 + 1 + true + + true + Body6\position[2] + + + true + Body6\position[3] + + + + 4089775 + 1 + 4089775 + 0 + true + 1 + 1 + 1 + true + + true + rectanglepath\output[1] + + + true + rectanglepath\output[2] + + + + + true + 0 + 16777215 + GraphPlot - 3 + 5 false 16777215 true @@ -9519,7 +10723,7 @@ equations 16777215 true 1 - StepperD + Stepper Control A true @@ -9569,7 +10773,7 @@ equations 0.0 - 50.0 + 6.0 true 3 @@ -9577,34 +10781,34 @@ equations - -1.0 - 1.0 + -500.0 + 500.0 true - 2 - + 1 + - -1.0 - 1.0 + 0.0 + 0.730288609349958 true - 2 - + 1 + - -1.0 - 1.0 + 0.0 + 0.730288609349958 true - 2 - + 1 + - 3355111 + 6076255 1 - 3355111 + 6076255 0 true 1 @@ -9617,13 +10821,13 @@ equations true - stepper_control1\setpoint + stepper_control\Acceleration_int\input - 6076255 + 12553035 1 - 6076255 + 12553035 0 true 1 @@ -9636,13 +10840,13 @@ equations true - Joint_dynC\angle + stepper_control\Velocity_int\output - 12553035 + 15086320 1 - 12553035 + 15086320 0 true 1 @@ -9655,12 +10859,16 @@ equations true - Joint_dynC\stepper_model\RotorAngle\angle + stepper_control\Velocity_int\input true + + 0.07602339181286549 + 0.877104377104377 + 0 16777215 @@ -9668,7 +10876,7 @@ equations GraphPlot - 4 + 6 false 16777215 true @@ -9686,7 +10894,7 @@ equations 16777215 true 1 - Plot + Stepper Control D true @@ -9735,62 +10943,34 @@ equations false - -0.025000001857932178 - 0.0831680151788676 + 0.0 + 6.0 true 3 - + - -0.020000000000000004 - 0.08 + -500.0 + 500.0 true - 2 - - - - -0.06 - 0.14 - true - 2 - - - - -0.06 - 0.14 - true - 2 - + 1 + - -0.06 - 0.14 - true - 2 - - - - -0.06 - 0.14 - true - 2 - - - - -0.06 - 0.14 + 0.0 + 39.669708688195 true - 2 - + 1 + - -0.06 - 0.14 + 0.0 + 39.669708688195 true - 2 - + 1 + @@ -9808,11 +10988,11 @@ equations true true - Body1\position[2] + time true - Body1\position[3] + stepper_control1\Acceleration_int\input @@ -9827,11 +11007,11 @@ equations true true - Body2\position[2] + time true - Body2\position[3] + stepper_control1\Velocity_int\output @@ -9846,92 +11026,20 @@ equations true true - Body3\position[2] - - - true - Body3\position[3] - - - - 15086320 - 1 - 15086320 - 0 - true - 1 - 1 - 1 - true - - true - Body4\position[2] - - - true - Body4\position[3] - - - - 15790150 - 1 - 15790150 - 0 - true - 1 - 1 - 1 - true - - true - Body5\position[2] - - - true - Body5\position[3] - - - - 1696255 - 1 - 1696255 - 0 - true - 1 - 1 - 1 - true - - true - Body6\position[2] - - - true - Body6\position[3] - - - - 4089775 - 1 - 4089775 - 0 - true - 1 - 1 - 1 - true - - true - rectanglepath\output[1] + time true - rectanglepath\output[2] + stepper_control1\Velocity_int\input true + + 0.07602339181286549 + 0.877104377104377 + 0 16777215 @@ -9946,6 +11054,8 @@ equations 1 3 + 5 + 6 Base @@ -9971,11 +11081,11 @@ equations - -0.135417 0.109259 0.885417 0.809259 + +0 0.0259259 1 1 -0.132292 0.142593 0.930729 0.903704 +0.075 0.0861111 0.917708 0.938889 0 0.0259259 1 1 @@ -9985,7 +11095,7 @@ equations 0.0 - 50.0 + 6.0 false false false @@ -10085,7 +11195,7 @@ equations false 0.0 - 9 + 8 @@ -10097,14 +11207,37 @@ equations true 0 0.0 - true - MultipleRun + false + Optimization true - - UseEndValue + Joint_dynC\stepper_model\angle + stepper_control1\setpoint + UseIntegralAbsolute 0.001 - BroydonFletcherGoldfarbShanno + DavidsonFletcherPowell + + stepper_control1\Position_control\kp + 13.0 + 39.0 + Linear + Uniform + 26.0 + 1.0 + 1.0 + 26.0 + + + stepper_control1\Velocity_control\kp + 33.0 + 99.0 + Linear + Uniform + 66.0 + 1.0 + 1.0 + 66.0 + @@ -10116,18 +11249,12 @@ equations false - - time - - - 1.0 -