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) */
25B_RaspberrySmithPredictor_T RaspberrySmithPredictor_B;
26
27/* Continuous states */
28X_RaspberrySmithPredictor_T RaspberrySmithPredictor_X;
29
30/* Block states (auto storage) */
31DW_RaspberrySmithPredictor_T RaspberrySmithPredictor_DW;
32
33/* Real-time model */
34RT_MODEL_RaspberrySmithPredic_T RaspberrySmithPredictor_M_;
35RT_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 */
42static 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 */
67void 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
86real_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 */
133void 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>' */
616void 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 */
676void 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 */
878void RaspberrySmithPredictor_terminate(void)
879{
880 /* (no terminate code required) */
881}
882