1/*
2 * VisionControl.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 "VisionControl".
8 *
9 * Model version : 1.14
10 * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
11 * C source code generated on : Thu Dec 01 15:18:37 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 "VisionControl.h"
21#include "VisionControl_private.h"
22#include "VisionControl_dt.h"
23
24/* Block signals (auto storage) */
25B_VisionControl_T VisionControl_B;
26
27/* Block states (auto storage) */
28DW_VisionControl_T VisionControl_DW;
29
30/* Real-time model */
31RT_MODEL_VisionControl_T VisionControl_M_;
32RT_MODEL_VisionControl_T *const VisionControl_M = &VisionControl_M_;
33real_T rt_powd_snf(real_T u0, real_T u1)
34{
35 real_T y;
36 real_T tmp;
37 real_T tmp_0;
38 if (rtIsNaN(u0) || rtIsNaN(u1)) {
39 y = (rtNaN);
40 } else {
41 tmp = fabs(u0);
42 tmp_0 = fabs(u1);
43 if (rtIsInf(u1)) {
44 if (tmp == 1.0) {
45 y = (rtNaN);
46 } else if (tmp > 1.0) {
47 if (u1 > 0.0) {
48 y = (rtInf);
49 } else {
50 y = 0.0;
51 }
52 } else if (u1 > 0.0) {
53 y = 0.0;
54 } else {
55 y = (rtInf);
56 }
57 } else if (tmp_0 == 0.0) {
58 y = 1.0;
59 } else if (tmp_0 == 1.0) {
60 if (u1 > 0.0) {
61 y = u0;
62 } else {
63 y = 1.0 / u0;
64 }
65 } else if (u1 == 2.0) {
66 y = u0 * u0;
67 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
68 y = sqrt(u0);
69 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
70 y = (rtNaN);
71 } else {
72 y = pow(u0, u1);
73 }
74 }
75
76 return y;
77}
78
79/* Model step function */
80void VisionControl_step(void)
81{
82 int32_T i;
83 int32_T xoffset;
84 real_T rtb_lim;
85 real_T rtb_FilterCoefficient_idx_0;
86 real_T rtb_FilterCoefficient_idx_1;
87 real_T rtb_Sum_idx_0;
88 real_T rtb_Sum_idx_1;
89
90 /* S-Function (sfcn_posread_shm): '<S1>/Shared memory camera position' */
91 sfcn_posread_shm_Outputs_wrapper
92 ( &VisionControl_B.Sharedmemorycameraposition_o1,
93 &VisionControl_B.Sharedmemorycameraposition_o2,
94 &VisionControl_DW.Sharedmemorycameraposition_DSTA);
95
96 /* DataTypeConversion: '<S1>/Data Type Conversion1' */
97 VisionControl_B.DataTypeConversion1 =
98 VisionControl_B.Sharedmemorycameraposition_o1;
99
100 /* DataTypeConversion: '<S1>/Data Type Conversion2' */
101 VisionControl_B.DataTypeConversion2 =
102 VisionControl_B.Sharedmemorycameraposition_o2;
103
104 /* MATLAB Function: '<Root>/Position transformation' incorporates:
105 * Constant: '<Root>/Raspicam Calibration matrix'
106 */
107 /* MATLAB Function 'Position transformation': '<S6>:1' */
108 /* out=km2*[x;y;x^2;y^2;x*y;1]; */
109 /* '<S6>:1:4' xa = [x,y]; */
110 /* '<S6>:1:5' H = km4(:); */
111 /* '<S6>:1:6' xa = [xa,ones(size(xa,1),1)]; */
112 /* homogeneous coordinates */
113 /* '<S6>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */
114 /* '<S6>:1:9' X=out(1); */
115 VisionControl_B.X = ((VisionControl_B.DataTypeConversion1 *
116 VisionControl_P.km4piCam[0] +
117 VisionControl_B.DataTypeConversion2 *
118 VisionControl_P.km4piCam[1]) + VisionControl_P.km4piCam
119 [2]) / ((VisionControl_B.DataTypeConversion1 *
120 VisionControl_P.km4piCam[6] + VisionControl_B.DataTypeConversion2 *
121 VisionControl_P.km4piCam[7]) + VisionControl_P.km4piCam[8]);
122
123 /* '<S6>:1:10' Y=out(2); */
124 VisionControl_B.Y = ((VisionControl_B.DataTypeConversion1 *
125 VisionControl_P.km4piCam[3] +
126 VisionControl_B.DataTypeConversion2 *
127 VisionControl_P.km4piCam[4]) + VisionControl_P.km4piCam
128 [5]) / ((VisionControl_B.DataTypeConversion1 *
129 VisionControl_P.km4piCam[6] + VisionControl_B.DataTypeConversion2 *
130 VisionControl_P.km4piCam[7]) + VisionControl_P.km4piCam[8]);
131
132 /* MATLAB Function: '<S3>/Embedded MATLAB Function' */
133 /* MATLAB Function 'Linearization/Embedded MATLAB Function': '<S7>:1' */
134 /* '<S7>:1:2' Ao=zeros(2,16); */
135 memset(&VisionControl_B.Ao[0], 0, sizeof(real_T) << 5U);
136
137 /* '<S7>:1:4' for iy=1:4 */
138 for (xoffset = 0; xoffset < 4; xoffset++) {
139 /* '<S7>:1:5' for ix=1:4 */
140 for (i = 0; i < 4; i++) {
141 /* '<S7>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */
142 /* '<S7>:1:14' d=dist(x,y, ix, iy); */
143 /* '<S7>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
144 rtb_lim = VisionControl_B.X - (1.0 + (real_T)i);
145 rtb_Sum_idx_1 = VisionControl_B.Y - (1.0 + (real_T)xoffset);
146 rtb_lim = sqrt(rtb_lim * rtb_lim + rtb_Sum_idx_1 * rtb_Sum_idx_1);
147
148 /* '<S7>:1:15' if d~=0 */
149 if (rtb_lim != 0.0) {
150 /* '<S7>:1:16' F=gForce(d); */
151 /* '<S7>:1:43' a = 622.06; */
152 /* '<S7>:1:44' b = 9.5032; */
153 /* '<S7>:1:45' c = 3.1884; */
154 /* '<S7>:1:46' F = x.*a./(b.*x.^2+ c).^3; */
155 rtb_lim = rtb_lim * 622.06 / rt_powd_snf(rtb_lim * rtb_lim * 9.5032 +
156 3.1884, 3.0);
157
158 /* '<S7>:1:17' Fv=F*vector(x,y,ix,iy); */
159 /* '<S7>:1:32' vx=ix-x; */
160 rtb_Sum_idx_1 = (1.0 + (real_T)i) - VisionControl_B.X;
161
162 /* '<S7>:1:33' vy=iy-y; */
163 rtb_FilterCoefficient_idx_0 = (1.0 + (real_T)xoffset) -
164 VisionControl_B.Y;
165
166 /* '<S7>:1:35' if(vx==0 && vy ==0) */
167 if ((rtb_Sum_idx_1 == 0.0) && (rtb_FilterCoefficient_idx_0 == 0.0)) {
168 /* '<S7>:1:36' v=[0 0]; */
169 rtb_Sum_idx_0 = 0.0;
170 rtb_Sum_idx_1 = 0.0;
171 } else {
172 /* '<S7>:1:37' else */
173 /* '<S7>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */
174 /* '<S7>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
175 rtb_FilterCoefficient_idx_1 = sqrt(rtb_Sum_idx_1 * rtb_Sum_idx_1 +
176 rtb_FilterCoefficient_idx_0 * rtb_FilterCoefficient_idx_0);
177 rtb_Sum_idx_0 = rtb_Sum_idx_1 / rtb_FilterCoefficient_idx_1;
178 rtb_Sum_idx_1 = rtb_FilterCoefficient_idx_0 /
179 rtb_FilterCoefficient_idx_1;
180 }
181
182 rtb_Sum_idx_0 *= rtb_lim;
183 rtb_Sum_idx_1 *= rtb_lim;
184
185 /* '<S7>:1:18' Fx=Fv(1); */
186 rtb_lim = rtb_Sum_idx_0;
187
188 /* '<S7>:1:19' Fy=Fv(2); */
189 } else {
190 /* '<S7>:1:20' else */
191 /* '<S7>:1:21' Fx=0; */
192 rtb_lim = 0.0;
193
194 /* '<S7>:1:22' Fy=0; */
195 rtb_Sum_idx_1 = 0.0;
196 }
197
198 /* '<S7>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */
199 VisionControl_B.Ao[((xoffset << 2) + i) << 1] = rtb_lim;
200
201 /* '<S7>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */
202 VisionControl_B.Ao[1 + (((xoffset << 2) + i) << 1)] = rtb_Sum_idx_1;
203 }
204 }
205
206 /* End of MATLAB Function: '<S3>/Embedded MATLAB Function' */
207
208 /* Gain: '<S4>/Filter Coefficient' incorporates:
209 * Constant: '<Root>/Referece position'
210 * DiscreteIntegrator: '<S4>/Filter'
211 * Gain: '<S4>/Derivative Gain'
212 * Gain: '<S4>/Setpoint Weighting (Derivative)'
213 * Sum: '<S4>/Sum3'
214 * Sum: '<S4>/SumD'
215 */
216 rtb_FilterCoefficient_idx_0 = ((VisionControl_P.PIDController2DOF1_c *
217 VisionControl_P.Refereceposition_Value[0] - VisionControl_B.X) *
218 VisionControl_P.PIDController2DOF1_D - VisionControl_DW.Filter_DSTATE[0]) *
219 VisionControl_P.PIDController2DOF1_N;
220 rtb_FilterCoefficient_idx_1 = ((VisionControl_P.PIDController2DOF1_c *
221 VisionControl_P.Refereceposition_Value[1] - VisionControl_B.Y) *
222 VisionControl_P.PIDController2DOF1_D - VisionControl_DW.Filter_DSTATE[1]) *
223 VisionControl_P.PIDController2DOF1_N;
224
225 /* Sum: '<S4>/Sum' incorporates:
226 * Constant: '<Root>/Referece position'
227 * DiscreteIntegrator: '<S4>/Integrator'
228 * Gain: '<S4>/Proportional Gain'
229 * Gain: '<S4>/Setpoint Weighting (Proportional)'
230 * Sum: '<S4>/Sum1'
231 */
232 rtb_Sum_idx_0 = ((VisionControl_P.PIDController2DOF1_b *
233 VisionControl_P.Refereceposition_Value[0] -
234 VisionControl_B.X) * VisionControl_P.PIDController2DOF1_P +
235 VisionControl_DW.Integrator_DSTATE[0]) +
236 rtb_FilterCoefficient_idx_0;
237 rtb_Sum_idx_1 = ((VisionControl_P.PIDController2DOF1_b *
238 VisionControl_P.Refereceposition_Value[1] -
239 VisionControl_B.Y) * VisionControl_P.PIDController2DOF1_P +
240 VisionControl_DW.Integrator_DSTATE[1]) +
241 rtb_FilterCoefficient_idx_1;
242
243 /* Saturate: '<S4>/Saturate' */
244 if (rtb_Sum_idx_0 > VisionControl_P.PIDController2DOF1_UpperSaturat) {
245 VisionControl_B.Saturate[0] =
246 VisionControl_P.PIDController2DOF1_UpperSaturat;
247 } else if (rtb_Sum_idx_0 < VisionControl_P.PIDController2DOF1_LowerSaturat) {
248 VisionControl_B.Saturate[0] =
249 VisionControl_P.PIDController2DOF1_LowerSaturat;
250 } else {
251 VisionControl_B.Saturate[0] = rtb_Sum_idx_0;
252 }
253
254 if (rtb_Sum_idx_1 > VisionControl_P.PIDController2DOF1_UpperSaturat) {
255 VisionControl_B.Saturate[1] =
256 VisionControl_P.PIDController2DOF1_UpperSaturat;
257 } else if (rtb_Sum_idx_1 < VisionControl_P.PIDController2DOF1_LowerSaturat) {
258 VisionControl_B.Saturate[1] =
259 VisionControl_P.PIDController2DOF1_LowerSaturat;
260 } else {
261 VisionControl_B.Saturate[1] = rtb_Sum_idx_1;
262 }
263
264 /* End of Saturate: '<S4>/Saturate' */
265
266 /* S-Function (cvxfasd): '<S3>/MagMan linearization' */
267 cvxfasd_Outputs_wrapper(&VisionControl_B.Ao[0], &VisionControl_B.Saturate[0],
268 &VisionControl_B.Saturate[1], &VisionControl_B.MagManlinearization_o1[0],
269 &VisionControl_B.MagManlinearization_o2 );
270
271 /* MATLAB Function: '<Root>/PWM calculation' */
272 /* MATLAB Function 'PWM calculation': '<S5>:1' */
273 /* conversion from scaling factor to duty cycle of PWM */
274 /* '<S5>:1:3' x = max(0, min(1,x) ); */
275 /* limits */
276 /* '<S5>:1:4' i = 0.18515*x + 0.069587*x.^3 + 0.19285*x.^(1/3); */
277 /* ratio to current conversion */
278 /* '<S5>:1:5' pw = i/0.455/2; */
279 /* '<S5>:1:6' pw(pw>0.05) = pw(pw>0.05) + 0.5; */
280 /* '<S5>:1:7' pw(pw<0.5) = 0; */
281 for (xoffset = 0; xoffset < 16; xoffset++) {
282 if ((1.0 <= VisionControl_B.MagManlinearization_o1[xoffset]) || rtIsNaN
283 (VisionControl_B.MagManlinearization_o1[xoffset])) {
284 rtb_lim = 1.0;
285 } else {
286 rtb_lim = VisionControl_B.MagManlinearization_o1[xoffset];
287 }
288
289 if ((0.0 >= rtb_lim) || rtIsNaN(rtb_lim)) {
290 VisionControl_B.pw[xoffset] = 0.0;
291 } else {
292 VisionControl_B.pw[xoffset] = rtb_lim;
293 }
294
295 VisionControl_B.pw[xoffset] = ((0.18515 * VisionControl_B.pw[xoffset] +
296 0.069587 * rt_powd_snf(VisionControl_B.pw[xoffset], 3.0)) + 0.19285 *
297 rt_powd_snf(VisionControl_B.pw[xoffset], 0.33333333333333331)) / 0.455 /
298 2.0;
299 if (VisionControl_B.pw[xoffset] > 0.05) {
300 VisionControl_B.pw[xoffset] += 0.5;
301 }
302
303 if (VisionControl_B.pw[xoffset] < 0.5) {
304 VisionControl_B.pw[xoffset] = 0.0;
305 }
306 }
307
308 /* '<S5>:1:8' pw=pw'; */
309 for (i = 0; i < 4; i++) {
310 VisionControl_B.y[i << 2] = VisionControl_B.pw[i];
311 VisionControl_B.y[1 + (i << 2)] = VisionControl_B.pw[i + 4];
312 VisionControl_B.y[2 + (i << 2)] = VisionControl_B.pw[i + 8];
313 VisionControl_B.y[3 + (i << 2)] = VisionControl_B.pw[i + 12];
314 }
315
316 for (i = 0; i < 4; i++) {
317 VisionControl_B.pw[i << 2] = VisionControl_B.y[i << 2];
318 VisionControl_B.pw[1 + (i << 2)] = VisionControl_B.y[(i << 2) + 1];
319 VisionControl_B.pw[2 + (i << 2)] = VisionControl_B.y[(i << 2) + 2];
320 VisionControl_B.pw[3 + (i << 2)] = VisionControl_B.y[(i << 2) + 3];
321 }
322
323 /* End of MATLAB Function: '<Root>/PWM calculation' */
324
325 /* MATLAB Function: '<Root>/Current' */
326 /* MATLAB Function 'Current': '<S2>:1' */
327 /* '<S2>:1:2' i = max(0, pw-0.5).*(0.455*2); */
328 for (xoffset = 0; xoffset < 16; xoffset++) {
329 if ((0.0 >= VisionControl_B.pw[xoffset] - 0.5) || rtIsNaN
330 (VisionControl_B.pw[xoffset] - 0.5)) {
331 VisionControl_B.y[xoffset] = 0.0;
332 } else {
333 VisionControl_B.y[xoffset] = VisionControl_B.pw[xoffset] - 0.5;
334 }
335 }
336
337 /* '<S2>:1:3' i=i'; */
338 for (i = 0; i < 4; i++) {
339 VisionControl_B.i[i << 2] = VisionControl_B.y[i] * 0.91;
340 VisionControl_B.i[1 + (i << 2)] = VisionControl_B.y[i + 4] * 0.91;
341 VisionControl_B.i[2 + (i << 2)] = VisionControl_B.y[i + 8] * 0.91;
342 VisionControl_B.i[3 + (i << 2)] = VisionControl_B.y[i + 12] * 0.91;
343 }
344
345 /* '<S2>:1:4' lim = 0; */
346 rtb_lim = 0.0;
347
348 /* '<S2>:1:5' if sum(sum(abs(i))) < 4 */
349 memcpy(&VisionControl_B.y[0], &VisionControl_B.i[0], sizeof(real_T) << 4U);
350 for (i = 0; i < 4; i++) {
351 xoffset = i << 2;
352 VisionControl_B.y_m[i] = ((VisionControl_B.y[xoffset + 1] +
353 VisionControl_B.y[xoffset]) + VisionControl_B.y[xoffset + 2]) +
354 VisionControl_B.y[xoffset + 3];
355 }
356
357 if (((VisionControl_B.y_m[0] + VisionControl_B.y_m[1]) + VisionControl_B.y_m[2])
358 + VisionControl_B.y_m[3] < 4.0) {
359 /* '<S2>:1:6' lim = 1; */
360 rtb_lim = 1.0;
361 }
362
363 /* End of MATLAB Function: '<Root>/Current' */
364
365 /* Switch: '<Root>/Safety switch' */
366 for (i = 0; i < 16; i++) {
367 if (rtb_lim != 0.0) {
368 VisionControl_B.Safetyswitch[i] = VisionControl_B.pw[i];
369 } else {
370 VisionControl_B.Safetyswitch[i] = 0.0;
371 }
372 }
373
374 /* End of Switch: '<Root>/Safety switch' */
375
376 /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */
377 sfcn_setPlatform_Outputs_wrapper(&VisionControl_B.Safetyswitch[0],
378 &VisionControl_DW.MagManCoils_DSTATE);
379
380 /* S-Function "sfcn_posread_shm_wrapper" Block: <S1>/Shared memory camera position */
381 sfcn_posread_shm_Update_wrapper
382 ( &VisionControl_B.Sharedmemorycameraposition_o1,
383 &VisionControl_B.Sharedmemorycameraposition_o2,
384 &VisionControl_DW.Sharedmemorycameraposition_DSTA);
385
386 /* Update for DiscreteIntegrator: '<S4>/Integrator' incorporates:
387 * Constant: '<Root>/Referece position'
388 * Gain: '<S4>/Integral Gain'
389 * Gain: '<S4>/Kb'
390 * Sum: '<S4>/Sum2'
391 * Sum: '<S4>/SumI1'
392 * Sum: '<S4>/SumI2'
393 */
394 VisionControl_DW.Integrator_DSTATE[0] +=
395 ((VisionControl_P.Refereceposition_Value[0] - VisionControl_B.X) *
396 VisionControl_P.PIDController2DOF1_I + (VisionControl_B.Saturate[0] -
397 rtb_Sum_idx_0) * VisionControl_P.PIDController2DOF1_Kb) *
398 VisionControl_P.Integrator_gainval;
399
400 /* Update for DiscreteIntegrator: '<S4>/Filter' */
401 VisionControl_DW.Filter_DSTATE[0] += VisionControl_P.Filter_gainval *
402 rtb_FilterCoefficient_idx_0;
403
404 /* Update for DiscreteIntegrator: '<S4>/Integrator' incorporates:
405 * Constant: '<Root>/Referece position'
406 * Gain: '<S4>/Integral Gain'
407 * Gain: '<S4>/Kb'
408 * Sum: '<S4>/Sum2'
409 * Sum: '<S4>/SumI1'
410 * Sum: '<S4>/SumI2'
411 */
412 VisionControl_DW.Integrator_DSTATE[1] +=
413 ((VisionControl_P.Refereceposition_Value[1] - VisionControl_B.Y) *
414 VisionControl_P.PIDController2DOF1_I + (VisionControl_B.Saturate[1] -
415 rtb_Sum_idx_1) * VisionControl_P.PIDController2DOF1_Kb) *
416 VisionControl_P.Integrator_gainval;
417
418 /* Update for DiscreteIntegrator: '<S4>/Filter' */
419 VisionControl_DW.Filter_DSTATE[1] += VisionControl_P.Filter_gainval *
420 rtb_FilterCoefficient_idx_1;
421
422 /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */
423 sfcn_setPlatform_Update_wrapper(&VisionControl_B.Safetyswitch[0],
424 &VisionControl_DW.MagManCoils_DSTATE);
425
426 /* External mode */
427 rtExtModeUploadCheckTrigger(1);
428
429 { /* Sample time: [0.02s, 0.0s] */
430 rtExtModeUpload(0, VisionControl_M->Timing.taskTime0);
431 }
432
433 /* signal main to stop simulation */
434 { /* Sample time: [0.02s, 0.0s] */
435 if ((rtmGetTFinal(VisionControl_M)!=-1) &&
436 !((rtmGetTFinal(VisionControl_M)-VisionControl_M->Timing.taskTime0) >
437 VisionControl_M->Timing.taskTime0 * (DBL_EPSILON))) {
438 rtmSetErrorStatus(VisionControl_M, "Simulation finished");
439 }
440
441 if (rtmGetStopRequested(VisionControl_M)) {
442 rtmSetErrorStatus(VisionControl_M, "Simulation finished");
443 }
444 }
445
446 /* Update absolute time for base rate */
447 /* The "clockTick0" counts the number of times the code of this task has
448 * been executed. The absolute time is the multiplication of "clockTick0"
449 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
450 * overflow during the application lifespan selected.
451 * Timer of this task consists of two 32 bit unsigned integers.
452 * The two integers represent the low bits Timing.clockTick0 and the high bits
453 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
454 */
455 if (!(++VisionControl_M->Timing.clockTick0)) {
456 ++VisionControl_M->Timing.clockTickH0;
457 }
458
459 VisionControl_M->Timing.taskTime0 = VisionControl_M->Timing.clockTick0 *
460 VisionControl_M->Timing.stepSize0 + VisionControl_M->Timing.clockTickH0 *
461 VisionControl_M->Timing.stepSize0 * 4294967296.0;
462}
463
464/* Model initialize function */
465void VisionControl_initialize(void)
466{
467 /* Registration code */
468
469 /* initialize non-finites */
470 rt_InitInfAndNaN(sizeof(real_T));
471
472 /* initialize real-time model */
473 (void) memset((void *)VisionControl_M, 0,
474 sizeof(RT_MODEL_VisionControl_T));
475 rtmSetTFinal(VisionControl_M, -1);
476 VisionControl_M->Timing.stepSize0 = 0.02;
477
478 /* External mode info */
479 VisionControl_M->Sizes.checksums[0] = (388049967U);
480 VisionControl_M->Sizes.checksums[1] = (3898533790U);
481 VisionControl_M->Sizes.checksums[2] = (952379559U);
482 VisionControl_M->Sizes.checksums[3] = (2438367565U);
483
484 {
485 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
486 static RTWExtModeInfo rt_ExtModeInfo;
487 static const sysRanDType *systemRan[5];
488 VisionControl_M->extModeInfo = (&rt_ExtModeInfo);
489 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
490 systemRan[0] = &rtAlwaysEnabled;
491 systemRan[1] = &rtAlwaysEnabled;
492 systemRan[2] = &rtAlwaysEnabled;
493 systemRan[3] = &rtAlwaysEnabled;
494 systemRan[4] = &rtAlwaysEnabled;
495 rteiSetModelMappingInfoPtr(VisionControl_M->extModeInfo,
496 &VisionControl_M->SpecialInfo.mappingInfo);
497 rteiSetChecksumsPtr(VisionControl_M->extModeInfo,
498 VisionControl_M->Sizes.checksums);
499 rteiSetTPtr(VisionControl_M->extModeInfo, rtmGetTPtr(VisionControl_M));
500 }
501
502 /* block I/O */
503 (void) memset(((void *) &VisionControl_B), 0,
504 sizeof(B_VisionControl_T));
505
506 /* states (dwork) */
507 (void) memset((void *)&VisionControl_DW, 0,
508 sizeof(DW_VisionControl_T));
509
510 /* data type transition information */
511 {
512 static DataTypeTransInfo dtInfo;
513 (void) memset((char_T *) &dtInfo, 0,
514 sizeof(dtInfo));
515 VisionControl_M->SpecialInfo.mappingInfo = (&dtInfo);
516 dtInfo.numDataTypes = 14;
517 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
518 dtInfo.dataTypeNames = &rtDataTypeNames[0];
519
520 /* Block I/O transition table */
521 dtInfo.BTransTable = &rtBTransTable;
522
523 /* Parameters transition table */
524 dtInfo.PTransTable = &rtPTransTable;
525 }
526
527 /* S-Function Block: <S1>/Shared memory camera position */
528 {
529 real_T initVector[1] = { 0 };
530
531 {
532 int_T i1;
533 for (i1=0; i1 < 1; i1++) {
534 VisionControl_DW.Sharedmemorycameraposition_DSTA = initVector[0];
535 }
536 }
537 }
538
539 /* InitializeConditions for DiscreteIntegrator: '<S4>/Integrator' */
540 VisionControl_DW.Integrator_DSTATE[0] = VisionControl_P.Integrator_IC;
541
542 /* InitializeConditions for DiscreteIntegrator: '<S4>/Filter' */
543 VisionControl_DW.Filter_DSTATE[0] = VisionControl_P.Filter_IC;
544
545 /* InitializeConditions for DiscreteIntegrator: '<S4>/Integrator' */
546 VisionControl_DW.Integrator_DSTATE[1] = VisionControl_P.Integrator_IC;
547
548 /* InitializeConditions for DiscreteIntegrator: '<S4>/Filter' */
549 VisionControl_DW.Filter_DSTATE[1] = VisionControl_P.Filter_IC;
550
551 /* S-Function Block: <Root>/MagMan Coils */
552 {
553 real_T initVector[1] = { 0 };
554
555 {
556 int_T i1;
557 for (i1=0; i1 < 1; i1++) {
558 VisionControl_DW.MagManCoils_DSTATE = initVector[0];
559 }
560 }
561 }
562}
563
564/* Model terminate function */
565void VisionControl_terminate(void)
566{
567 /* (no terminate code required) */
568}
569