1/*
2 * RaspberryPrediction.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 "RaspberryPrediction".
8 *
9 * Model version : 1.13
10 * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
11 * C source code generated on : Tue Dec 20 13:49:58 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 "RaspberryPrediction.h"
21#include "RaspberryPrediction_private.h"
22#include "RaspberryPrediction_dt.h"
23
24/* Block signals (auto storage) */
25B_RaspberryPrediction_T RaspberryPrediction_B;
26
27/* Continuous states */
28X_RaspberryPrediction_T RaspberryPrediction_X;
29
30/* Block states (auto storage) */
31DW_RaspberryPrediction_T RaspberryPrediction_DW;
32
33/* Real-time model */
34RT_MODEL_RaspberryPrediction_T RaspberryPrediction_M_;
35RT_MODEL_RaspberryPrediction_T *const RaspberryPrediction_M =
36 &RaspberryPrediction_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 = 4;
51 rtsiSetSimTimeStep(si,MINOR_TIME_STEP);
52 rtsiSetdX(si, f0);
53 RaspberryPrediction_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 * '<S4>/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 RaspberryPrediction_step(void)
134{
135 int_T iy;
136 int32_T ix;
137 real_T d;
138 real_T vy;
139 real_T b_d;
140 real_T Fv_idx_0;
141 real_T Fv_idx_1;
142 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
143 /* set solver stop time */
144 if (!(RaspberryPrediction_M->Timing.clockTick0+1)) {
145 rtsiSetSolverStopTime(&RaspberryPrediction_M->solverInfo,
146 ((RaspberryPrediction_M->Timing.clockTickH0 + 1) *
147 RaspberryPrediction_M->Timing.stepSize0 * 4294967296.0));
148 } else {
149 rtsiSetSolverStopTime(&RaspberryPrediction_M->solverInfo,
150 ((RaspberryPrediction_M->Timing.clockTick0 + 1) *
151 RaspberryPrediction_M->Timing.stepSize0 +
152 RaspberryPrediction_M->Timing.clockTickH0 *
153 RaspberryPrediction_M->Timing.stepSize0 * 4294967296.0));
154 }
155 } /* end MajorTimeStep */
156
157 /* Update absolute time of base rate at minor time step */
158 if (rtmIsMinorTimeStep(RaspberryPrediction_M)) {
159 RaspberryPrediction_M->Timing.t[0] = rtsiGetT
160 (&RaspberryPrediction_M->solverInfo);
161 }
162
163 /* StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */
164 RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] = 0.0;
165 RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] = 0.0;
166 RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] +=
167 RaspberryPrediction_P.Linearmodeloftherollingball2i_h[0] *
168 RaspberryPrediction_X.Linearmodeloftherollingball2ind[1];
169 RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] +=
170 RaspberryPrediction_P.Linearmodeloftherollingball2i_h[1] *
171 RaspberryPrediction_X.Linearmodeloftherollingball2ind[3];
172
173 /* MATLAB Function: '<S5>/Embedded MATLAB Function' */
174 /* MATLAB Function 'Force to Currents/Embedded MATLAB Function': '<S10>:1' */
175 /* '<S10>:1:2' Ao=zeros(2,16); */
176 memset(&RaspberryPrediction_B.Ao[0], 0, sizeof(real_T) << 5U);
177
178 /* '<S10>:1:4' for iy=1:4 */
179 for (iy = 0; iy < 4; iy++) {
180 /* '<S10>:1:5' for ix=1:4 */
181 for (ix = 0; ix < 4; ix++) {
182 /* '<S10>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */
183 /* '<S10>:1:14' d=dist(x,y, ix, iy); */
184 /* '<S10>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
185 d = RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] - (1.0 +
186 (real_T)ix);
187 Fv_idx_1 = RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] - (1.0
188 + (real_T)iy);
189 d = sqrt(d * d + Fv_idx_1 * Fv_idx_1);
190
191 /* '<S10>:1:15' if d~=0 */
192 if (d != 0.0) {
193 /* '<S10>:1:16' F=gForce(d); */
194 /* '<S10>:1:43' a = 622.06; */
195 /* '<S10>:1:44' b = 9.5032; */
196 /* '<S10>:1:45' c = 3.1884; */
197 /* '<S10>:1:46' F = x.*a./(b.*x.^2+ c).^3; */
198 d = d * 622.06 / rt_powd_snf(d * d * 9.5032 + 3.1884, 3.0);
199
200 /* '<S10>:1:17' Fv=F*vector(x,y,ix,iy); */
201 /* '<S10>:1:32' vx=ix-x; */
202 Fv_idx_1 = (1.0 + (real_T)ix) -
203 RaspberryPrediction_B.Linearmodeloftherollingball2ind[0];
204
205 /* '<S10>:1:33' vy=iy-y; */
206 vy = (1.0 + (real_T)iy) -
207 RaspberryPrediction_B.Linearmodeloftherollingball2ind[1];
208
209 /* '<S10>:1:35' if(vx==0 && vy ==0) */
210 if ((Fv_idx_1 == 0.0) && (vy == 0.0)) {
211 /* '<S10>:1:36' v=[0 0]; */
212 Fv_idx_0 = 0.0;
213 Fv_idx_1 = 0.0;
214 } else {
215 /* '<S10>:1:37' else */
216 /* '<S10>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */
217 /* '<S10>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
218 b_d = sqrt(Fv_idx_1 * Fv_idx_1 + vy * vy);
219 Fv_idx_0 = Fv_idx_1 / b_d;
220 Fv_idx_1 = vy / b_d;
221 }
222
223 Fv_idx_0 *= d;
224 Fv_idx_1 *= d;
225
226 /* '<S10>:1:18' Fx=Fv(1); */
227 d = Fv_idx_0;
228
229 /* '<S10>:1:19' Fy=Fv(2); */
230 } else {
231 /* '<S10>:1:20' else */
232 /* '<S10>:1:21' Fx=0; */
233 d = 0.0;
234
235 /* '<S10>:1:22' Fy=0; */
236 Fv_idx_1 = 0.0;
237 }
238
239 /* '<S10>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */
240 RaspberryPrediction_B.Ao[((iy << 2) + ix) << 1] = d;
241
242 /* '<S10>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */
243 RaspberryPrediction_B.Ao[1 + (((iy << 2) + ix) << 1)] = Fv_idx_1;
244 }
245 }
246
247 /* End of MATLAB Function: '<S5>/Embedded MATLAB Function' */
248
249 /* S-Function (cvxfasd): '<S5>/MagMan linearization1' */
250 cvxfasd_Outputs_wrapper(&RaspberryPrediction_B.Ao[0],
251 &RaspberryPrediction_P.ReferenceForce_Value[0],
252 &RaspberryPrediction_P.ReferenceForce_Value[1],
253 &RaspberryPrediction_B.MagManlinearization1_o1[0],
254 &RaspberryPrediction_B.MagManlinearization1_o2 );
255
256 /* Math: '<S5>/Math Function' */
257 for (iy = 0; iy < 4; iy++) {
258 RaspberryPrediction_B.MathFunction[iy << 2] =
259 RaspberryPrediction_B.MagManlinearization1_o1[iy];
260 RaspberryPrediction_B.MathFunction[1 + (iy << 2)] =
261 RaspberryPrediction_B.MagManlinearization1_o1[iy + 4];
262 RaspberryPrediction_B.MathFunction[2 + (iy << 2)] =
263 RaspberryPrediction_B.MagManlinearization1_o1[iy + 8];
264 RaspberryPrediction_B.MathFunction[3 + (iy << 2)] =
265 RaspberryPrediction_B.MagManlinearization1_o1[iy + 12];
266 }
267
268 /* End of Math: '<S5>/Math Function' */
269
270 /* MATLAB Function: '<Root>/Currents to PWM1' */
271 /* MATLAB Function 'Currents to PWM': '<S3>:1' */
272 /* conversion from scaling factor to duty cycle of PWM */
273 /* '<S3>:1:3' Currents = max(0, min(1,Currents) ); */
274 /* limits */
275 /* '<S3>:1:4' i = 0.18515*Currents + 0.069587*Currents.^3 + 0.19285*Currents.^(1/3); */
276 /* ratio to current conversion */
277 /* '<S3>:1:5' PWM = i/0.455/2; */
278 /* '<S3>:1:6' PWM(PWM>0.05) = PWM(PWM>0.05) + 0.5; */
279 /* '<S3>:1:7' PWM(PWM<0.5) = 0; */
280 for (iy = 0; iy < 16; iy++) {
281 if ((1.0 <= RaspberryPrediction_B.MathFunction[iy]) || rtIsNaN
282 (RaspberryPrediction_B.MathFunction[iy])) {
283 d = 1.0;
284 } else {
285 d = RaspberryPrediction_B.MathFunction[iy];
286 }
287
288 if ((0.0 >= d) || rtIsNaN(d)) {
289 RaspberryPrediction_B.PWM[iy] = 0.0;
290 } else {
291 RaspberryPrediction_B.PWM[iy] = d;
292 }
293
294 RaspberryPrediction_B.PWM[iy] = ((0.18515 * RaspberryPrediction_B.PWM[iy] +
295 0.069587 * rt_powd_snf(RaspberryPrediction_B.PWM[iy], 3.0)) + 0.19285 *
296 rt_powd_snf(RaspberryPrediction_B.PWM[iy], 0.33333333333333331)) / 0.455 /
297 2.0;
298 if (RaspberryPrediction_B.PWM[iy] > 0.05) {
299 RaspberryPrediction_B.PWM[iy] += 0.5;
300 }
301
302 if (RaspberryPrediction_B.PWM[iy] < 0.5) {
303 RaspberryPrediction_B.PWM[iy] = 0.0;
304 }
305 }
306
307 /* End of MATLAB Function: '<Root>/Currents to PWM1' */
308
309 /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */
310 sfcn_setPlatform_Outputs_wrapper(&RaspberryPrediction_B.PWM[0],
311 &RaspberryPrediction_DW.MagManCoils_DSTATE);
312 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
313 /* S-Function (sfcn_getFoilCheck): '<S4>/MagMan Foil with error' */
314 sfcn_getFoilCheck_Outputs_wrapper
315 ( &RaspberryPrediction_B.MagManFoilwitherror_o1,
316 &RaspberryPrediction_B.MagManFoilwitherror_o2,
317 &RaspberryPrediction_B.MagManFoilwitherror_o3,
318 &RaspberryPrediction_B.MagManFoilwitherror_o4,
319 &RaspberryPrediction_DW.MagManFoilwitherror_DSTATE);
320
321 /* MATLAB Function: '<S4>/Position transformation' incorporates:
322 * Constant: '<S4>/Sentinel Calibration matrix'
323 */
324 Raspber_Positiontransformation1(RaspberryPrediction_B.MagManFoilwitherror_o1,
325 RaspberryPrediction_B.MagManFoilwitherror_o2, RaspberryPrediction_P.km4pi,
326 &RaspberryPrediction_B.sf_Positiontransformation);
327 }
328
329 /* S-Function (sfcn_posread_shm): '<S6>/Shared memory camera position' */
330 sfcn_posread_shm_Outputs_wrapper
331 ( &RaspberryPrediction_B.Sharedmemorycameraposition_o1,
332 &RaspberryPrediction_B.Sharedmemorycameraposition_o2,
333 &RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA);
334
335 /* DataTypeConversion: '<S6>/Data Type Conversion1' */
336 RaspberryPrediction_B.DataTypeConversion1 =
337 RaspberryPrediction_B.Sharedmemorycameraposition_o1;
338
339 /* DataTypeConversion: '<S6>/Data Type Conversion2' */
340 RaspberryPrediction_B.DataTypeConversion2 =
341 RaspberryPrediction_B.Sharedmemorycameraposition_o2;
342
343 /* MATLAB Function: '<S1>/Position transformation1' incorporates:
344 * Constant: '<S1>/Sentinel Calibration matrix1'
345 */
346 Raspber_Positiontransformation1(RaspberryPrediction_B.DataTypeConversion1,
347 RaspberryPrediction_B.DataTypeConversion2, RaspberryPrediction_P.km4piCam,
348 &RaspberryPrediction_B.sf_Positiontransformation1);
349 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
350 /* Delay: '<Root>/Delay' */
351 RaspberryPrediction_B.Delay[0] = RaspberryPrediction_DW.Delay_DSTATE[0];
352 RaspberryPrediction_B.Delay[1] = RaspberryPrediction_DW.Delay_DSTATE[1];
353 }
354
355 /* Sum: '<Root>/Sum1' incorporates:
356 * Constant: '<Root>/Reference Force'
357 * Gain: '<Root>/L'
358 * Sum: '<Root>/Sum'
359 */
360 RaspberryPrediction_B.Sum1[0] = RaspberryPrediction_P.ReferenceForce_Value[0]
361 - (RaspberryPrediction_B.Delay[0] -
362 RaspberryPrediction_B.sf_Positiontransformation1.X) *
363 RaspberryPrediction_P.L_Gain;
364 RaspberryPrediction_B.Sum1[1] = RaspberryPrediction_P.ReferenceForce_Value[1]
365 - (RaspberryPrediction_B.Delay[1] -
366 RaspberryPrediction_B.sf_Positiontransformation1.Y) *
367 RaspberryPrediction_P.L_Gain;
368 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
369 /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */
370 sfcn_setPlatform_Update_wrapper(&RaspberryPrediction_B.PWM[0],
371 &RaspberryPrediction_DW.MagManCoils_DSTATE);
372 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
373 /* S-Function "sfcn_getFoilCheck_wrapper" Block: <S4>/MagMan Foil with error */
374 sfcn_getFoilCheck_Update_wrapper
375 ( &RaspberryPrediction_B.MagManFoilwitherror_o1,
376 &RaspberryPrediction_B.MagManFoilwitherror_o2,
377 &RaspberryPrediction_B.MagManFoilwitherror_o3,
378 &RaspberryPrediction_B.MagManFoilwitherror_o4,
379 &RaspberryPrediction_DW.MagManFoilwitherror_DSTATE);
380 }
381
382 /* S-Function "sfcn_posread_shm_wrapper" Block: <S6>/Shared memory camera position */
383 sfcn_posread_shm_Update_wrapper
384 ( &RaspberryPrediction_B.Sharedmemorycameraposition_o1,
385 &RaspberryPrediction_B.Sharedmemorycameraposition_o2,
386 &RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA);
387 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
388 /* Update for Delay: '<Root>/Delay' */
389 RaspberryPrediction_DW.Delay_DSTATE[0] =
390 RaspberryPrediction_DW.Delay_DSTATE[2];
391 RaspberryPrediction_DW.Delay_DSTATE[1] =
392 RaspberryPrediction_DW.Delay_DSTATE[3];
393 RaspberryPrediction_DW.Delay_DSTATE[2] =
394 RaspberryPrediction_B.Linearmodeloftherollingball2ind[0];
395 RaspberryPrediction_DW.Delay_DSTATE[3] =
396 RaspberryPrediction_B.Linearmodeloftherollingball2ind[1];
397 }
398
399 /* External mode */
400 rtExtModeUploadCheckTrigger(2);
401
402 { /* Sample time: [0.0s, 0.0s] */
403 rtExtModeUpload(0, RaspberryPrediction_M->Timing.t[0]);
404 }
405
406 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {/* Sample time: [0.02s, 0.0s] */
407 rtExtModeUpload(1, (((RaspberryPrediction_M->Timing.clockTick1+
408 RaspberryPrediction_M->Timing.clockTickH1*
409 4294967296.0)) * 0.02));
410 }
411 } /* end MajorTimeStep */
412
413 if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {
414 /* signal main to stop simulation */
415 { /* Sample time: [0.0s, 0.0s] */
416 if ((rtmGetTFinal(RaspberryPrediction_M)!=-1) &&
417 !((rtmGetTFinal(RaspberryPrediction_M)-
418 (((RaspberryPrediction_M->Timing.clockTick1+
419 RaspberryPrediction_M->Timing.clockTickH1* 4294967296.0)) * 0.02))
420 > (((RaspberryPrediction_M->Timing.clockTick1+
421 RaspberryPrediction_M->Timing.clockTickH1* 4294967296.0)) *
422 0.02) * (DBL_EPSILON))) {
423 rtmSetErrorStatus(RaspberryPrediction_M, "Simulation finished");
424 }
425
426 if (rtmGetStopRequested(RaspberryPrediction_M)) {
427 rtmSetErrorStatus(RaspberryPrediction_M, "Simulation finished");
428 }
429 }
430
431 rt_ertODEUpdateContinuousStates(&RaspberryPrediction_M->solverInfo);
432
433 /* Update absolute time for base rate */
434 /* The "clockTick0" counts the number of times the code of this task has
435 * been executed. The absolute time is the multiplication of "clockTick0"
436 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
437 * overflow during the application lifespan selected.
438 * Timer of this task consists of two 32 bit unsigned integers.
439 * The two integers represent the low bits Timing.clockTick0 and the high bits
440 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
441 */
442 if (!(++RaspberryPrediction_M->Timing.clockTick0)) {
443 ++RaspberryPrediction_M->Timing.clockTickH0;
444 }
445
446 RaspberryPrediction_M->Timing.t[0] = rtsiGetSolverStopTime
447 (&RaspberryPrediction_M->solverInfo);
448
449 {
450 /* Update absolute timer for sample time: [0.02s, 0.0s] */
451 /* The "clockTick1" counts the number of times the code of this task has
452 * been executed. The resolution of this integer timer is 0.02, which is the step size
453 * of the task. Size of "clockTick1" ensures timer will not overflow during the
454 * application lifespan selected.
455 * Timer of this task consists of two 32 bit unsigned integers.
456 * The two integers represent the low bits Timing.clockTick1 and the high bits
457 * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
458 */
459 RaspberryPrediction_M->Timing.clockTick1++;
460 if (!RaspberryPrediction_M->Timing.clockTick1) {
461 RaspberryPrediction_M->Timing.clockTickH1++;
462 }
463 }
464 } /* end MajorTimeStep */
465}
466
467/* Derivatives for root system: '<Root>' */
468void RaspberryPrediction_derivatives(void)
469{
470 XDot_RaspberryPrediction_T *_rtXdot;
471 _rtXdot = ((XDot_RaspberryPrediction_T *) RaspberryPrediction_M->derivs);
472
473 /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */
474 _rtXdot->Linearmodeloftherollingball2ind[0] = 0.0;
475 _rtXdot->Linearmodeloftherollingball2ind[1] = 0.0;
476 _rtXdot->Linearmodeloftherollingball2ind[2] = 0.0;
477 _rtXdot->Linearmodeloftherollingball2ind[3] = 0.0;
478 _rtXdot->Linearmodeloftherollingball2ind[1] +=
479 RaspberryPrediction_P.Linearmodeloftherollingball2ind[0] *
480 RaspberryPrediction_X.Linearmodeloftherollingball2ind[0];
481 _rtXdot->Linearmodeloftherollingball2ind[3] +=
482 RaspberryPrediction_P.Linearmodeloftherollingball2ind[1] *
483 RaspberryPrediction_X.Linearmodeloftherollingball2ind[2];
484 _rtXdot->Linearmodeloftherollingball2ind[0] +=
485 RaspberryPrediction_P.Linearmodeloftherollingball2i_l[0] *
486 RaspberryPrediction_B.Sum1[0];
487 _rtXdot->Linearmodeloftherollingball2ind[2] +=
488 RaspberryPrediction_P.Linearmodeloftherollingball2i_l[1] *
489 RaspberryPrediction_B.Sum1[1];
490}
491
492/* Model initialize function */
493void RaspberryPrediction_initialize(void)
494{
495 /* Registration code */
496
497 /* initialize non-finites */
498 rt_InitInfAndNaN(sizeof(real_T));
499
500 /* initialize real-time model */
501 (void) memset((void *)RaspberryPrediction_M, 0,
502 sizeof(RT_MODEL_RaspberryPrediction_T));
503
504 {
505 /* Setup solver object */
506 rtsiSetSimTimeStepPtr(&RaspberryPrediction_M->solverInfo,
507 &RaspberryPrediction_M->Timing.simTimeStep);
508 rtsiSetTPtr(&RaspberryPrediction_M->solverInfo, &rtmGetTPtr
509 (RaspberryPrediction_M));
510 rtsiSetStepSizePtr(&RaspberryPrediction_M->solverInfo,
511 &RaspberryPrediction_M->Timing.stepSize0);
512 rtsiSetdXPtr(&RaspberryPrediction_M->solverInfo,
513 &RaspberryPrediction_M->derivs);
514 rtsiSetContStatesPtr(&RaspberryPrediction_M->solverInfo, (real_T **)
515 &RaspberryPrediction_M->contStates);
516 rtsiSetNumContStatesPtr(&RaspberryPrediction_M->solverInfo,
517 &RaspberryPrediction_M->Sizes.numContStates);
518 rtsiSetNumPeriodicContStatesPtr(&RaspberryPrediction_M->solverInfo,
519 &RaspberryPrediction_M->Sizes.numPeriodicContStates);
520 rtsiSetPeriodicContStateIndicesPtr(&RaspberryPrediction_M->solverInfo,
521 &RaspberryPrediction_M->periodicContStateIndices);
522 rtsiSetPeriodicContStateRangesPtr(&RaspberryPrediction_M->solverInfo,
523 &RaspberryPrediction_M->periodicContStateRanges);
524 rtsiSetErrorStatusPtr(&RaspberryPrediction_M->solverInfo,
525 (&rtmGetErrorStatus(RaspberryPrediction_M)));
526 rtsiSetRTModelPtr(&RaspberryPrediction_M->solverInfo, RaspberryPrediction_M);
527 }
528
529 rtsiSetSimTimeStep(&RaspberryPrediction_M->solverInfo, MAJOR_TIME_STEP);
530 RaspberryPrediction_M->intgData.f[0] = RaspberryPrediction_M->odeF[0];
531 RaspberryPrediction_M->contStates = ((X_RaspberryPrediction_T *)
532 &RaspberryPrediction_X);
533 rtsiSetSolverData(&RaspberryPrediction_M->solverInfo, (void *)
534 &RaspberryPrediction_M->intgData);
535 rtsiSetSolverName(&RaspberryPrediction_M->solverInfo,"ode1");
536 rtmSetTPtr(RaspberryPrediction_M, &RaspberryPrediction_M->Timing.tArray[0]);
537 rtmSetTFinal(RaspberryPrediction_M, -1);
538 RaspberryPrediction_M->Timing.stepSize0 = 0.02;
539
540 /* External mode info */
541 RaspberryPrediction_M->Sizes.checksums[0] = (3595132348U);
542 RaspberryPrediction_M->Sizes.checksums[1] = (4069635331U);
543 RaspberryPrediction_M->Sizes.checksums[2] = (3308004464U);
544 RaspberryPrediction_M->Sizes.checksums[3] = (2480287856U);
545
546 {
547 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
548 static RTWExtModeInfo rt_ExtModeInfo;
549 static const sysRanDType *systemRan[5];
550 RaspberryPrediction_M->extModeInfo = (&rt_ExtModeInfo);
551 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
552 systemRan[0] = &rtAlwaysEnabled;
553 systemRan[1] = &rtAlwaysEnabled;
554 systemRan[2] = &rtAlwaysEnabled;
555 systemRan[3] = &rtAlwaysEnabled;
556 systemRan[4] = &rtAlwaysEnabled;
557 rteiSetModelMappingInfoPtr(RaspberryPrediction_M->extModeInfo,
558 &RaspberryPrediction_M->SpecialInfo.mappingInfo);
559 rteiSetChecksumsPtr(RaspberryPrediction_M->extModeInfo,
560 RaspberryPrediction_M->Sizes.checksums);
561 rteiSetTPtr(RaspberryPrediction_M->extModeInfo, rtmGetTPtr
562 (RaspberryPrediction_M));
563 }
564
565 /* block I/O */
566 (void) memset(((void *) &RaspberryPrediction_B), 0,
567 sizeof(B_RaspberryPrediction_T));
568
569 /* states (continuous) */
570 {
571 (void) memset((void *)&RaspberryPrediction_X, 0,
572 sizeof(X_RaspberryPrediction_T));
573 }
574
575 /* states (dwork) */
576 (void) memset((void *)&RaspberryPrediction_DW, 0,
577 sizeof(DW_RaspberryPrediction_T));
578
579 /* data type transition information */
580 {
581 static DataTypeTransInfo dtInfo;
582 (void) memset((char_T *) &dtInfo, 0,
583 sizeof(dtInfo));
584 RaspberryPrediction_M->SpecialInfo.mappingInfo = (&dtInfo);
585 dtInfo.numDataTypes = 14;
586 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
587 dtInfo.dataTypeNames = &rtDataTypeNames[0];
588
589 /* Block I/O transition table */
590 dtInfo.BTransTable = &rtBTransTable;
591
592 /* Parameters transition table */
593 dtInfo.PTransTable = &rtPTransTable;
594 }
595
596 /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */
597 RaspberryPrediction_X.Linearmodeloftherollingball2ind[0] =
598 RaspberryPrediction_P.Linearmodeloftherollingball2i_a[0];
599 RaspberryPrediction_X.Linearmodeloftherollingball2ind[1] =
600 RaspberryPrediction_P.Linearmodeloftherollingball2i_a[1];
601 RaspberryPrediction_X.Linearmodeloftherollingball2ind[2] =
602 RaspberryPrediction_P.Linearmodeloftherollingball2i_a[2];
603 RaspberryPrediction_X.Linearmodeloftherollingball2ind[3] =
604 RaspberryPrediction_P.Linearmodeloftherollingball2i_a[3];
605
606 /* S-Function Block: <Root>/MagMan Coils */
607 {
608 real_T initVector[1] = { 0 };
609
610 {
611 int_T i1;
612 for (i1=0; i1 < 1; i1++) {
613 RaspberryPrediction_DW.MagManCoils_DSTATE = initVector[0];
614 }
615 }
616 }
617
618 /* S-Function Block: <S4>/MagMan Foil with error */
619 {
620 real_T initVector[1] = { 0 };
621
622 {
623 int_T i1;
624 for (i1=0; i1 < 1; i1++) {
625 RaspberryPrediction_DW.MagManFoilwitherror_DSTATE = initVector[0];
626 }
627 }
628 }
629
630 /* S-Function Block: <S6>/Shared memory camera position */
631 {
632 real_T initVector[1] = { 0 };
633
634 {
635 int_T i1;
636 for (i1=0; i1 < 1; i1++) {
637 RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA = initVector[0];
638 }
639 }
640 }
641
642 /* InitializeConditions for Delay: '<Root>/Delay' */
643 RaspberryPrediction_DW.Delay_DSTATE[0] =
644 RaspberryPrediction_P.Delay_InitialCondition;
645 RaspberryPrediction_DW.Delay_DSTATE[1] =
646 RaspberryPrediction_P.Delay_InitialCondition;
647 RaspberryPrediction_DW.Delay_DSTATE[2] =
648 RaspberryPrediction_P.Delay_InitialCondition;
649 RaspberryPrediction_DW.Delay_DSTATE[3] =
650 RaspberryPrediction_P.Delay_InitialCondition;
651}
652
653/* Model terminate function */
654void RaspberryPrediction_terminate(void)
655{
656 /* (no terminate code required) */
657}
658