1 | /* |
2 | * FoilControlRaspberrySentinel.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 "FoilControlRaspberrySentinel". |
8 | * |
9 | * Model version : 1.24 |
10 | * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 |
11 | * C source code generated on : Wed Nov 23 14:06:32 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 "FoilControlRaspberrySentinel.h" |
21 | #include "FoilControlRaspberrySentinel_private.h" |
22 | #include "FoilControlRaspberrySentinel_dt.h" |
23 | #define FoilControlRaspberryS_PinNumber (17.0) |
24 | |
25 | /* Block signals (auto storage) */ |
26 | B_FoilControlRaspberrySentine_T FoilControlRaspberrySentinel_B; |
27 | |
28 | /* Block states (auto storage) */ |
29 | DW_FoilControlRaspberrySentin_T FoilControlRaspberrySentinel_DW; |
30 | |
31 | /* Real-time model */ |
32 | RT_MODEL_FoilControlRaspberry_T FoilControlRaspberrySentinel_M_; |
33 | RT_MODEL_FoilControlRaspberry_T *const FoilControlRaspberrySentinel_M = |
34 | &FoilControlRaspberrySentinel_M_; |
35 | real_T rt_powd_snf(real_T u0, real_T u1) |
36 | { |
37 | real_T y; |
38 | real_T tmp; |
39 | real_T tmp_0; |
40 | if (rtIsNaN(u0) || rtIsNaN(u1)) { |
41 | y = (rtNaN); |
42 | } else { |
43 | tmp = fabs(u0); |
44 | tmp_0 = fabs(u1); |
45 | if (rtIsInf(u1)) { |
46 | if (tmp == 1.0) { |
47 | y = (rtNaN); |
48 | } else if (tmp > 1.0) { |
49 | if (u1 > 0.0) { |
50 | y = (rtInf); |
51 | } else { |
52 | y = 0.0; |
53 | } |
54 | } else if (u1 > 0.0) { |
55 | y = 0.0; |
56 | } else { |
57 | y = (rtInf); |
58 | } |
59 | } else if (tmp_0 == 0.0) { |
60 | y = 1.0; |
61 | } else if (tmp_0 == 1.0) { |
62 | if (u1 > 0.0) { |
63 | y = u0; |
64 | } else { |
65 | y = 1.0 / u0; |
66 | } |
67 | } else if (u1 == 2.0) { |
68 | y = u0 * u0; |
69 | } else if ((u1 == 0.5) && (u0 >= 0.0)) { |
70 | y = sqrt(u0); |
71 | } else if ((u0 < 0.0) && (u1 > floor(u1))) { |
72 | y = (rtNaN); |
73 | } else { |
74 | y = pow(u0, u1); |
75 | } |
76 | } |
77 | |
78 | return y; |
79 | } |
80 | |
81 | /* Model step function */ |
82 | void FoilControlRaspberrySentinel_step(void) |
83 | { |
84 | int32_T i; |
85 | int32_T xoffset; |
86 | uint8_T rtb_FixPtSum1; |
87 | real_T rtb_FilterCoefficient_idx_0; |
88 | real_T rtb_FilterCoefficient_idx_1; |
89 | real_T rtb_Sum_idx_0; |
90 | real_T rtb_Sum_idx_1; |
91 | |
92 | /* S-Function (sfcn_getFoilCheck): '<Root>/MagMan Foil with error' */ |
93 | sfcn_getFoilCheck_Outputs_wrapper |
94 | ( &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1, |
95 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2, |
96 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o3, |
97 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o4, |
98 | &FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE); |
99 | |
100 | /* MATLAB Function: '<Root>/ht1' incorporates: |
101 | * Constant: '<Root>/Sentinel Calibration matrix' |
102 | */ |
103 | /* MATLAB Function 'ht1': '<S6>:1' */ |
104 | /* out=km2*[x;y;x^2;y^2;x*y;1]; */ |
105 | /* '<S6>:1:4' xa = [x,y]; */ |
106 | /* '<S6>:1:5' H = km4(:); */ |
107 | /* '<S6>:1:6' xa = [xa,ones(size(xa,1),1)]; */ |
108 | /* homogeneous coordinates */ |
109 | /* '<S6>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */ |
110 | /* '<S6>:1:9' X=out(1); */ |
111 | FoilControlRaspberrySentinel_B.X = |
112 | ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 * |
113 | FoilControlRaspberrySentinel_P.km4pi[0] + |
114 | FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 * |
115 | FoilControlRaspberrySentinel_P.km4pi[1]) + |
116 | FoilControlRaspberrySentinel_P.km4pi[2]) / |
117 | ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 * |
118 | FoilControlRaspberrySentinel_P.km4pi[6] + |
119 | FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 * |
120 | FoilControlRaspberrySentinel_P.km4pi[7]) + |
121 | FoilControlRaspberrySentinel_P.km4pi[8]); |
122 | |
123 | /* '<S6>:1:10' Y=out(2); */ |
124 | FoilControlRaspberrySentinel_B.Y = |
125 | ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 * |
126 | FoilControlRaspberrySentinel_P.km4pi[3] + |
127 | FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 * |
128 | FoilControlRaspberrySentinel_P.km4pi[4]) + |
129 | FoilControlRaspberrySentinel_P.km4pi[5]) / |
130 | ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1 * |
131 | FoilControlRaspberrySentinel_P.km4pi[6] + |
132 | FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 * |
133 | FoilControlRaspberrySentinel_P.km4pi[7]) + |
134 | FoilControlRaspberrySentinel_P.km4pi[8]); |
135 | |
136 | /* MATLAB Function: '<S7>/Embedded MATLAB Function' */ |
137 | /* MATLAB Function 'linearization/Embedded MATLAB Function': '<S12>:1' */ |
138 | /* '<S12>:1:2' Ao=zeros(2,16); */ |
139 | memset(&FoilControlRaspberrySentinel_B.Ao[0], 0, sizeof(real_T) << 5U); |
140 | |
141 | /* '<S12>:1:4' for iy=1:4 */ |
142 | for (xoffset = 0; xoffset < 4; xoffset++) { |
143 | /* '<S12>:1:5' for ix=1:4 */ |
144 | for (i = 0; i < 4; i++) { |
145 | /* '<S12>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */ |
146 | /* '<S12>:1:14' d=dist(x,y, ix, iy); */ |
147 | /* '<S12>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
148 | FoilControlRaspberrySentinel_B.lim = FoilControlRaspberrySentinel_B.X - |
149 | (1.0 + (real_T)i); |
150 | rtb_Sum_idx_1 = FoilControlRaspberrySentinel_B.Y - (1.0 + (real_T)xoffset); |
151 | FoilControlRaspberrySentinel_B.lim = sqrt |
152 | (FoilControlRaspberrySentinel_B.lim * FoilControlRaspberrySentinel_B.lim |
153 | + rtb_Sum_idx_1 * rtb_Sum_idx_1); |
154 | |
155 | /* '<S12>:1:15' if d~=0 */ |
156 | if (FoilControlRaspberrySentinel_B.lim != 0.0) { |
157 | /* '<S12>:1:16' F=gForce(d); */ |
158 | /* '<S12>:1:43' a = 622.06; */ |
159 | /* '<S12>:1:44' b = 9.5032; */ |
160 | /* '<S12>:1:45' c = 3.1884; */ |
161 | /* '<S12>:1:46' F = x.*a./(b.*x.^2+ c).^3; */ |
162 | FoilControlRaspberrySentinel_B.lim = FoilControlRaspberrySentinel_B.lim * |
163 | 622.06 / rt_powd_snf(FoilControlRaspberrySentinel_B.lim * |
164 | FoilControlRaspberrySentinel_B.lim * 9.5032 + 3.1884, 3.0); |
165 | |
166 | /* '<S12>:1:17' Fv=F*vector(x,y,ix,iy); */ |
167 | /* '<S12>:1:32' vx=ix-x; */ |
168 | rtb_Sum_idx_1 = (1.0 + (real_T)i) - FoilControlRaspberrySentinel_B.X; |
169 | |
170 | /* '<S12>:1:33' vy=iy-y; */ |
171 | rtb_FilterCoefficient_idx_0 = (1.0 + (real_T)xoffset) - |
172 | FoilControlRaspberrySentinel_B.Y; |
173 | |
174 | /* '<S12>:1:35' if(vx==0 && vy ==0) */ |
175 | if ((rtb_Sum_idx_1 == 0.0) && (rtb_FilterCoefficient_idx_0 == 0.0)) { |
176 | /* '<S12>:1:36' v=[0 0]; */ |
177 | rtb_Sum_idx_0 = 0.0; |
178 | rtb_Sum_idx_1 = 0.0; |
179 | } else { |
180 | /* '<S12>:1:37' else */ |
181 | /* '<S12>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */ |
182 | /* '<S12>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
183 | rtb_FilterCoefficient_idx_1 = sqrt(rtb_Sum_idx_1 * rtb_Sum_idx_1 + |
184 | rtb_FilterCoefficient_idx_0 * rtb_FilterCoefficient_idx_0); |
185 | rtb_Sum_idx_0 = rtb_Sum_idx_1 / rtb_FilterCoefficient_idx_1; |
186 | rtb_Sum_idx_1 = rtb_FilterCoefficient_idx_0 / |
187 | rtb_FilterCoefficient_idx_1; |
188 | } |
189 | |
190 | rtb_Sum_idx_0 *= FoilControlRaspberrySentinel_B.lim; |
191 | rtb_Sum_idx_1 *= FoilControlRaspberrySentinel_B.lim; |
192 | |
193 | /* '<S12>:1:18' Fx=Fv(1); */ |
194 | FoilControlRaspberrySentinel_B.lim = rtb_Sum_idx_0; |
195 | |
196 | /* '<S12>:1:19' Fy=Fv(2); */ |
197 | } else { |
198 | /* '<S12>:1:20' else */ |
199 | /* '<S12>:1:21' Fx=0; */ |
200 | FoilControlRaspberrySentinel_B.lim = 0.0; |
201 | |
202 | /* '<S12>:1:22' Fy=0; */ |
203 | rtb_Sum_idx_1 = 0.0; |
204 | } |
205 | |
206 | /* '<S12>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */ |
207 | FoilControlRaspberrySentinel_B.Ao[((xoffset << 2) + i) << 1] = |
208 | FoilControlRaspberrySentinel_B.lim; |
209 | |
210 | /* '<S12>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */ |
211 | FoilControlRaspberrySentinel_B.Ao[1 + (((xoffset << 2) + i) << 1)] = |
212 | rtb_Sum_idx_1; |
213 | } |
214 | } |
215 | |
216 | /* End of MATLAB Function: '<S7>/Embedded MATLAB Function' */ |
217 | |
218 | /* Gain: '<S5>/Filter Coefficient' incorporates: |
219 | * Constant: '<Root>/Constant' |
220 | * Constant: '<Root>/Constant1' |
221 | * DiscreteIntegrator: '<S5>/Filter' |
222 | * Gain: '<S5>/Derivative Gain' |
223 | * Gain: '<S5>/Setpoint Weighting (Derivative)' |
224 | * Sum: '<S5>/Sum3' |
225 | * Sum: '<S5>/SumD' |
226 | */ |
227 | rtb_FilterCoefficient_idx_0 = |
228 | ((FoilControlRaspberrySentinel_P.PIDController2DOF1_c * |
229 | FoilControlRaspberrySentinel_P.Constant_Value - |
230 | FoilControlRaspberrySentinel_B.X) * |
231 | FoilControlRaspberrySentinel_P.PIDController2DOF1_D - |
232 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[0]) * |
233 | FoilControlRaspberrySentinel_P.PIDController2DOF1_N; |
234 | rtb_FilterCoefficient_idx_1 = |
235 | ((FoilControlRaspberrySentinel_P.PIDController2DOF1_c * |
236 | FoilControlRaspberrySentinel_P.Constant1_Value - |
237 | FoilControlRaspberrySentinel_B.Y) * |
238 | FoilControlRaspberrySentinel_P.PIDController2DOF1_D - |
239 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[1]) * |
240 | FoilControlRaspberrySentinel_P.PIDController2DOF1_N; |
241 | |
242 | /* Sum: '<S5>/Sum' incorporates: |
243 | * Constant: '<Root>/Constant' |
244 | * Constant: '<Root>/Constant1' |
245 | * DiscreteIntegrator: '<S5>/Integrator' |
246 | * Gain: '<S5>/Proportional Gain' |
247 | * Gain: '<S5>/Setpoint Weighting (Proportional)' |
248 | * Sum: '<S5>/Sum1' |
249 | */ |
250 | rtb_Sum_idx_0 = ((FoilControlRaspberrySentinel_P.PIDController2DOF1_b * |
251 | FoilControlRaspberrySentinel_P.Constant_Value - |
252 | FoilControlRaspberrySentinel_B.X) * |
253 | FoilControlRaspberrySentinel_P.PIDController2DOF1_P + |
254 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0]) + |
255 | rtb_FilterCoefficient_idx_0; |
256 | rtb_Sum_idx_1 = ((FoilControlRaspberrySentinel_P.PIDController2DOF1_b * |
257 | FoilControlRaspberrySentinel_P.Constant1_Value - |
258 | FoilControlRaspberrySentinel_B.Y) * |
259 | FoilControlRaspberrySentinel_P.PIDController2DOF1_P + |
260 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1]) + |
261 | rtb_FilterCoefficient_idx_1; |
262 | |
263 | /* Saturate: '<S5>/Saturate' */ |
264 | if (rtb_Sum_idx_0 > |
265 | FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat) { |
266 | FoilControlRaspberrySentinel_B.Saturate[0] = |
267 | FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat; |
268 | } else if (rtb_Sum_idx_0 < |
269 | FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat) { |
270 | FoilControlRaspberrySentinel_B.Saturate[0] = |
271 | FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat; |
272 | } else { |
273 | FoilControlRaspberrySentinel_B.Saturate[0] = rtb_Sum_idx_0; |
274 | } |
275 | |
276 | if (rtb_Sum_idx_1 > |
277 | FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat) { |
278 | FoilControlRaspberrySentinel_B.Saturate[1] = |
279 | FoilControlRaspberrySentinel_P.PIDController2DOF1_UpperSaturat; |
280 | } else if (rtb_Sum_idx_1 < |
281 | FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat) { |
282 | FoilControlRaspberrySentinel_B.Saturate[1] = |
283 | FoilControlRaspberrySentinel_P.PIDController2DOF1_LowerSaturat; |
284 | } else { |
285 | FoilControlRaspberrySentinel_B.Saturate[1] = rtb_Sum_idx_1; |
286 | } |
287 | |
288 | /* End of Saturate: '<S5>/Saturate' */ |
289 | |
290 | /* S-Function (cvxfasd): '<S7>/MagMan linearization' */ |
291 | cvxfasd_Outputs_wrapper(&FoilControlRaspberrySentinel_B.Ao[0], |
292 | &FoilControlRaspberrySentinel_B.Saturate[0], |
293 | &FoilControlRaspberrySentinel_B.Saturate[1], |
294 | &FoilControlRaspberrySentinel_B.MagManlinearization_o1[0], |
295 | &FoilControlRaspberrySentinel_B.MagManlinearization_o2 ); |
296 | |
297 | /* MATLAB Function: '<Root>/pwm' */ |
298 | /* MATLAB Function 'pwm': '<S9>:1' */ |
299 | /* conversion from scaling factor to duty cycle of PWM */ |
300 | /* '<S9>:1:3' x = max(0, min(1,x) ); */ |
301 | /* limits */ |
302 | /* '<S9>:1:4' i = 0.18515*x + 0.069587*x.^3 + 0.19285*x.^(1/3); */ |
303 | /* ratio to current conversion */ |
304 | /* '<S9>:1:5' pw = i/0.455/2; */ |
305 | /* '<S9>:1:6' pw(pw>0.05) = pw(pw>0.05) + 0.5; */ |
306 | /* '<S9>:1:7' pw(pw<0.5) = 0; */ |
307 | for (xoffset = 0; xoffset < 16; xoffset++) { |
308 | if ((1.0 <= FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset]) || |
309 | rtIsNaN(FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset])) |
310 | { |
311 | FoilControlRaspberrySentinel_B.lim = 1.0; |
312 | } else { |
313 | FoilControlRaspberrySentinel_B.lim = |
314 | FoilControlRaspberrySentinel_B.MagManlinearization_o1[xoffset]; |
315 | } |
316 | |
317 | if ((0.0 >= FoilControlRaspberrySentinel_B.lim) || rtIsNaN |
318 | (FoilControlRaspberrySentinel_B.lim)) { |
319 | FoilControlRaspberrySentinel_B.pw[xoffset] = 0.0; |
320 | } else { |
321 | FoilControlRaspberrySentinel_B.pw[xoffset] = |
322 | FoilControlRaspberrySentinel_B.lim; |
323 | } |
324 | |
325 | FoilControlRaspberrySentinel_B.pw[xoffset] = ((0.18515 * |
326 | FoilControlRaspberrySentinel_B.pw[xoffset] + 0.069587 * rt_powd_snf |
327 | (FoilControlRaspberrySentinel_B.pw[xoffset], 3.0)) + 0.19285 * rt_powd_snf |
328 | (FoilControlRaspberrySentinel_B.pw[xoffset], 0.33333333333333331)) / 0.455 |
329 | / 2.0; |
330 | if (FoilControlRaspberrySentinel_B.pw[xoffset] > 0.05) { |
331 | FoilControlRaspberrySentinel_B.pw[xoffset] += 0.5; |
332 | } |
333 | |
334 | if (FoilControlRaspberrySentinel_B.pw[xoffset] < 0.5) { |
335 | FoilControlRaspberrySentinel_B.pw[xoffset] = 0.0; |
336 | } |
337 | } |
338 | |
339 | /* '<S9>:1:8' pw=pw'; */ |
340 | for (i = 0; i < 4; i++) { |
341 | FoilControlRaspberrySentinel_B.y[i << 2] = |
342 | FoilControlRaspberrySentinel_B.pw[i]; |
343 | FoilControlRaspberrySentinel_B.y[1 + (i << 2)] = |
344 | FoilControlRaspberrySentinel_B.pw[i + 4]; |
345 | FoilControlRaspberrySentinel_B.y[2 + (i << 2)] = |
346 | FoilControlRaspberrySentinel_B.pw[i + 8]; |
347 | FoilControlRaspberrySentinel_B.y[3 + (i << 2)] = |
348 | FoilControlRaspberrySentinel_B.pw[i + 12]; |
349 | } |
350 | |
351 | for (i = 0; i < 4; i++) { |
352 | FoilControlRaspberrySentinel_B.pw[i << 2] = |
353 | FoilControlRaspberrySentinel_B.y[i << 2]; |
354 | FoilControlRaspberrySentinel_B.pw[1 + (i << 2)] = |
355 | FoilControlRaspberrySentinel_B.y[(i << 2) + 1]; |
356 | FoilControlRaspberrySentinel_B.pw[2 + (i << 2)] = |
357 | FoilControlRaspberrySentinel_B.y[(i << 2) + 2]; |
358 | FoilControlRaspberrySentinel_B.pw[3 + (i << 2)] = |
359 | FoilControlRaspberrySentinel_B.y[(i << 2) + 3]; |
360 | } |
361 | |
362 | /* End of MATLAB Function: '<Root>/pwm' */ |
363 | |
364 | /* Start for MATLABSystem: '<S4>/Digital Write' incorporates: |
365 | * Constant: '<S1>/Constant' |
366 | * Constant: '<S2>/Constant' |
367 | * Logic: '<Root>/Logical Operator' |
368 | * MATLABSystem: '<S4>/Digital Write' |
369 | * RelationalOperator: '<S1>/Compare' |
370 | * RelationalOperator: '<S2>/Compare' |
371 | * UnitDelay: '<S3>/Output' |
372 | */ |
373 | MW_gpioWrite((uint32_T)FoilControlRaspberryS_PinNumber, (uint8_T) |
374 | ((FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2 > |
375 | FoilControlRaspberrySentinel_P.Constant_Value_a) && |
376 | (FoilControlRaspberrySentinel_DW.Output_DSTATE > |
377 | FoilControlRaspberrySentinel_P.Constant_Value_p))); |
378 | |
379 | /* MATLAB Function: '<Root>/proud' */ |
380 | /* MATLAB Function 'proud': '<S8>:1' */ |
381 | /* '<S8>:1:2' i = (max(0, pw-0.5).*(0.455*2))'; */ |
382 | for (xoffset = 0; xoffset < 16; xoffset++) { |
383 | if ((0.0 >= FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5) || rtIsNaN |
384 | (FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5)) { |
385 | FoilControlRaspberrySentinel_B.y[xoffset] = 0.0; |
386 | } else { |
387 | FoilControlRaspberrySentinel_B.y[xoffset] = |
388 | FoilControlRaspberrySentinel_B.pw[xoffset] - 0.5; |
389 | } |
390 | } |
391 | |
392 | for (i = 0; i < 4; i++) { |
393 | FoilControlRaspberrySentinel_B.i[i << 2] = |
394 | FoilControlRaspberrySentinel_B.y[i] * 0.91; |
395 | FoilControlRaspberrySentinel_B.i[1 + (i << 2)] = |
396 | FoilControlRaspberrySentinel_B.y[i + 4] * 0.91; |
397 | FoilControlRaspberrySentinel_B.i[2 + (i << 2)] = |
398 | FoilControlRaspberrySentinel_B.y[i + 8] * 0.91; |
399 | FoilControlRaspberrySentinel_B.i[3 + (i << 2)] = |
400 | FoilControlRaspberrySentinel_B.y[i + 12] * 0.91; |
401 | } |
402 | |
403 | /* '<S8>:1:3' lim = 0; */ |
404 | FoilControlRaspberrySentinel_B.lim = 0.0; |
405 | |
406 | /* '<S8>:1:4' if sum(sum(abs(i))) < 4 */ |
407 | memcpy(&FoilControlRaspberrySentinel_B.y[0], |
408 | &FoilControlRaspberrySentinel_B.i[0], sizeof(real_T) << 4U); |
409 | for (i = 0; i < 4; i++) { |
410 | xoffset = i << 2; |
411 | FoilControlRaspberrySentinel_B.y_m[i] = |
412 | ((FoilControlRaspberrySentinel_B.y[xoffset + 1] + |
413 | FoilControlRaspberrySentinel_B.y[xoffset]) + |
414 | FoilControlRaspberrySentinel_B.y[xoffset + 2]) + |
415 | FoilControlRaspberrySentinel_B.y[xoffset + 3]; |
416 | } |
417 | |
418 | if (((FoilControlRaspberrySentinel_B.y_m[0] + |
419 | FoilControlRaspberrySentinel_B.y_m[1]) + |
420 | FoilControlRaspberrySentinel_B.y_m[2]) + |
421 | FoilControlRaspberrySentinel_B.y_m[3] < 4.0) { |
422 | /* '<S8>:1:5' lim = 1; */ |
423 | FoilControlRaspberrySentinel_B.lim = 1.0; |
424 | } |
425 | |
426 | /* End of MATLAB Function: '<Root>/proud' */ |
427 | |
428 | /* Switch: '<Root>/Switch1' incorporates: |
429 | * Constant: '<Root>/Constant2' |
430 | */ |
431 | if (FoilControlRaspberrySentinel_B.lim != 0.0) { |
432 | memcpy(&FoilControlRaspberrySentinel_B.Switch1[0], |
433 | &FoilControlRaspberrySentinel_B.pw[0], sizeof(real_T) << 4U); |
434 | } else { |
435 | memcpy(&FoilControlRaspberrySentinel_B.Switch1[0], |
436 | &FoilControlRaspberrySentinel_P.Constant2_Value[0], sizeof(real_T) << |
437 | 4U); |
438 | } |
439 | |
440 | /* End of Switch: '<Root>/Switch1' */ |
441 | |
442 | /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */ |
443 | sfcn_setPlatform_Outputs_wrapper(&FoilControlRaspberrySentinel_B.Switch1[0], |
444 | &FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE); |
445 | |
446 | /* Sum: '<S10>/FixPt Sum1' incorporates: |
447 | * Constant: '<S10>/FixPt Constant' |
448 | * UnitDelay: '<S3>/Output' |
449 | */ |
450 | rtb_FixPtSum1 = (uint8_T)((uint32_T) |
451 | FoilControlRaspberrySentinel_DW.Output_DSTATE + |
452 | FoilControlRaspberrySentinel_P.FixPtConstant_Value); |
453 | |
454 | /* S-Function "sfcn_getFoilCheck_wrapper" Block: <Root>/MagMan Foil with error */ |
455 | sfcn_getFoilCheck_Update_wrapper |
456 | ( &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o1, |
457 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o2, |
458 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o3, |
459 | &FoilControlRaspberrySentinel_B.MagManFoilwitherror_o4, |
460 | &FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE); |
461 | |
462 | /* Update for DiscreteIntegrator: '<S5>/Integrator' incorporates: |
463 | * Constant: '<Root>/Constant' |
464 | * Gain: '<S5>/Integral Gain' |
465 | * Gain: '<S5>/Kb' |
466 | * Sum: '<S5>/Sum2' |
467 | * Sum: '<S5>/SumI1' |
468 | * Sum: '<S5>/SumI2' |
469 | */ |
470 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0] += |
471 | ((FoilControlRaspberrySentinel_P.Constant_Value - |
472 | FoilControlRaspberrySentinel_B.X) * |
473 | FoilControlRaspberrySentinel_P.PIDController2DOF1_I + |
474 | (FoilControlRaspberrySentinel_B.Saturate[0] - rtb_Sum_idx_0) * |
475 | FoilControlRaspberrySentinel_P.PIDController2DOF1_Kb) * |
476 | FoilControlRaspberrySentinel_P.Integrator_gainval; |
477 | |
478 | /* Update for DiscreteIntegrator: '<S5>/Filter' */ |
479 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[0] += |
480 | FoilControlRaspberrySentinel_P.Filter_gainval * rtb_FilterCoefficient_idx_0; |
481 | |
482 | /* Update for DiscreteIntegrator: '<S5>/Integrator' incorporates: |
483 | * Constant: '<Root>/Constant1' |
484 | * Gain: '<S5>/Integral Gain' |
485 | * Gain: '<S5>/Kb' |
486 | * Sum: '<S5>/Sum2' |
487 | * Sum: '<S5>/SumI1' |
488 | * Sum: '<S5>/SumI2' |
489 | */ |
490 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1] += |
491 | ((FoilControlRaspberrySentinel_P.Constant1_Value - |
492 | FoilControlRaspberrySentinel_B.Y) * |
493 | FoilControlRaspberrySentinel_P.PIDController2DOF1_I + |
494 | (FoilControlRaspberrySentinel_B.Saturate[1] - rtb_Sum_idx_1) * |
495 | FoilControlRaspberrySentinel_P.PIDController2DOF1_Kb) * |
496 | FoilControlRaspberrySentinel_P.Integrator_gainval; |
497 | |
498 | /* Update for DiscreteIntegrator: '<S5>/Filter' */ |
499 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[1] += |
500 | FoilControlRaspberrySentinel_P.Filter_gainval * rtb_FilterCoefficient_idx_1; |
501 | |
502 | /* Switch: '<S11>/FixPt Switch' */ |
503 | if (rtb_FixPtSum1 > FoilControlRaspberrySentinel_P.CounterLimited_uplimit) { |
504 | /* Update for UnitDelay: '<S3>/Output' incorporates: |
505 | * Constant: '<S11>/Constant' |
506 | */ |
507 | FoilControlRaspberrySentinel_DW.Output_DSTATE = |
508 | FoilControlRaspberrySentinel_P.Constant_Value_h; |
509 | } else { |
510 | /* Update for UnitDelay: '<S3>/Output' */ |
511 | FoilControlRaspberrySentinel_DW.Output_DSTATE = rtb_FixPtSum1; |
512 | } |
513 | |
514 | /* End of Switch: '<S11>/FixPt Switch' */ |
515 | |
516 | /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */ |
517 | sfcn_setPlatform_Update_wrapper(&FoilControlRaspberrySentinel_B.Switch1[0], |
518 | &FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE); |
519 | |
520 | /* External mode */ |
521 | rtExtModeUploadCheckTrigger(1); |
522 | |
523 | { /* Sample time: [0.02s, 0.0s] */ |
524 | rtExtModeUpload(0, FoilControlRaspberrySentinel_M->Timing.taskTime0); |
525 | } |
526 | |
527 | /* signal main to stop simulation */ |
528 | { /* Sample time: [0.02s, 0.0s] */ |
529 | if ((rtmGetTFinal(FoilControlRaspberrySentinel_M)!=-1) && |
530 | !((rtmGetTFinal(FoilControlRaspberrySentinel_M)- |
531 | FoilControlRaspberrySentinel_M->Timing.taskTime0) > |
532 | FoilControlRaspberrySentinel_M->Timing.taskTime0 * (DBL_EPSILON))) { |
533 | rtmSetErrorStatus(FoilControlRaspberrySentinel_M, "Simulation finished"); |
534 | } |
535 | |
536 | if (rtmGetStopRequested(FoilControlRaspberrySentinel_M)) { |
537 | rtmSetErrorStatus(FoilControlRaspberrySentinel_M, "Simulation finished"); |
538 | } |
539 | } |
540 | |
541 | /* Update absolute time for base rate */ |
542 | /* The "clockTick0" counts the number of times the code of this task has |
543 | * been executed. The absolute time is the multiplication of "clockTick0" |
544 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
545 | * overflow during the application lifespan selected. |
546 | * Timer of this task consists of two 32 bit unsigned integers. |
547 | * The two integers represent the low bits Timing.clockTick0 and the high bits |
548 | * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. |
549 | */ |
550 | if (!(++FoilControlRaspberrySentinel_M->Timing.clockTick0)) { |
551 | ++FoilControlRaspberrySentinel_M->Timing.clockTickH0; |
552 | } |
553 | |
554 | FoilControlRaspberrySentinel_M->Timing.taskTime0 = |
555 | FoilControlRaspberrySentinel_M->Timing.clockTick0 * |
556 | FoilControlRaspberrySentinel_M->Timing.stepSize0 + |
557 | FoilControlRaspberrySentinel_M->Timing.clockTickH0 * |
558 | FoilControlRaspberrySentinel_M->Timing.stepSize0 * 4294967296.0; |
559 | } |
560 | |
561 | /* Model initialize function */ |
562 | void FoilControlRaspberrySentinel_initialize(void) |
563 | { |
564 | /* Registration code */ |
565 | |
566 | /* initialize non-finites */ |
567 | rt_InitInfAndNaN(sizeof(real_T)); |
568 | |
569 | /* initialize real-time model */ |
570 | (void) memset((void *)FoilControlRaspberrySentinel_M, 0, |
571 | sizeof(RT_MODEL_FoilControlRaspberry_T)); |
572 | rtmSetTFinal(FoilControlRaspberrySentinel_M, -1); |
573 | FoilControlRaspberrySentinel_M->Timing.stepSize0 = 0.02; |
574 | |
575 | /* External mode info */ |
576 | FoilControlRaspberrySentinel_M->Sizes.checksums[0] = (495282541U); |
577 | FoilControlRaspberrySentinel_M->Sizes.checksums[1] = (2055743715U); |
578 | FoilControlRaspberrySentinel_M->Sizes.checksums[2] = (3829557736U); |
579 | FoilControlRaspberrySentinel_M->Sizes.checksums[3] = (3329260777U); |
580 | |
581 | { |
582 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
583 | static RTWExtModeInfo rt_ExtModeInfo; |
584 | static const sysRanDType *systemRan[8]; |
585 | FoilControlRaspberrySentinel_M->extModeInfo = (&rt_ExtModeInfo); |
586 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
587 | systemRan[0] = &rtAlwaysEnabled; |
588 | systemRan[1] = &rtAlwaysEnabled; |
589 | systemRan[2] = &rtAlwaysEnabled; |
590 | systemRan[3] = &rtAlwaysEnabled; |
591 | systemRan[4] = &rtAlwaysEnabled; |
592 | systemRan[5] = &rtAlwaysEnabled; |
593 | systemRan[6] = &rtAlwaysEnabled; |
594 | systemRan[7] = &rtAlwaysEnabled; |
595 | rteiSetModelMappingInfoPtr(FoilControlRaspberrySentinel_M->extModeInfo, |
596 | &FoilControlRaspberrySentinel_M->SpecialInfo.mappingInfo); |
597 | rteiSetChecksumsPtr(FoilControlRaspberrySentinel_M->extModeInfo, |
598 | FoilControlRaspberrySentinel_M->Sizes.checksums); |
599 | rteiSetTPtr(FoilControlRaspberrySentinel_M->extModeInfo, rtmGetTPtr |
600 | (FoilControlRaspberrySentinel_M)); |
601 | } |
602 | |
603 | /* block I/O */ |
604 | (void) memset(((void *) &FoilControlRaspberrySentinel_B), 0, |
605 | sizeof(B_FoilControlRaspberrySentine_T)); |
606 | |
607 | /* states (dwork) */ |
608 | (void) memset((void *)&FoilControlRaspberrySentinel_DW, 0, |
609 | sizeof(DW_FoilControlRaspberrySentin_T)); |
610 | |
611 | /* data type transition information */ |
612 | { |
613 | static DataTypeTransInfo dtInfo; |
614 | (void) memset((char_T *) &dtInfo, 0, |
615 | sizeof(dtInfo)); |
616 | FoilControlRaspberrySentinel_M->SpecialInfo.mappingInfo = (&dtInfo); |
617 | dtInfo.numDataTypes = 15; |
618 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
619 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
620 | |
621 | /* Block I/O transition table */ |
622 | dtInfo.BTransTable = &rtBTransTable; |
623 | |
624 | /* Parameters transition table */ |
625 | dtInfo.PTransTable = &rtPTransTable; |
626 | } |
627 | |
628 | /* Start for MATLABSystem: '<S4>/Digital Write' */ |
629 | FoilControlRaspberrySentinel_DW.obj.isInitialized = 0; |
630 | FoilControlRaspberrySentinel_DW.objisempty = true; |
631 | FoilControlRaspberrySentinel_DW.obj.isInitialized = 1; |
632 | MW_gpioInit((uint32_T)FoilControlRaspberryS_PinNumber, true); |
633 | |
634 | /* S-Function Block: <Root>/MagMan Foil with error */ |
635 | { |
636 | real_T initVector[1] = { 0 }; |
637 | |
638 | { |
639 | int_T i1; |
640 | for (i1=0; i1 < 1; i1++) { |
641 | FoilControlRaspberrySentinel_DW.MagManFoilwitherror_DSTATE = initVector |
642 | [0]; |
643 | } |
644 | } |
645 | } |
646 | |
647 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */ |
648 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[0] = |
649 | FoilControlRaspberrySentinel_P.Integrator_IC; |
650 | |
651 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */ |
652 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[0] = |
653 | FoilControlRaspberrySentinel_P.Filter_IC; |
654 | |
655 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Integrator' */ |
656 | FoilControlRaspberrySentinel_DW.Integrator_DSTATE[1] = |
657 | FoilControlRaspberrySentinel_P.Integrator_IC; |
658 | |
659 | /* InitializeConditions for DiscreteIntegrator: '<S5>/Filter' */ |
660 | FoilControlRaspberrySentinel_DW.Filter_DSTATE[1] = |
661 | FoilControlRaspberrySentinel_P.Filter_IC; |
662 | |
663 | /* InitializeConditions for UnitDelay: '<S3>/Output' */ |
664 | FoilControlRaspberrySentinel_DW.Output_DSTATE = |
665 | FoilControlRaspberrySentinel_P.Output_InitialCondition; |
666 | |
667 | /* S-Function Block: <Root>/MagMan Coils */ |
668 | { |
669 | real_T initVector[1] = { 0 }; |
670 | |
671 | { |
672 | int_T i1; |
673 | for (i1=0; i1 < 1; i1++) { |
674 | FoilControlRaspberrySentinel_DW.MagManCoils_DSTATE = initVector[0]; |
675 | } |
676 | } |
677 | } |
678 | } |
679 | |
680 | /* Model terminate function */ |
681 | void FoilControlRaspberrySentinel_terminate(void) |
682 | { |
683 | /* Start for MATLABSystem: '<S4>/Digital Write' incorporates: |
684 | * Terminate for MATLABSystem: '<S4>/Digital Write' |
685 | */ |
686 | if (FoilControlRaspberrySentinel_DW.obj.isInitialized == 1) { |
687 | FoilControlRaspberrySentinel_DW.obj.isInitialized = 2; |
688 | MW_gpioTerminate((uint32_T)FoilControlRaspberryS_PinNumber); |
689 | } |
690 | |
691 | /* End of Start for MATLABSystem: '<S4>/Digital Write' */ |
692 | } |
693 | |