1 | /* |
2 | * RaspberrySmithPredictor.c |
3 | * |
4 | * Classroom License -- for classroom instructional use only. Not for |
5 | * government, commercial, academic research, or other organizational use. |
6 | * |
7 | * Code generation for model "RaspberrySmithPredictor". |
8 | * |
9 | * Model version : 1.1 |
10 | * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 |
11 | * C source code generated on : Tue Dec 20 14:52:06 2016 |
12 | * |
13 | * Target selection: ert.tlc |
14 | * Note: GRT includes extra infrastructure and instrumentation for prototyping |
15 | * Embedded hardware selection: ARM Compatible->ARM Cortex |
16 | * Code generation objectives: Unspecified |
17 | * Validation result: Not run |
18 | */ |
19 | |
20 | #include "RaspberrySmithPredictor.h" |
21 | #include "RaspberrySmithPredictor_private.h" |
22 | #include "RaspberrySmithPredictor_dt.h" |
23 | |
24 | /* Block signals (auto storage) */ |
25 | B_RaspberrySmithPredictor_T RaspberrySmithPredictor_B; |
26 | |
27 | /* Continuous states */ |
28 | X_RaspberrySmithPredictor_T RaspberrySmithPredictor_X; |
29 | |
30 | /* Block states (auto storage) */ |
31 | DW_RaspberrySmithPredictor_T RaspberrySmithPredictor_DW; |
32 | |
33 | /* Real-time model */ |
34 | RT_MODEL_RaspberrySmithPredic_T RaspberrySmithPredictor_M_; |
35 | RT_MODEL_RaspberrySmithPredic_T *const RaspberrySmithPredictor_M = |
36 | &RaspberrySmithPredictor_M_; |
37 | |
38 | /* |
39 | * This function updates continuous states using the ODE1 fixed-step |
40 | * solver algorithm |
41 | */ |
42 | static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) |
43 | { |
44 | time_T tnew = rtsiGetSolverStopTime(si); |
45 | time_T h = rtsiGetStepSize(si); |
46 | real_T *x = rtsiGetContStates(si); |
47 | ODE1_IntgData *id = (ODE1_IntgData *)rtsiGetSolverData(si); |
48 | real_T *f0 = id->f[0]; |
49 | int_T i; |
50 | int_T nXc = 8; |
51 | rtsiSetSimTimeStep(si,MINOR_TIME_STEP); |
52 | rtsiSetdX(si, f0); |
53 | RaspberrySmithPredictor_derivatives(); |
54 | rtsiSetT(si, tnew); |
55 | for (i = 0; i < nXc; ++i) { |
56 | x[i] += h * f0[i]; |
57 | } |
58 | |
59 | rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); |
60 | } |
61 | |
62 | /* |
63 | * Output and update for atomic system: |
64 | * '<S1>/Position transformation1' |
65 | * '<S3>/Position transformation' |
66 | */ |
67 | void Raspber_Positiontransformation1(real_T rtu_x, real_T rtu_y, const real_T |
68 | rtu_km4[9], B_Positiontransformation1_Ras_T *localB) |
69 | { |
70 | /* MATLAB Function 'Camera position/Position transformation1': '<S7>:1' */ |
71 | /* out=km2*[x;y;x^2;y^2;x*y;1]; */ |
72 | /* '<S7>:1:4' xa = [x,y]; */ |
73 | /* '<S7>:1:5' H = km4(:); */ |
74 | /* '<S7>:1:6' xa = [xa,ones(size(xa,1),1)]; */ |
75 | /* homogeneous coordinates */ |
76 | /* '<S7>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */ |
77 | /* '<S7>:1:9' X=out(1); */ |
78 | localB->X = ((rtu_x * rtu_km4[0] + rtu_y * rtu_km4[1]) + rtu_km4[2]) / ((rtu_x |
79 | * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]); |
80 | |
81 | /* '<S7>:1:10' Y=out(2); */ |
82 | localB->Y = ((rtu_x * rtu_km4[3] + rtu_y * rtu_km4[4]) + rtu_km4[5]) / ((rtu_x |
83 | * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]); |
84 | } |
85 | |
86 | real_T rt_powd_snf(real_T u0, real_T u1) |
87 | { |
88 | real_T y; |
89 | real_T tmp; |
90 | real_T tmp_0; |
91 | if (rtIsNaN(u0) || rtIsNaN(u1)) { |
92 | y = (rtNaN); |
93 | } else { |
94 | tmp = fabs(u0); |
95 | tmp_0 = fabs(u1); |
96 | if (rtIsInf(u1)) { |
97 | if (tmp == 1.0) { |
98 | y = (rtNaN); |
99 | } else if (tmp > 1.0) { |
100 | if (u1 > 0.0) { |
101 | y = (rtInf); |
102 | } else { |
103 | y = 0.0; |
104 | } |
105 | } else if (u1 > 0.0) { |
106 | y = 0.0; |
107 | } else { |
108 | y = (rtInf); |
109 | } |
110 | } else if (tmp_0 == 0.0) { |
111 | y = 1.0; |
112 | } else if (tmp_0 == 1.0) { |
113 | if (u1 > 0.0) { |
114 | y = u0; |
115 | } else { |
116 | y = 1.0 / u0; |
117 | } |
118 | } else if (u1 == 2.0) { |
119 | y = u0 * u0; |
120 | } else if ((u1 == 0.5) && (u0 >= 0.0)) { |
121 | y = sqrt(u0); |
122 | } else if ((u0 < 0.0) && (u1 > floor(u1))) { |
123 | y = (rtNaN); |
124 | } else { |
125 | y = pow(u0, u1); |
126 | } |
127 | } |
128 | |
129 | return y; |
130 | } |
131 | |
132 | /* Model step function */ |
133 | void RaspberrySmithPredictor_step(void) |
134 | { |
135 | int32_T iy; |
136 | int32_T ix; |
137 | real_T rtb_Sum_l_idx_0; |
138 | real_T rtb_Sum_l_idx_1; |
139 | real_T rtb_Saturate_idx_0; |
140 | real_T rtb_Saturate_idx_1; |
141 | real_T Fv_idx_1; |
142 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
143 | /* set solver stop time */ |
144 | if (!(RaspberrySmithPredictor_M->Timing.clockTick0+1)) { |
145 | rtsiSetSolverStopTime(&RaspberrySmithPredictor_M->solverInfo, |
146 | ((RaspberrySmithPredictor_M->Timing.clockTickH0 + 1) |
147 | * RaspberrySmithPredictor_M->Timing.stepSize0 * 4294967296.0)); |
148 | } else { |
149 | rtsiSetSolverStopTime(&RaspberrySmithPredictor_M->solverInfo, |
150 | ((RaspberrySmithPredictor_M->Timing.clockTick0 + 1) * |
151 | RaspberrySmithPredictor_M->Timing.stepSize0 + |
152 | RaspberrySmithPredictor_M->Timing.clockTickH0 * |
153 | RaspberrySmithPredictor_M->Timing.stepSize0 * 4294967296.0)); |
154 | } |
155 | } /* end MajorTimeStep */ |
156 | |
157 | /* Update absolute time of base rate at minor time step */ |
158 | if (rtmIsMinorTimeStep(RaspberrySmithPredictor_M)) { |
159 | RaspberrySmithPredictor_M->Timing.t[0] = rtsiGetT |
160 | (&RaspberrySmithPredictor_M->solverInfo); |
161 | } |
162 | |
163 | /* S-Function (sfcn_getFoilCheck): '<S3>/MagMan Foil with error' */ |
164 | sfcn_getFoilCheck_Outputs_wrapper |
165 | ( &RaspberrySmithPredictor_B.MagManFoilwitherror_o1, |
166 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o2, |
167 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o3, |
168 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o4, |
169 | &RaspberrySmithPredictor_DW.MagManFoilwitherror_DSTATE); |
170 | |
171 | /* MATLAB Function: '<S3>/Position transformation' incorporates: |
172 | * Constant: '<S3>/Sentinel Calibration matrix' |
173 | */ |
174 | Raspber_Positiontransformation1 |
175 | (RaspberrySmithPredictor_B.MagManFoilwitherror_o1, |
176 | RaspberrySmithPredictor_B.MagManFoilwitherror_o2, |
177 | RaspberrySmithPredictor_P.km4pi, |
178 | &RaspberrySmithPredictor_B.sf_Positiontransformation); |
179 | |
180 | /* MATLAB Function: '<S4>/Embedded MATLAB Function' */ |
181 | /* MATLAB Function 'Force to Currents/Embedded MATLAB Function': '<S9>:1' */ |
182 | /* '<S9>:1:2' Ao=zeros(2,16); */ |
183 | memset(&RaspberrySmithPredictor_B.Ao[0], 0, sizeof(real_T) << 5U); |
184 | |
185 | /* '<S9>:1:4' for iy=1:4 */ |
186 | for (iy = 0; iy < 4; iy++) { |
187 | /* '<S9>:1:5' for ix=1:4 */ |
188 | for (ix = 0; ix < 4; ix++) { |
189 | /* '<S9>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */ |
190 | /* '<S9>:1:14' d=dist(x,y, ix, iy); */ |
191 | /* '<S9>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
192 | rtb_Sum_l_idx_0 = RaspberrySmithPredictor_B.sf_Positiontransformation.X - |
193 | (1.0 + (real_T)ix); |
194 | Fv_idx_1 = RaspberrySmithPredictor_B.sf_Positiontransformation.Y - (1.0 + |
195 | (real_T)iy); |
196 | rtb_Sum_l_idx_0 = sqrt(rtb_Sum_l_idx_0 * rtb_Sum_l_idx_0 + Fv_idx_1 * |
197 | Fv_idx_1); |
198 | |
199 | /* '<S9>:1:15' if d~=0 */ |
200 | if (rtb_Sum_l_idx_0 != 0.0) { |
201 | /* '<S9>:1:16' F=gForce(d); */ |
202 | /* '<S9>:1:43' a = 622.06; */ |
203 | /* '<S9>:1:44' b = 9.5032; */ |
204 | /* '<S9>:1:45' c = 3.1884; */ |
205 | /* '<S9>:1:46' F = x.*a./(b.*x.^2+ c).^3; */ |
206 | rtb_Sum_l_idx_0 = rtb_Sum_l_idx_0 * 622.06 / rt_powd_snf(rtb_Sum_l_idx_0 |
207 | * rtb_Sum_l_idx_0 * 9.5032 + 3.1884, 3.0); |
208 | |
209 | /* '<S9>:1:17' Fv=F*vector(x,y,ix,iy); */ |
210 | /* '<S9>:1:32' vx=ix-x; */ |
211 | Fv_idx_1 = (1.0 + (real_T)ix) - |
212 | RaspberrySmithPredictor_B.sf_Positiontransformation.X; |
213 | |
214 | /* '<S9>:1:33' vy=iy-y; */ |
215 | rtb_Sum_l_idx_1 = (1.0 + (real_T)iy) - |
216 | RaspberrySmithPredictor_B.sf_Positiontransformation.Y; |
217 | |
218 | /* '<S9>:1:35' if(vx==0 && vy ==0) */ |
219 | if ((Fv_idx_1 == 0.0) && (rtb_Sum_l_idx_1 == 0.0)) { |
220 | /* '<S9>:1:36' v=[0 0]; */ |
221 | rtb_Saturate_idx_1 = 0.0; |
222 | Fv_idx_1 = 0.0; |
223 | } else { |
224 | /* '<S9>:1:37' else */ |
225 | /* '<S9>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */ |
226 | /* '<S9>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
227 | rtb_Saturate_idx_0 = sqrt(Fv_idx_1 * Fv_idx_1 + rtb_Sum_l_idx_1 * |
228 | rtb_Sum_l_idx_1); |
229 | rtb_Saturate_idx_1 = Fv_idx_1 / rtb_Saturate_idx_0; |
230 | Fv_idx_1 = rtb_Sum_l_idx_1 / rtb_Saturate_idx_0; |
231 | } |
232 | |
233 | rtb_Saturate_idx_1 *= rtb_Sum_l_idx_0; |
234 | Fv_idx_1 *= rtb_Sum_l_idx_0; |
235 | |
236 | /* '<S9>:1:18' Fx=Fv(1); */ |
237 | rtb_Sum_l_idx_0 = rtb_Saturate_idx_1; |
238 | |
239 | /* '<S9>:1:19' Fy=Fv(2); */ |
240 | } else { |
241 | /* '<S9>:1:20' else */ |
242 | /* '<S9>:1:21' Fx=0; */ |
243 | rtb_Sum_l_idx_0 = 0.0; |
244 | |
245 | /* '<S9>:1:22' Fy=0; */ |
246 | Fv_idx_1 = 0.0; |
247 | } |
248 | |
249 | /* '<S9>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */ |
250 | RaspberrySmithPredictor_B.Ao[((iy << 2) + ix) << 1] = rtb_Sum_l_idx_0; |
251 | |
252 | /* '<S9>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */ |
253 | RaspberrySmithPredictor_B.Ao[1 + (((iy << 2) + ix) << 1)] = Fv_idx_1; |
254 | } |
255 | } |
256 | |
257 | /* End of MATLAB Function: '<S4>/Embedded MATLAB Function' */ |
258 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
259 | /* Gain: '<S5>/Setpoint Weighting (Proportional)' incorporates: |
260 | * Constant: '<Root>/Referece position' |
261 | */ |
262 | RaspberrySmithPredictor_B.SetpointWeightingProportional[0] = |
263 | RaspberrySmithPredictor_P.PIDController2DOF_b * |
264 | RaspberrySmithPredictor_P.Refereceposition_Value[0]; |
265 | RaspberrySmithPredictor_B.SetpointWeightingProportional[1] = |
266 | RaspberrySmithPredictor_P.PIDController2DOF_b * |
267 | RaspberrySmithPredictor_P.Refereceposition_Value[1]; |
268 | } |
269 | |
270 | /* S-Function (sfcn_posread_shm): '<S6>/Shared memory camera position' */ |
271 | sfcn_posread_shm_Outputs_wrapper |
272 | ( &RaspberrySmithPredictor_B.Sharedmemorycameraposition_o1, |
273 | &RaspberrySmithPredictor_B.Sharedmemorycameraposition_o2, |
274 | &RaspberrySmithPredictor_DW.Sharedmemorycameraposition_DSTA); |
275 | |
276 | /* DataTypeConversion: '<S6>/Data Type Conversion1' */ |
277 | RaspberrySmithPredictor_B.DataTypeConversion1 = |
278 | RaspberrySmithPredictor_B.Sharedmemorycameraposition_o1; |
279 | |
280 | /* DataTypeConversion: '<S6>/Data Type Conversion2' */ |
281 | RaspberrySmithPredictor_B.DataTypeConversion2 = |
282 | RaspberrySmithPredictor_B.Sharedmemorycameraposition_o2; |
283 | |
284 | /* MATLAB Function: '<S1>/Position transformation1' incorporates: |
285 | * Constant: '<S1>/Sentinel Calibration matrix1' |
286 | */ |
287 | Raspber_Positiontransformation1(RaspberrySmithPredictor_B.DataTypeConversion1, |
288 | RaspberrySmithPredictor_B.DataTypeConversion2, |
289 | RaspberrySmithPredictor_P.km4piCam, |
290 | &RaspberrySmithPredictor_B.sf_Positiontransformation1); |
291 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
292 | /* DiscreteIntegrator: '<S5>/Integrator' */ |
293 | RaspberrySmithPredictor_B.Integrator[0] = |
294 | RaspberrySmithPredictor_DW.Integrator_DSTATE[0]; |
295 | |
296 | /* Gain: '<S5>/Setpoint Weighting (Derivative)' incorporates: |
297 | * Constant: '<Root>/Referece position' |
298 | */ |
299 | RaspberrySmithPredictor_B.SetpointWeightingDerivative[0] = |
300 | RaspberrySmithPredictor_P.PIDController2DOF_c * |
301 | RaspberrySmithPredictor_P.Refereceposition_Value[0]; |
302 | |
303 | /* DiscreteIntegrator: '<S5>/Filter' */ |
304 | RaspberrySmithPredictor_B.Filter[0] = |
305 | RaspberrySmithPredictor_DW.Filter_DSTATE[0]; |
306 | |
307 | /* DiscreteIntegrator: '<S5>/Integrator' */ |
308 | RaspberrySmithPredictor_B.Integrator[1] = |
309 | RaspberrySmithPredictor_DW.Integrator_DSTATE[1]; |
310 | |
311 | /* Gain: '<S5>/Setpoint Weighting (Derivative)' incorporates: |
312 | * Constant: '<Root>/Referece position' |
313 | */ |
314 | RaspberrySmithPredictor_B.SetpointWeightingDerivative[1] = |
315 | RaspberrySmithPredictor_P.PIDController2DOF_c * |
316 | RaspberrySmithPredictor_P.Refereceposition_Value[1]; |
317 | |
318 | /* DiscreteIntegrator: '<S5>/Filter' */ |
319 | RaspberrySmithPredictor_B.Filter[1] = |
320 | RaspberrySmithPredictor_DW.Filter_DSTATE[1]; |
321 | } |
322 | |
323 | /* Gain: '<S5>/Filter Coefficient' incorporates: |
324 | * Gain: '<S5>/Derivative Gain' |
325 | * Sum: '<S5>/Sum3' |
326 | * Sum: '<S5>/SumD' |
327 | */ |
328 | RaspberrySmithPredictor_B.FilterCoefficient[0] = |
329 | ((RaspberrySmithPredictor_B.SetpointWeightingDerivative[0] - |
330 | RaspberrySmithPredictor_B.sf_Positiontransformation1.X) * |
331 | RaspberrySmithPredictor_P.PIDController2DOF_D - |
332 | RaspberrySmithPredictor_B.Filter[0]) * |
333 | RaspberrySmithPredictor_P.PIDController2DOF_N; |
334 | RaspberrySmithPredictor_B.FilterCoefficient[1] = |
335 | ((RaspberrySmithPredictor_B.SetpointWeightingDerivative[1] - |
336 | RaspberrySmithPredictor_B.sf_Positiontransformation1.Y) * |
337 | RaspberrySmithPredictor_P.PIDController2DOF_D - |
338 | RaspberrySmithPredictor_B.Filter[1]) * |
339 | RaspberrySmithPredictor_P.PIDController2DOF_N; |
340 | |
341 | /* Sum: '<S5>/Sum' incorporates: |
342 | * Gain: '<S5>/Proportional Gain' |
343 | * Sum: '<S5>/Sum1' |
344 | */ |
345 | rtb_Sum_l_idx_0 = ((RaspberrySmithPredictor_B.SetpointWeightingProportional[0] |
346 | - RaspberrySmithPredictor_B.sf_Positiontransformation1.X) * |
347 | RaspberrySmithPredictor_P.PIDController2DOF_P + |
348 | RaspberrySmithPredictor_B.Integrator[0]) + |
349 | RaspberrySmithPredictor_B.FilterCoefficient[0]; |
350 | rtb_Sum_l_idx_1 = ((RaspberrySmithPredictor_B.SetpointWeightingProportional[1] |
351 | - RaspberrySmithPredictor_B.sf_Positiontransformation1.Y) * |
352 | RaspberrySmithPredictor_P.PIDController2DOF_P + |
353 | RaspberrySmithPredictor_B.Integrator[1]) + |
354 | RaspberrySmithPredictor_B.FilterCoefficient[1]; |
355 | |
356 | /* Saturate: '<S5>/Saturate' */ |
357 | if (rtb_Sum_l_idx_0 > |
358 | RaspberrySmithPredictor_P.PIDController2DOF_UpperSaturati) { |
359 | rtb_Saturate_idx_0 = |
360 | RaspberrySmithPredictor_P.PIDController2DOF_UpperSaturati; |
361 | } else if (rtb_Sum_l_idx_0 < |
362 | RaspberrySmithPredictor_P.PIDController2DOF_LowerSaturati) { |
363 | rtb_Saturate_idx_0 = |
364 | RaspberrySmithPredictor_P.PIDController2DOF_LowerSaturati; |
365 | } else { |
366 | rtb_Saturate_idx_0 = rtb_Sum_l_idx_0; |
367 | } |
368 | |
369 | if (rtb_Sum_l_idx_1 > |
370 | RaspberrySmithPredictor_P.PIDController2DOF_UpperSaturati) { |
371 | rtb_Saturate_idx_1 = |
372 | RaspberrySmithPredictor_P.PIDController2DOF_UpperSaturati; |
373 | } else if (rtb_Sum_l_idx_1 < |
374 | RaspberrySmithPredictor_P.PIDController2DOF_LowerSaturati) { |
375 | rtb_Saturate_idx_1 = |
376 | RaspberrySmithPredictor_P.PIDController2DOF_LowerSaturati; |
377 | } else { |
378 | rtb_Saturate_idx_1 = rtb_Sum_l_idx_1; |
379 | } |
380 | |
381 | /* End of Saturate: '<S5>/Saturate' */ |
382 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
383 | /* Delay: '<Root>/Delay' */ |
384 | RaspberrySmithPredictor_B.Delay[0] = |
385 | RaspberrySmithPredictor_DW.Delay_DSTATE[0]; |
386 | RaspberrySmithPredictor_B.Delay[1] = |
387 | RaspberrySmithPredictor_DW.Delay_DSTATE[1]; |
388 | } |
389 | |
390 | /* Sum: '<Root>/Sum1' incorporates: |
391 | * StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' |
392 | * Sum: '<Root>/Sum' |
393 | */ |
394 | RaspberrySmithPredictor_B.Sum1[0] = rtb_Saturate_idx_0 - |
395 | (RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_d[0] * |
396 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[1] - |
397 | RaspberrySmithPredictor_B.Delay[0]); |
398 | RaspberrySmithPredictor_B.Sum1[1] = rtb_Saturate_idx_1 - |
399 | (RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_d[1] * |
400 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[3] - |
401 | RaspberrySmithPredictor_B.Delay[1]); |
402 | |
403 | /* S-Function (cvxfasd): '<S4>/MagMan linearization1' */ |
404 | cvxfasd_Outputs_wrapper(&RaspberrySmithPredictor_B.Ao[0], |
405 | &RaspberrySmithPredictor_B.Sum1[0], &RaspberrySmithPredictor_B.Sum1[1], |
406 | &RaspberrySmithPredictor_B.MagManlinearization1_o1[0], |
407 | &RaspberrySmithPredictor_B.MagManlinearization1_o2 ); |
408 | |
409 | /* Math: '<S4>/Math Function' */ |
410 | for (iy = 0; iy < 4; iy++) { |
411 | RaspberrySmithPredictor_B.MathFunction[iy << 2] = |
412 | RaspberrySmithPredictor_B.MagManlinearization1_o1[iy]; |
413 | RaspberrySmithPredictor_B.MathFunction[1 + (iy << 2)] = |
414 | RaspberrySmithPredictor_B.MagManlinearization1_o1[iy + 4]; |
415 | RaspberrySmithPredictor_B.MathFunction[2 + (iy << 2)] = |
416 | RaspberrySmithPredictor_B.MagManlinearization1_o1[iy + 8]; |
417 | RaspberrySmithPredictor_B.MathFunction[3 + (iy << 2)] = |
418 | RaspberrySmithPredictor_B.MagManlinearization1_o1[iy + 12]; |
419 | } |
420 | |
421 | /* End of Math: '<S4>/Math Function' */ |
422 | |
423 | /* MATLAB Function: '<Root>/Currents to PWM' */ |
424 | /* MATLAB Function 'Currents to PWM': '<S2>:1' */ |
425 | /* conversion from scaling factor to duty cycle of PWM */ |
426 | /* '<S2>:1:3' Currents = max(0, min(1,Currents) ); */ |
427 | /* limits */ |
428 | /* '<S2>:1:4' i = 0.18515*Currents + 0.069587*Currents.^3 + 0.19285*Currents.^(1/3); */ |
429 | /* ratio to current conversion */ |
430 | /* '<S2>:1:5' PWM = i/0.455/2; */ |
431 | /* '<S2>:1:6' PWM(PWM>0.05) = PWM(PWM>0.05) + 0.5; */ |
432 | /* '<S2>:1:7' PWM(PWM<0.5) = 0; */ |
433 | for (iy = 0; iy < 16; iy++) { |
434 | if ((1.0 <= RaspberrySmithPredictor_B.MathFunction[iy]) || rtIsNaN |
435 | (RaspberrySmithPredictor_B.MathFunction[iy])) { |
436 | Fv_idx_1 = 1.0; |
437 | } else { |
438 | Fv_idx_1 = RaspberrySmithPredictor_B.MathFunction[iy]; |
439 | } |
440 | |
441 | if ((0.0 >= Fv_idx_1) || rtIsNaN(Fv_idx_1)) { |
442 | RaspberrySmithPredictor_B.PWM[iy] = 0.0; |
443 | } else { |
444 | RaspberrySmithPredictor_B.PWM[iy] = Fv_idx_1; |
445 | } |
446 | |
447 | RaspberrySmithPredictor_B.PWM[iy] = ((0.18515 * |
448 | RaspberrySmithPredictor_B.PWM[iy] + 0.069587 * rt_powd_snf |
449 | (RaspberrySmithPredictor_B.PWM[iy], 3.0)) + 0.19285 * rt_powd_snf |
450 | (RaspberrySmithPredictor_B.PWM[iy], 0.33333333333333331)) / 0.455 / 2.0; |
451 | if (RaspberrySmithPredictor_B.PWM[iy] > 0.05) { |
452 | RaspberrySmithPredictor_B.PWM[iy] += 0.5; |
453 | } |
454 | |
455 | if (RaspberrySmithPredictor_B.PWM[iy] < 0.5) { |
456 | RaspberrySmithPredictor_B.PWM[iy] = 0.0; |
457 | } |
458 | } |
459 | |
460 | /* End of MATLAB Function: '<Root>/Currents to PWM' */ |
461 | |
462 | /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */ |
463 | sfcn_setPlatform_Outputs_wrapper(&RaspberrySmithPredictor_B.PWM[0], |
464 | &RaspberrySmithPredictor_DW.MagManCoils_DSTATE); |
465 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
466 | } |
467 | |
468 | /* StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
469 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[0] = 0.0; |
470 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[1] = 0.0; |
471 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[0] += |
472 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_k[0] * |
473 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[1]; |
474 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[1] += |
475 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_k[1] * |
476 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[3]; |
477 | |
478 | /* Sum: '<S5>/SumI1' incorporates: |
479 | * Constant: '<Root>/Referece position' |
480 | * Gain: '<S5>/Integral Gain' |
481 | * Gain: '<S5>/Kb' |
482 | * Sum: '<S5>/Sum2' |
483 | * Sum: '<S5>/SumI2' |
484 | */ |
485 | RaspberrySmithPredictor_B.SumI1[0] = |
486 | (RaspberrySmithPredictor_P.Refereceposition_Value[0] - |
487 | RaspberrySmithPredictor_B.sf_Positiontransformation1.X) * |
488 | RaspberrySmithPredictor_P.PIDController2DOF_I + (rtb_Saturate_idx_0 - |
489 | rtb_Sum_l_idx_0) * RaspberrySmithPredictor_P.PIDController2DOF_Kb; |
490 | RaspberrySmithPredictor_B.SumI1[1] = |
491 | (RaspberrySmithPredictor_P.Refereceposition_Value[1] - |
492 | RaspberrySmithPredictor_B.sf_Positiontransformation1.Y) * |
493 | RaspberrySmithPredictor_P.PIDController2DOF_I + (rtb_Saturate_idx_1 - |
494 | rtb_Sum_l_idx_1) * RaspberrySmithPredictor_P.PIDController2DOF_Kb; |
495 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
496 | /* S-Function "sfcn_getFoilCheck_wrapper" Block: <S3>/MagMan Foil with error */ |
497 | sfcn_getFoilCheck_Update_wrapper |
498 | ( &RaspberrySmithPredictor_B.MagManFoilwitherror_o1, |
499 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o2, |
500 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o3, |
501 | &RaspberrySmithPredictor_B.MagManFoilwitherror_o4, |
502 | &RaspberrySmithPredictor_DW.MagManFoilwitherror_DSTATE); |
503 | |
504 | /* S-Function "sfcn_posread_shm_wrapper" Block: <S6>/Shared memory camera position */ |
505 | sfcn_posread_shm_Update_wrapper |
506 | ( &RaspberrySmithPredictor_B.Sharedmemorycameraposition_o1, |
507 | &RaspberrySmithPredictor_B.Sharedmemorycameraposition_o2, |
508 | &RaspberrySmithPredictor_DW.Sharedmemorycameraposition_DSTA); |
509 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
510 | /* Update for DiscreteIntegrator: '<S5>/Integrator' */ |
511 | RaspberrySmithPredictor_DW.Integrator_DSTATE[0] += |
512 | RaspberrySmithPredictor_P.Integrator_gainval * |
513 | RaspberrySmithPredictor_B.SumI1[0]; |
514 | |
515 | /* Update for DiscreteIntegrator: '<S5>/Filter' */ |
516 | RaspberrySmithPredictor_DW.Filter_DSTATE[0] += |
517 | RaspberrySmithPredictor_P.Filter_gainval * |
518 | RaspberrySmithPredictor_B.FilterCoefficient[0]; |
519 | |
520 | /* Update for Delay: '<Root>/Delay' */ |
521 | RaspberrySmithPredictor_DW.Delay_DSTATE[0] = |
522 | RaspberrySmithPredictor_DW.Delay_DSTATE[2]; |
523 | |
524 | /* Update for DiscreteIntegrator: '<S5>/Integrator' */ |
525 | RaspberrySmithPredictor_DW.Integrator_DSTATE[1] += |
526 | RaspberrySmithPredictor_P.Integrator_gainval * |
527 | RaspberrySmithPredictor_B.SumI1[1]; |
528 | |
529 | /* Update for DiscreteIntegrator: '<S5>/Filter' */ |
530 | RaspberrySmithPredictor_DW.Filter_DSTATE[1] += |
531 | RaspberrySmithPredictor_P.Filter_gainval * |
532 | RaspberrySmithPredictor_B.FilterCoefficient[1]; |
533 | |
534 | /* Update for Delay: '<Root>/Delay' */ |
535 | RaspberrySmithPredictor_DW.Delay_DSTATE[1] = |
536 | RaspberrySmithPredictor_DW.Delay_DSTATE[3]; |
537 | RaspberrySmithPredictor_DW.Delay_DSTATE[2] = |
538 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[0]; |
539 | RaspberrySmithPredictor_DW.Delay_DSTATE[3] = |
540 | RaspberrySmithPredictor_B.Linearmodeloftherollingball2ind[1]; |
541 | } |
542 | |
543 | /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */ |
544 | sfcn_setPlatform_Update_wrapper(&RaspberrySmithPredictor_B.PWM[0], |
545 | &RaspberrySmithPredictor_DW.MagManCoils_DSTATE); |
546 | |
547 | /* External mode */ |
548 | rtExtModeUploadCheckTrigger(2); |
549 | |
550 | { /* Sample time: [0.0s, 0.0s] */ |
551 | rtExtModeUpload(0, RaspberrySmithPredictor_M->Timing.t[0]); |
552 | } |
553 | |
554 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) {/* Sample time: [0.02s, 0.0s] */ |
555 | rtExtModeUpload(1, (((RaspberrySmithPredictor_M->Timing.clockTick1+ |
556 | RaspberrySmithPredictor_M->Timing.clockTickH1* |
557 | 4294967296.0)) * 0.02)); |
558 | } |
559 | } /* end MajorTimeStep */ |
560 | |
561 | if (rtmIsMajorTimeStep(RaspberrySmithPredictor_M)) { |
562 | /* signal main to stop simulation */ |
563 | { /* Sample time: [0.0s, 0.0s] */ |
564 | if ((rtmGetTFinal(RaspberrySmithPredictor_M)!=-1) && |
565 | !((rtmGetTFinal(RaspberrySmithPredictor_M)- |
566 | (((RaspberrySmithPredictor_M->Timing.clockTick1+ |
567 | RaspberrySmithPredictor_M->Timing.clockTickH1* 4294967296.0)) * |
568 | 0.02)) > (((RaspberrySmithPredictor_M->Timing.clockTick1+ |
569 | RaspberrySmithPredictor_M->Timing.clockTickH1* |
570 | 4294967296.0)) * 0.02) * (DBL_EPSILON))) { |
571 | rtmSetErrorStatus(RaspberrySmithPredictor_M, "Simulation finished"); |
572 | } |
573 | |
574 | if (rtmGetStopRequested(RaspberrySmithPredictor_M)) { |
575 | rtmSetErrorStatus(RaspberrySmithPredictor_M, "Simulation finished"); |
576 | } |
577 | } |
578 | |
579 | rt_ertODEUpdateContinuousStates(&RaspberrySmithPredictor_M->solverInfo); |
580 | |
581 | /* Update absolute time for base rate */ |
582 | /* The "clockTick0" counts the number of times the code of this task has |
583 | * been executed. The absolute time is the multiplication of "clockTick0" |
584 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
585 | * overflow during the application lifespan selected. |
586 | * Timer of this task consists of two 32 bit unsigned integers. |
587 | * The two integers represent the low bits Timing.clockTick0 and the high bits |
588 | * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. |
589 | */ |
590 | if (!(++RaspberrySmithPredictor_M->Timing.clockTick0)) { |
591 | ++RaspberrySmithPredictor_M->Timing.clockTickH0; |
592 | } |
593 | |
594 | RaspberrySmithPredictor_M->Timing.t[0] = rtsiGetSolverStopTime |
595 | (&RaspberrySmithPredictor_M->solverInfo); |
596 | |
597 | { |
598 | /* Update absolute timer for sample time: [0.02s, 0.0s] */ |
599 | /* The "clockTick1" counts the number of times the code of this task has |
600 | * been executed. The resolution of this integer timer is 0.02, which is the step size |
601 | * of the task. Size of "clockTick1" ensures timer will not overflow during the |
602 | * application lifespan selected. |
603 | * Timer of this task consists of two 32 bit unsigned integers. |
604 | * The two integers represent the low bits Timing.clockTick1 and the high bits |
605 | * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. |
606 | */ |
607 | RaspberrySmithPredictor_M->Timing.clockTick1++; |
608 | if (!RaspberrySmithPredictor_M->Timing.clockTick1) { |
609 | RaspberrySmithPredictor_M->Timing.clockTickH1++; |
610 | } |
611 | } |
612 | } /* end MajorTimeStep */ |
613 | } |
614 | |
615 | /* Derivatives for root system: '<Root>' */ |
616 | void RaspberrySmithPredictor_derivatives(void) |
617 | { |
618 | XDot_RaspberrySmithPredictor_T *_rtXdot; |
619 | _rtXdot = ((XDot_RaspberrySmithPredictor_T *) |
620 | RaspberrySmithPredictor_M->derivs); |
621 | |
622 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
623 | _rtXdot->Linearmodeloftherollingball2ind[0] = 0.0; |
624 | |
625 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
626 | _rtXdot->Linearmodeloftherollingball2i_n[0] = 0.0; |
627 | |
628 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
629 | _rtXdot->Linearmodeloftherollingball2ind[1] = 0.0; |
630 | |
631 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
632 | _rtXdot->Linearmodeloftherollingball2i_n[1] = 0.0; |
633 | |
634 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
635 | _rtXdot->Linearmodeloftherollingball2ind[2] = 0.0; |
636 | |
637 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
638 | _rtXdot->Linearmodeloftherollingball2i_n[2] = 0.0; |
639 | |
640 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
641 | _rtXdot->Linearmodeloftherollingball2ind[3] = 0.0; |
642 | |
643 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
644 | _rtXdot->Linearmodeloftherollingball2i_n[3] = 0.0; |
645 | |
646 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
647 | _rtXdot->Linearmodeloftherollingball2ind[1] += |
648 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2ind[0] * |
649 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[0]; |
650 | _rtXdot->Linearmodeloftherollingball2ind[3] += |
651 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2ind[1] * |
652 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[2]; |
653 | _rtXdot->Linearmodeloftherollingball2ind[0] += |
654 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_c[0] * |
655 | RaspberrySmithPredictor_B.Sum1[0]; |
656 | _rtXdot->Linearmodeloftherollingball2ind[2] += |
657 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_c[1] * |
658 | RaspberrySmithPredictor_B.Sum1[1]; |
659 | |
660 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
661 | _rtXdot->Linearmodeloftherollingball2i_n[1] += |
662 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_o[0] * |
663 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[0]; |
664 | _rtXdot->Linearmodeloftherollingball2i_n[3] += |
665 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_o[1] * |
666 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[2]; |
667 | _rtXdot->Linearmodeloftherollingball2i_n[0] += |
668 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_g[0] * |
669 | RaspberrySmithPredictor_B.Sum1[0]; |
670 | _rtXdot->Linearmodeloftherollingball2i_n[2] += |
671 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_g[1] * |
672 | RaspberrySmithPredictor_B.Sum1[1]; |
673 | } |
674 | |
675 | /* Model initialize function */ |
676 | void RaspberrySmithPredictor_initialize(void) |
677 | { |
678 | /* Registration code */ |
679 | |
680 | /* initialize non-finites */ |
681 | rt_InitInfAndNaN(sizeof(real_T)); |
682 | |
683 | /* initialize real-time model */ |
684 | (void) memset((void *)RaspberrySmithPredictor_M, 0, |
685 | sizeof(RT_MODEL_RaspberrySmithPredic_T)); |
686 | |
687 | { |
688 | /* Setup solver object */ |
689 | rtsiSetSimTimeStepPtr(&RaspberrySmithPredictor_M->solverInfo, |
690 | &RaspberrySmithPredictor_M->Timing.simTimeStep); |
691 | rtsiSetTPtr(&RaspberrySmithPredictor_M->solverInfo, &rtmGetTPtr |
692 | (RaspberrySmithPredictor_M)); |
693 | rtsiSetStepSizePtr(&RaspberrySmithPredictor_M->solverInfo, |
694 | &RaspberrySmithPredictor_M->Timing.stepSize0); |
695 | rtsiSetdXPtr(&RaspberrySmithPredictor_M->solverInfo, |
696 | &RaspberrySmithPredictor_M->derivs); |
697 | rtsiSetContStatesPtr(&RaspberrySmithPredictor_M->solverInfo, (real_T **) |
698 | &RaspberrySmithPredictor_M->contStates); |
699 | rtsiSetNumContStatesPtr(&RaspberrySmithPredictor_M->solverInfo, |
700 | &RaspberrySmithPredictor_M->Sizes.numContStates); |
701 | rtsiSetNumPeriodicContStatesPtr(&RaspberrySmithPredictor_M->solverInfo, |
702 | &RaspberrySmithPredictor_M->Sizes.numPeriodicContStates); |
703 | rtsiSetPeriodicContStateIndicesPtr(&RaspberrySmithPredictor_M->solverInfo, |
704 | &RaspberrySmithPredictor_M->periodicContStateIndices); |
705 | rtsiSetPeriodicContStateRangesPtr(&RaspberrySmithPredictor_M->solverInfo, |
706 | &RaspberrySmithPredictor_M->periodicContStateRanges); |
707 | rtsiSetErrorStatusPtr(&RaspberrySmithPredictor_M->solverInfo, |
708 | (&rtmGetErrorStatus(RaspberrySmithPredictor_M))); |
709 | rtsiSetRTModelPtr(&RaspberrySmithPredictor_M->solverInfo, |
710 | RaspberrySmithPredictor_M); |
711 | } |
712 | |
713 | rtsiSetSimTimeStep(&RaspberrySmithPredictor_M->solverInfo, MAJOR_TIME_STEP); |
714 | RaspberrySmithPredictor_M->intgData.f[0] = RaspberrySmithPredictor_M->odeF[0]; |
715 | RaspberrySmithPredictor_M->contStates = ((X_RaspberrySmithPredictor_T *) |
716 | &RaspberrySmithPredictor_X); |
717 | rtsiSetSolverData(&RaspberrySmithPredictor_M->solverInfo, (void *) |
718 | &RaspberrySmithPredictor_M->intgData); |
719 | rtsiSetSolverName(&RaspberrySmithPredictor_M->solverInfo,"ode1"); |
720 | rtmSetTPtr(RaspberrySmithPredictor_M, |
721 | &RaspberrySmithPredictor_M->Timing.tArray[0]); |
722 | rtmSetTFinal(RaspberrySmithPredictor_M, 200.0); |
723 | RaspberrySmithPredictor_M->Timing.stepSize0 = 0.02; |
724 | |
725 | /* External mode info */ |
726 | RaspberrySmithPredictor_M->Sizes.checksums[0] = (1572438744U); |
727 | RaspberrySmithPredictor_M->Sizes.checksums[1] = (1708337573U); |
728 | RaspberrySmithPredictor_M->Sizes.checksums[2] = (3258557356U); |
729 | RaspberrySmithPredictor_M->Sizes.checksums[3] = (2585635U); |
730 | |
731 | { |
732 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
733 | static RTWExtModeInfo rt_ExtModeInfo; |
734 | static const sysRanDType *systemRan[5]; |
735 | RaspberrySmithPredictor_M->extModeInfo = (&rt_ExtModeInfo); |
736 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
737 | systemRan[0] = &rtAlwaysEnabled; |
738 | systemRan[1] = &rtAlwaysEnabled; |
739 | systemRan[2] = &rtAlwaysEnabled; |
740 | systemRan[3] = &rtAlwaysEnabled; |
741 | systemRan[4] = &rtAlwaysEnabled; |
742 | rteiSetModelMappingInfoPtr(RaspberrySmithPredictor_M->extModeInfo, |
743 | &RaspberrySmithPredictor_M->SpecialInfo.mappingInfo); |
744 | rteiSetChecksumsPtr(RaspberrySmithPredictor_M->extModeInfo, |
745 | RaspberrySmithPredictor_M->Sizes.checksums); |
746 | rteiSetTPtr(RaspberrySmithPredictor_M->extModeInfo, rtmGetTPtr |
747 | (RaspberrySmithPredictor_M)); |
748 | } |
749 | |
750 | /* block I/O */ |
751 | (void) memset(((void *) &RaspberrySmithPredictor_B), 0, |
752 | sizeof(B_RaspberrySmithPredictor_T)); |
753 | |
754 | /* states (continuous) */ |
755 | { |
756 | (void) memset((void *)&RaspberrySmithPredictor_X, 0, |
757 | sizeof(X_RaspberrySmithPredictor_T)); |
758 | } |
759 | |
760 | /* states (dwork) */ |
761 | (void) memset((void *)&RaspberrySmithPredictor_DW, 0, |
762 | sizeof(DW_RaspberrySmithPredictor_T)); |
763 | |
764 | /* data type transition information */ |
765 | { |
766 | static DataTypeTransInfo dtInfo; |
767 | (void) memset((char_T *) &dtInfo, 0, |
768 | sizeof(dtInfo)); |
769 | RaspberrySmithPredictor_M->SpecialInfo.mappingInfo = (&dtInfo); |
770 | dtInfo.numDataTypes = 14; |
771 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
772 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
773 | |
774 | /* Block I/O transition table */ |
775 | dtInfo.BTransTable = &rtBTransTable; |
776 | |
777 | /* Parameters transition table */ |
778 | dtInfo.PTransTable = &rtPTransTable; |
779 | } |
780 | |
781 | /* S-Function Block: <S3>/MagMan Foil with error */ |
782 | { |
783 | real_T initVector[1] = { 0 }; |
784 | |
785 | { |
786 | int_T i1; |
787 | for (i1=0; i1 < 1; i1++) { |
788 | RaspberrySmithPredictor_DW.MagManFoilwitherror_DSTATE = initVector[0]; |
789 | } |
790 | } |
791 | } |
792 | |
793 | /* S-Function Block: <S6>/Shared memory camera position */ |
794 | { |
795 | real_T initVector[1] = { 0 }; |
796 | |
797 | { |
798 | int_T i1; |
799 | for (i1=0; i1 < 1; i1++) { |
800 | RaspberrySmithPredictor_DW.Sharedmemorycameraposition_DSTA = initVector |
801 | [0]; |
802 | } |
803 | } |
804 | } |
805 | |
806 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */ |
807 | RaspberrySmithPredictor_DW.Integrator_DSTATE[0] = |
808 | RaspberrySmithPredictor_P.Integrator_IC; |
809 | |
810 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */ |
811 | RaspberrySmithPredictor_DW.Filter_DSTATE[0] = |
812 | RaspberrySmithPredictor_P.Filter_IC; |
813 | |
814 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */ |
815 | RaspberrySmithPredictor_DW.Integrator_DSTATE[1] = |
816 | RaspberrySmithPredictor_P.Integrator_IC; |
817 | |
818 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */ |
819 | RaspberrySmithPredictor_DW.Filter_DSTATE[1] = |
820 | RaspberrySmithPredictor_P.Filter_IC; |
821 | |
822 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
823 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[0] = |
824 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_l[0]; |
825 | |
826 | /* InitializeConditions for Delay: '<Root>/Delay' */ |
827 | RaspberrySmithPredictor_DW.Delay_DSTATE[0] = |
828 | RaspberrySmithPredictor_P.Delay_InitialCondition; |
829 | |
830 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
831 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[1] = |
832 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_l[1]; |
833 | |
834 | /* InitializeConditions for Delay: '<Root>/Delay' */ |
835 | RaspberrySmithPredictor_DW.Delay_DSTATE[1] = |
836 | RaspberrySmithPredictor_P.Delay_InitialCondition; |
837 | |
838 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
839 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[2] = |
840 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_l[2]; |
841 | |
842 | /* InitializeConditions for Delay: '<Root>/Delay' */ |
843 | RaspberrySmithPredictor_DW.Delay_DSTATE[2] = |
844 | RaspberrySmithPredictor_P.Delay_InitialCondition; |
845 | |
846 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
847 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2ind[3] = |
848 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_l[3]; |
849 | |
850 | /* InitializeConditions for Delay: '<Root>/Delay' */ |
851 | RaspberrySmithPredictor_DW.Delay_DSTATE[3] = |
852 | RaspberrySmithPredictor_P.Delay_InitialCondition; |
853 | |
854 | /* S-Function Block: <Root>/MagMan Coils */ |
855 | { |
856 | real_T initVector[1] = { 0 }; |
857 | |
858 | { |
859 | int_T i1; |
860 | for (i1=0; i1 < 1; i1++) { |
861 | RaspberrySmithPredictor_DW.MagManCoils_DSTATE = initVector[0]; |
862 | } |
863 | } |
864 | } |
865 | |
866 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)1' */ |
867 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[0] = |
868 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_a[0]; |
869 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[1] = |
870 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_a[1]; |
871 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[2] = |
872 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_a[2]; |
873 | RaspberrySmithPredictor_X.Linearmodeloftherollingball2i_n[3] = |
874 | RaspberrySmithPredictor_P.Linearmodeloftherollingball2i_a[3]; |
875 | } |
876 | |
877 | /* Model terminate function */ |
878 | void RaspberrySmithPredictor_terminate(void) |
879 | { |
880 | /* (no terminate code required) */ |
881 | } |
882 | |