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) */ |
25 | B_VisionControl_T VisionControl_B; |
26 | |
27 | /* Block states (auto storage) */ |
28 | DW_VisionControl_T VisionControl_DW; |
29 | |
30 | /* Real-time model */ |
31 | RT_MODEL_VisionControl_T VisionControl_M_; |
32 | RT_MODEL_VisionControl_T *const VisionControl_M = &VisionControl_M_; |
33 | real_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 */ |
80 | void 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 */ |
465 | void 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 */ |
565 | void VisionControl_terminate(void) |
566 | { |
567 | /* (no terminate code required) */ |
568 | } |
569 | |