|
| 1 | +%SLPLOTBOT S-function for robot animation |
| 2 | +% |
| 3 | +% This is the S-function for animating the robot. It assumes input |
| 4 | +% data u to be the joint angles q. |
| 5 | +% |
| 6 | +% Implemented as an S-function so as to update display at the end of |
| 7 | +% each Simulink major integration step. |
| 8 | + |
| 9 | +function [sys,x0,str,ts] = sltrprint(t,x,u,flag, options) |
| 10 | + switch flag |
| 11 | + |
| 12 | + case 0 |
| 13 | + % initialize |
| 14 | + [sys,x0,str,ts] = mdlInitializeSizes(); % Init |
| 15 | + |
| 16 | + |
| 17 | + case 3 |
| 18 | + % come here on update |
| 19 | + if ~isempty(u) |
| 20 | + if length(u) == 3 |
| 21 | + s = trprint2(transl2(u(1), u(2))*trot2(u(3))); |
| 22 | + elseif ishomog2(u) |
| 23 | + s = trprint2(u, options); |
| 24 | + elseif ishomog(u) |
| 25 | + s = trprint(u, options); |
| 26 | + else |
| 27 | + error('RTB:sltrprint', 'unknown type passed, expecting 3x1, 3x3, 4x4'); |
| 28 | + end |
| 29 | + set_param(gcs, 'MaskDisplay', ['disp(''' s ''');']) |
| 30 | + end |
| 31 | + ret = []; |
| 32 | + case {1, 2, 4, 9} |
| 33 | + ret = []; |
| 34 | + end |
| 35 | +% |
| 36 | +%============================================================================= |
| 37 | +% mdlInitializeSizes |
| 38 | +% Return the sizes, initial conditions, and sample times for the S-function. |
| 39 | +%============================================================================= |
| 40 | +% |
| 41 | +function [sys,x0,str,ts]=mdlInitializeSizes() |
| 42 | + |
| 43 | +% |
| 44 | +% call simsizes for a sizes structure, fill it in and convert it to a |
| 45 | +% sizes array. |
| 46 | +% |
| 47 | +% Note that in this example, the values are hard coded. This is not a |
| 48 | +% recommended practice as the characteristics of the block are typically |
| 49 | +% defined by the S-function parameters. |
| 50 | +% |
| 51 | +sizes = simsizes; |
| 52 | + |
| 53 | +sizes.NumContStates = 0; |
| 54 | +sizes.NumDiscStates = 0; |
| 55 | +sizes.NumOutputs = 0; |
| 56 | +sizes.NumInputs = -1; |
| 57 | +sizes.DirFeedthrough = 1; |
| 58 | +sizes.NumSampleTimes = 1; % at least one sample time is needed |
| 59 | + |
| 60 | +sys = simsizes(sizes); |
| 61 | + |
| 62 | +% |
| 63 | +% initialize the initial conditions |
| 64 | +% |
| 65 | +x0 = []; |
| 66 | + |
| 67 | +% |
| 68 | +% str is always an empty matrix |
| 69 | +% |
| 70 | +str = []; |
| 71 | + |
| 72 | +% |
| 73 | +% initialize the array of sample times |
| 74 | +% |
| 75 | +ts = [-1 0]; |
| 76 | + |
| 77 | +% end mdlInitializeSizes |
0 commit comments