-
Notifications
You must be signed in to change notification settings - Fork 0
/
sfun_motiongen.tlc
199 lines (175 loc) · 8.79 KB
/
sfun_motiongen.tlc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
%% Abstract:
%% Tlc file for S-function sfun_motiongen.cpp
%implements "sfun_motiongen" "C"
%% Function: BlockTypeSetup ====================================================
%function BlockTypeSetup(block, system) void
%%
%% The Target Language must be C++
%if ::GenCPP==0 && !IsModelReferenceSimTarget()
%<LibReportFatalError("This S-Function generated by the Legacy Code Tool must be only used with the C++ Target Language")>
%endif
%<LibAddToCommonIncludes("<ruckig/ruckig.hpp>")>
%<LibAddToModelSources("brake")>
%<LibAddToModelSources("position_first_step1")>
%<LibAddToModelSources("position_first_step2")>
%<LibAddToModelSources("position_second_step1")>
%<LibAddToModelSources("position_second_step2")>
%<LibAddToModelSources("position_third_step1")>
%<LibAddToModelSources("position_third_step2")>
%<LibAddToModelSources("velocity_second_step1")>
%<LibAddToModelSources("velocity_second_step2")>
%<LibAddToModelSources("velocity_third_step1")>
%<LibAddToModelSources("velocity_third_step2")>
%endfunction
%% Function: mdlInitializeConditions ========================================
%% Abstract:
%% In this function, you should initialize the continuous and discrete
%% states for your S-function block. The initial states are placed
%% in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
%% You can also perform any other initialization activities that your
%% S-function may require. Note, this routine will be called at the
%% start of simulation and if it is present in an enabled subsystem
%% configured to reset states, it will be call when the enabled
%% subsystem restarts execution to reset the states.
%function InitializeConditions(block, system) Output
/* %<Type> Block: %<Name> */
{
%% The mdlInitializeConditions method is called when the simulation
%% start and every time an enabled subsystem is re-enabled.
%% Reset the IWork flag to 1 when values need to be reinitialized.
%<LibBlockIWork(BlockInit, "", "", 0)> = 1;
}
%endfunction
%% Function: mdlStart =======================================================
%% Abstract:
%% This function is called once at start of model execution. If you have
%% states that should be initialized once, this is the place to do it.
%function Start(block, system) Output
/* %<Type> Block: %<Name> */
{
%% Create instances: the Ruckig OTG as well as input and output parameters
const unsigned int ndofs = static_cast<unsigned int>(%<SFcnParamSettings.NumberOfDoFs>);
auto *OTG = new ruckig::Ruckig<ruckig::DynamicDOFs>( ndofs, %<SFcnParamSettings.SampleTime> );
auto *IP = new ruckig::InputParameter<ruckig::DynamicDOFs>( ndofs );
auto *OP = new ruckig::OutputParameter<ruckig::DynamicDOFs>( ndofs );
const char* control = "%<SFcnParamSettings.ControlInterface>";
if (strcmp(control,"POSITION") == 0){
IP->control_interface = ruckig::ControlInterface::Position;
}
else if (strcmp(control,"VELOCITY") == 0){
IP->control_interface = ruckig::ControlInterface::Velocity;
}
else {
IP->control_interface = ruckig::ControlInterface::Position;
}
const char* sync = "%<SFcnParamSettings.Synchronization>";
if (strcmp(sync,"NO_SYNCHRONIZATION") == 0){
IP->synchronization = ruckig::Synchronization::None;
}
else if (strcmp(sync,"TIME_SYNCHRONIZATION_IF_NECESSARY") == 0){
IP->synchronization = ruckig::Synchronization::TimeIfNecessary;
}
else if (strcmp(sync,"ONLY_TIME_SYNCHRONIZATION") == 0){
IP->synchronization = ruckig::Synchronization::Time;
}
else if (strcmp(sync,"ONLY_PHASE_SYNCHRONIZATION") == 0){
IP->synchronization = ruckig::Synchronization::Phase;
}
else {
IP->synchronization = ruckig::Synchronization::Time;
}
const char* duration = "%<SFcnParamSettings.DurationDiscretization>";
if (strcmp(duration,"CONTINUOUS") == 0){
IP->duration_discretization = ruckig::DurationDiscretization::Continuous;
}
else if (strcmp(duration,"DISCRETE") == 0){
IP->duration_discretization = ruckig::DurationDiscretization::Discrete;
}
else {
IP->duration_discretization = ruckig::DurationDiscretization::Continuous;
}
%% Store new C++ object in the pointers vector
%<LibBlockPWork(Ruckig, "", "", 0)> = OTG;
%<LibBlockPWork(InputParameter, "", "", 0)> = IP;
%<LibBlockPWork(OutputParameter, "", "", 0)> = OP;
}
%endfunction
%% Function: mdlOutputs =======================================================
%% Abstract:
%% In this function, you compute the outputs of your S-function block.
%function Outputs(block, system) Output
/* %<Type> Block: %<Name> */
{
%% Initialize values if the IWork vector flag is true.
if (%<LibBlockIWork(BlockInit, "", "", 0)> == 1) {
const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
for (int_T dof=0; dof < ndofs; dof++)
{
(%<LibBlockOutputSignalAddr(0, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(3, "", "", 0)>)[dof];
(%<LibBlockOutputSignalAddr(1, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(4, "", "", 0)>)[dof];
(%<LibBlockOutputSignalAddr(2, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(5, "", "", 0)>)[dof];
}
%<LibBlockIWork(BlockInit, "", "", 0)> = 0;
}
else {
%% Retrieve C++ object from the pointers vector
auto *OP = static_cast<ruckig::OutputParameter<ruckig::DynamicDOFs> *>(%<LibBlockPWork(OutputParameter, "", "", 0)>);
const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
for (int_T dof=0; dof < ndofs; dof++)
{
(%<LibBlockOutputSignalAddr(0, "", "", 0)>)[dof] = OP->new_position[dof];
(%<LibBlockOutputSignalAddr(1, "", "", 0)>)[dof] = OP->new_velocity[dof];
(%<LibBlockOutputSignalAddr(2, "", "", 0)>)[dof] = OP->new_acceleration[dof];
}
}
}
%endfunction
%% Function: mdlUpdate ======================================================
%% Abstract:
%% This function is called once for every major integration time step.
%% Discrete states are typically updated here, but this function is
%% useful for performing any tasks that should only take place once per
%% integration step.
%function Update(block, system) Output
/* %<Type> Block: %<Name> */
{
%% Retrieve C++ object from the pointers vector
auto *OTG = static_cast<ruckig::Ruckig<ruckig::DynamicDOFs> *>(%<LibBlockPWork(Ruckig, "", "", 0)>);
auto *IP = static_cast<ruckig::InputParameter<ruckig::DynamicDOFs> *>(%<LibBlockPWork(InputParameter, "", "", 0)>);
auto *OP = static_cast<ruckig::OutputParameter<ruckig::DynamicDOFs> *>(%<LibBlockPWork(OutputParameter, "", "", 0)>);
%% Set-up the input parameters
const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
for (int_T dof=0; dof < ndofs; dof++)
{
IP->current_position[dof] = (%<LibBlockInputSignalAddr(3, "", "", 0)>)[dof];
IP->current_velocity[dof] = (%<LibBlockInputSignalAddr(4, "", "", 0)>)[dof];
IP->current_acceleration[dof] = (%<LibBlockInputSignalAddr(5, "", "", 0)>)[dof];
IP->max_velocity[dof] = (%<LibBlockParameterAddr(VelLimit, "", "", 0)>)[dof];
IP->max_acceleration[dof] = (%<LibBlockParameterAddr(AccLimit, "", "", 0)>)[dof];
IP->max_jerk[dof] = (%<LibBlockParameterAddr(JrkLimit, "", "", 0)>)[dof];
IP->target_position[dof] = (%<LibBlockInputSignalAddr(0, "", "", 0)>)[dof];
IP->target_velocity[dof] = (%<LibBlockInputSignalAddr(1, "", "", 0)>)[dof];
IP->target_acceleration[dof] = (%<LibBlockInputSignalAddr(2, "", "", 0)>)[dof];
}
%% Calling the Ruckig OTG algorithm
int_T ResultValue = OTG->update(*IP,*OP);
}
%endfunction
%% Function: mdlTerminate =====================================================
%% Abstract:
%% In this function, you should perform any actions that are necessary
%% at the termination of a simulation. For example, if memory was
%% allocated in mdlStart, this is the place to free it.
%function Terminate(block, system) Output
/* %<Type> Block: %<Name> */
{
%% Retrieve C++ object from the pointers vector
auto *OTG = static_cast<ruckig::Ruckig<ruckig::DynamicDOFs> *>(%<LibBlockPWork(Ruckig, "", "", 0)>);
auto *IP = static_cast<ruckig::InputParameter<ruckig::DynamicDOFs> *>(%<LibBlockPWork(InputParameter, "", "", 0)>);
auto *OP = static_cast<ruckig::OutputParameter<ruckig::DynamicDOFs> *>(%<LibBlockPWork(OutputParameter, "", "", 0)>);
%% Deleting the objects of the Ruckig Library end terminating the process
delete OTG;
delete IP;
delete OP;
}
%endfunction