1/*
2 * RaspberryMagmanControl.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 "RaspberryMagmanControl".
8 *
9 * Model version : 1.39
10 * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016
11 * C source code generated on : Wed Jan 04 15:46:45 2017
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 "RaspberryMagmanControl.h"
21#include "RaspberryMagmanControl_private.h"
22#include "RaspberryMagmanControl_dt.h"
23
24/* Block signals (auto storage) */
25B_RaspberryMagmanControl_T RaspberryMagmanControl_B;
26
27/* Block states (auto storage) */
28DW_RaspberryMagmanControl_T RaspberryMagmanControl_DW;
29
30/* Real-time model */
31RT_MODEL_RaspberryMagmanControl_T RaspberryMagmanControl_M_;
32RT_MODEL_RaspberryMagmanControl_T *const RaspberryMagmanControl_M =
33 &RaspberryMagmanControl_M_;
34
35/*
36 * Output and update for atomic system:
37 * '<S1>/Position transformation1'
38 * '<S4>/Position transformation'
39 */
40void Raspber_Positiontransformation1(real_T rtu_x, real_T rtu_y, const real_T
41 rtu_km4[9], B_Positiontransformation1_Ras_T *localB)
42{
43 /* MATLAB Function 'Camera position/Position transformation1': '<S11>:1' */
44 /* out=km2*[x;y;x^2;y^2;x*y;1]; */
45 /* '<S11>:1:4' xa = [x,y]; */
46 /* '<S11>:1:5' H = km4(:); */
47 /* '<S11>:1:6' xa = [xa,ones(size(xa,1),1)]; */
48 /* homogeneous coordinates */
49 /* '<S11>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */
50 /* '<S11>:1:9' X=out(1); */
51 localB->X = ((rtu_x * rtu_km4[0] + rtu_y * rtu_km4[1]) + rtu_km4[2]) / ((rtu_x
52 * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]);
53
54 /* '<S11>:1:10' Y=out(2); */
55 localB->Y = ((rtu_x * rtu_km4[3] + rtu_y * rtu_km4[4]) + rtu_km4[5]) / ((rtu_x
56 * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]);
57}
58
59real_T rt_powd_snf(real_T u0, real_T u1)
60{
61 real_T y;
62 real_T tmp;
63 real_T tmp_0;
64 if (rtIsNaN(u0) || rtIsNaN(u1)) {
65 y = (rtNaN);
66 } else {
67 tmp = fabs(u0);
68 tmp_0 = fabs(u1);
69 if (rtIsInf(u1)) {
70 if (tmp == 1.0) {
71 y = (rtNaN);
72 } else if (tmp > 1.0) {
73 if (u1 > 0.0) {
74 y = (rtInf);
75 } else {
76 y = 0.0;
77 }
78 } else if (u1 > 0.0) {
79 y = 0.0;
80 } else {
81 y = (rtInf);
82 }
83 } else if (tmp_0 == 0.0) {
84 y = 1.0;
85 } else if (tmp_0 == 1.0) {
86 if (u1 > 0.0) {
87 y = u0;
88 } else {
89 y = 1.0 / u0;
90 }
91 } else if (u1 == 2.0) {
92 y = u0 * u0;
93 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
94 y = sqrt(u0);
95 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
96 y = (rtNaN);
97 } else {
98 y = pow(u0, u1);
99 }
100 }
101
102 return y;
103}
104
105/* Model step function */
106void RaspberryMagmanControl_step(void)
107{
108 int32_T i;
109 int32_T xoffset;
110 real_T Fv_idx_0;
111 real_T Fv_idx_1;
112 real_T rtb_Sum_idx_0;
113 real_T rtb_F_idx_0;
114 real_T rtb_F_idx_1;
115
116 /* S-Function (sfcn_getFoilCheck): '<S4>/MagMan Foil with error' */
117 sfcn_getFoilCheck_Outputs_wrapper
118 ( &RaspberryMagmanControl_B.MagManFoilwitherror_o1,
119 &RaspberryMagmanControl_B.MagManFoilwitherror_o2,
120 &RaspberryMagmanControl_B.MagManFoilwitherror_o3,
121 &RaspberryMagmanControl_B.MagManFoilwitherror_o4,
122 &RaspberryMagmanControl_DW.MagManFoilwitherror_DSTATE);
123
124 /* MATLAB Function: '<S4>/Position transformation' incorporates:
125 * Constant: '<S4>/Sentinel Calibration matrix'
126 */
127 Raspber_Positiontransformation1
128 (RaspberryMagmanControl_B.MagManFoilwitherror_o1,
129 RaspberryMagmanControl_B.MagManFoilwitherror_o2,
130 RaspberryMagmanControl_P.km4pi,
131 &RaspberryMagmanControl_B.sf_Positiontransformation);
132
133 /* S-Function (sfcn_posread_shm): '<S10>/Shared memory camera position' */
134 sfcn_posread_shm_Outputs_wrapper
135 ( &RaspberryMagmanControl_B.Sharedmemorycameraposition_o1,
136 &RaspberryMagmanControl_B.Sharedmemorycameraposition_o2,
137 &RaspberryMagmanControl_DW.Sharedmemorycameraposition_DSTA);
138
139 /* DataTypeConversion: '<S10>/Data Type Conversion1' */
140 RaspberryMagmanControl_B.DataTypeConversion1 =
141 RaspberryMagmanControl_B.Sharedmemorycameraposition_o1;
142
143 /* DataTypeConversion: '<S10>/Data Type Conversion2' */
144 RaspberryMagmanControl_B.DataTypeConversion2 =
145 RaspberryMagmanControl_B.Sharedmemorycameraposition_o2;
146
147 /* MATLAB Function: '<S1>/Position transformation1' incorporates:
148 * Constant: '<S1>/Sentinel Calibration matrix1'
149 */
150 Raspber_Positiontransformation1(RaspberryMagmanControl_B.DataTypeConversion1,
151 RaspberryMagmanControl_B.DataTypeConversion2,
152 RaspberryMagmanControl_P.km4piCam,
153 &RaspberryMagmanControl_B.sf_Positiontransformation1);
154
155 /* ManualSwitch: '<Root>/Manual Switch' */
156 if (RaspberryMagmanControl_P.ManualSwitch_CurrentSetting == 1) {
157 RaspberryMagmanControl_B.ManualSwitch[0] =
158 RaspberryMagmanControl_B.sf_Positiontransformation.X;
159 RaspberryMagmanControl_B.ManualSwitch[1] =
160 RaspberryMagmanControl_B.sf_Positiontransformation.Y;
161 } else {
162 RaspberryMagmanControl_B.ManualSwitch[0] =
163 RaspberryMagmanControl_B.sf_Positiontransformation1.X;
164 RaspberryMagmanControl_B.ManualSwitch[1] =
165 RaspberryMagmanControl_B.sf_Positiontransformation1.Y;
166 }
167
168 /* End of ManualSwitch: '<Root>/Manual Switch' */
169
170 /* MATLAB Function: '<S5>/Embedded MATLAB Function' */
171 /* MATLAB Function 'Force to Currents/Embedded MATLAB Function': '<S14>:1' */
172 /* '<S14>:1:2' Ao=zeros(2,16); */
173 memset(&RaspberryMagmanControl_B.Ao[0], 0, sizeof(real_T) << 5U);
174
175 /* '<S14>:1:4' for iy=1:4 */
176 for (xoffset = 0; xoffset < 4; xoffset++) {
177 /* '<S14>:1:5' for ix=1:4 */
178 for (i = 0; i < 4; i++) {
179 /* '<S14>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */
180 /* '<S14>:1:14' d=dist(x,y, ix, iy); */
181 /* '<S14>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
182 RaspberryMagmanControl_B.lim = RaspberryMagmanControl_B.ManualSwitch[0] -
183 (1.0 + (real_T)i);
184 Fv_idx_1 = RaspberryMagmanControl_B.ManualSwitch[1] - (1.0 + (real_T)
185 xoffset);
186 RaspberryMagmanControl_B.lim = sqrt(RaspberryMagmanControl_B.lim *
187 RaspberryMagmanControl_B.lim + Fv_idx_1 * Fv_idx_1);
188
189 /* '<S14>:1:15' if d~=0 */
190 if (RaspberryMagmanControl_B.lim != 0.0) {
191 /* '<S14>:1:16' F=gForce(d); */
192 /* '<S14>:1:43' a = 622.06; */
193 /* '<S14>:1:44' b = 9.5032; */
194 /* '<S14>:1:45' c = 3.1884; */
195 /* '<S14>:1:46' F = x.*a./(b.*x.^2+ c).^3; */
196 RaspberryMagmanControl_B.lim = RaspberryMagmanControl_B.lim * 622.06 /
197 rt_powd_snf(RaspberryMagmanControl_B.lim *
198 RaspberryMagmanControl_B.lim * 9.5032 + 3.1884, 3.0);
199
200 /* '<S14>:1:17' Fv=F*vector(x,y,ix,iy); */
201 /* '<S14>:1:32' vx=ix-x; */
202 Fv_idx_1 = (1.0 + (real_T)i) - RaspberryMagmanControl_B.ManualSwitch[0];
203
204 /* '<S14>:1:33' vy=iy-y; */
205 RaspberryMagmanControl_B.vy = (1.0 + (real_T)xoffset) -
206 RaspberryMagmanControl_B.ManualSwitch[1];
207
208 /* '<S14>:1:35' if(vx==0 && vy ==0) */
209 if ((Fv_idx_1 == 0.0) && (RaspberryMagmanControl_B.vy == 0.0)) {
210 /* '<S14>:1:36' v=[0 0]; */
211 Fv_idx_0 = 0.0;
212 Fv_idx_1 = 0.0;
213 } else {
214 /* '<S14>:1:37' else */
215 /* '<S14>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */
216 /* '<S14>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */
217 RaspberryMagmanControl_B.d_a = sqrt(Fv_idx_1 * Fv_idx_1 +
218 RaspberryMagmanControl_B.vy * RaspberryMagmanControl_B.vy);
219 Fv_idx_0 = Fv_idx_1 / RaspberryMagmanControl_B.d_a;
220 Fv_idx_1 = RaspberryMagmanControl_B.vy / RaspberryMagmanControl_B.d_a;
221 }
222
223 Fv_idx_0 *= RaspberryMagmanControl_B.lim;
224 Fv_idx_1 *= RaspberryMagmanControl_B.lim;
225
226 /* '<S14>:1:18' Fx=Fv(1); */
227 RaspberryMagmanControl_B.lim = Fv_idx_0;
228
229 /* '<S14>:1:19' Fy=Fv(2); */
230 } else {
231 /* '<S14>:1:20' else */
232 /* '<S14>:1:21' Fx=0; */
233 RaspberryMagmanControl_B.lim = 0.0;
234
235 /* '<S14>:1:22' Fy=0; */
236 Fv_idx_1 = 0.0;
237 }
238
239 /* '<S14>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */
240 RaspberryMagmanControl_B.Ao[((xoffset << 2) + i) << 1] =
241 RaspberryMagmanControl_B.lim;
242
243 /* '<S14>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */
244 RaspberryMagmanControl_B.Ao[1 + (((xoffset << 2) + i) << 1)] = Fv_idx_1;
245 }
246 }
247
248 /* End of MATLAB Function: '<S5>/Embedded MATLAB Function' */
249
250 /* Sin: '<Root>/Sine Wave' */
251 RaspberryMagmanControl_B.SineWave = sin(RaspberryMagmanControl_P.SineWave_Freq
252 * RaspberryMagmanControl_M->Timing.t[0] +
253 RaspberryMagmanControl_P.SineWave_Phase) *
254 RaspberryMagmanControl_P.SineWave_Amp +
255 RaspberryMagmanControl_P.SineWave_Bias;
256
257 /* Sin: '<Root>/Sine Wave1' */
258 RaspberryMagmanControl_B.SineWave1 = sin
259 (RaspberryMagmanControl_P.SineWave1_Freq *
260 RaspberryMagmanControl_M->Timing.t[0] +
261 RaspberryMagmanControl_P.SineWave1_Phase) *
262 RaspberryMagmanControl_P.SineWave1_Amp +
263 RaspberryMagmanControl_P.SineWave1_Bias;
264
265 /* ManualSwitch: '<Root>/Manual Switch1' incorporates:
266 * Constant: '<Root>/Referece position'
267 */
268 if (RaspberryMagmanControl_P.ManualSwitch1_CurrentSetting == 1) {
269 RaspberryMagmanControl_B.ManualSwitch1[0] =
270 RaspberryMagmanControl_B.SineWave;
271 RaspberryMagmanControl_B.ManualSwitch1[1] =
272 RaspberryMagmanControl_B.SineWave1;
273 } else {
274 RaspberryMagmanControl_B.ManualSwitch1[0] =
275 RaspberryMagmanControl_P.Refereceposition_Value[0];
276 RaspberryMagmanControl_B.ManualSwitch1[1] =
277 RaspberryMagmanControl_P.Refereceposition_Value[1];
278 }
279
280 /* End of ManualSwitch: '<Root>/Manual Switch1' */
281
282 /* DiscreteIntegrator: '<S6>/Integrator' */
283 RaspberryMagmanControl_B.Integrator[0] =
284 RaspberryMagmanControl_DW.Integrator_DSTATE[0];
285
286 /* DiscreteIntegrator: '<S6>/Filter' */
287 RaspberryMagmanControl_B.Filter[0] = RaspberryMagmanControl_DW.Filter_DSTATE[0];
288
289 /* Gain: '<S6>/Filter Coefficient' incorporates:
290 * Gain: '<S6>/Derivative Gain'
291 * Gain: '<S6>/Setpoint Weighting (Derivative)'
292 * Sum: '<S6>/Sum3'
293 * Sum: '<S6>/SumD'
294 */
295 RaspberryMagmanControl_B.FilterCoefficient[0] =
296 ((RaspberryMagmanControl_P.PIDController2DOF_c *
297 RaspberryMagmanControl_B.ManualSwitch1[0] -
298 RaspberryMagmanControl_B.ManualSwitch[0]) *
299 RaspberryMagmanControl_P.PIDController2DOF_D -
300 RaspberryMagmanControl_B.Filter[0]) *
301 RaspberryMagmanControl_P.PIDController2DOF_N;
302
303 /* Sum: '<S6>/Sum' incorporates:
304 * Gain: '<S6>/Proportional Gain'
305 * Gain: '<S6>/Setpoint Weighting (Proportional)'
306 * Sum: '<S6>/Sum1'
307 */
308 Fv_idx_0 = ((RaspberryMagmanControl_P.PIDController2DOF_b *
309 RaspberryMagmanControl_B.ManualSwitch1[0] -
310 RaspberryMagmanControl_B.ManualSwitch[0]) *
311 RaspberryMagmanControl_P.PIDController2DOF_P +
312 RaspberryMagmanControl_B.Integrator[0]) +
313 RaspberryMagmanControl_B.FilterCoefficient[0];
314
315 /* Saturate: '<S6>/Saturate' */
316 if (Fv_idx_0 > RaspberryMagmanControl_P.PIDController2DOF_UpperSaturati) {
317 RaspberryMagmanControl_B.Saturate[0] =
318 RaspberryMagmanControl_P.PIDController2DOF_UpperSaturati;
319 } else if (Fv_idx_0 < RaspberryMagmanControl_P.PIDController2DOF_LowerSaturati)
320 {
321 RaspberryMagmanControl_B.Saturate[0] =
322 RaspberryMagmanControl_P.PIDController2DOF_LowerSaturati;
323 } else {
324 RaspberryMagmanControl_B.Saturate[0] = Fv_idx_0;
325 }
326
327 /* Sum: '<S6>/Sum' */
328 rtb_Sum_idx_0 = Fv_idx_0;
329
330 /* DiscreteIntegrator: '<S6>/Integrator' */
331 RaspberryMagmanControl_B.Integrator[1] =
332 RaspberryMagmanControl_DW.Integrator_DSTATE[1];
333
334 /* DiscreteIntegrator: '<S6>/Filter' */
335 RaspberryMagmanControl_B.Filter[1] = RaspberryMagmanControl_DW.Filter_DSTATE[1];
336
337 /* Gain: '<S6>/Filter Coefficient' incorporates:
338 * Gain: '<S6>/Derivative Gain'
339 * Gain: '<S6>/Setpoint Weighting (Derivative)'
340 * Sum: '<S6>/Sum3'
341 * Sum: '<S6>/SumD'
342 */
343 RaspberryMagmanControl_B.FilterCoefficient[1] =
344 ((RaspberryMagmanControl_P.PIDController2DOF_c *
345 RaspberryMagmanControl_B.ManualSwitch1[1] -
346 RaspberryMagmanControl_B.ManualSwitch[1]) *
347 RaspberryMagmanControl_P.PIDController2DOF_D -
348 RaspberryMagmanControl_B.Filter[1]) *
349 RaspberryMagmanControl_P.PIDController2DOF_N;
350
351 /* Sum: '<S6>/Sum' incorporates:
352 * Gain: '<S6>/Proportional Gain'
353 * Gain: '<S6>/Setpoint Weighting (Proportional)'
354 * Sum: '<S6>/Sum1'
355 */
356 Fv_idx_0 = ((RaspberryMagmanControl_P.PIDController2DOF_b *
357 RaspberryMagmanControl_B.ManualSwitch1[1] -
358 RaspberryMagmanControl_B.ManualSwitch[1]) *
359 RaspberryMagmanControl_P.PIDController2DOF_P +
360 RaspberryMagmanControl_B.Integrator[1]) +
361 RaspberryMagmanControl_B.FilterCoefficient[1];
362
363 /* Saturate: '<S6>/Saturate' */
364 if (Fv_idx_0 > RaspberryMagmanControl_P.PIDController2DOF_UpperSaturati) {
365 RaspberryMagmanControl_B.Saturate[1] =
366 RaspberryMagmanControl_P.PIDController2DOF_UpperSaturati;
367 } else if (Fv_idx_0 < RaspberryMagmanControl_P.PIDController2DOF_LowerSaturati)
368 {
369 RaspberryMagmanControl_B.Saturate[1] =
370 RaspberryMagmanControl_P.PIDController2DOF_LowerSaturati;
371 } else {
372 RaspberryMagmanControl_B.Saturate[1] = Fv_idx_0;
373 }
374
375 /* S-Function (cvxfasd): '<S5>/MagMan linearization1' */
376 cvxfasd_Outputs_wrapper(&RaspberryMagmanControl_B.Ao[0],
377 &RaspberryMagmanControl_B.Saturate[0], &RaspberryMagmanControl_B.Saturate[1],
378 &RaspberryMagmanControl_B.MagManlinearization1_o1[0],
379 &RaspberryMagmanControl_B.MagManlinearization1_o2 );
380
381 /* Math: '<S5>/Math Function' */
382 for (i = 0; i < 4; i++) {
383 RaspberryMagmanControl_B.MathFunction[i << 2] =
384 RaspberryMagmanControl_B.MagManlinearization1_o1[i];
385 RaspberryMagmanControl_B.MathFunction[1 + (i << 2)] =
386 RaspberryMagmanControl_B.MagManlinearization1_o1[i + 4];
387 RaspberryMagmanControl_B.MathFunction[2 + (i << 2)] =
388 RaspberryMagmanControl_B.MagManlinearization1_o1[i + 8];
389 RaspberryMagmanControl_B.MathFunction[3 + (i << 2)] =
390 RaspberryMagmanControl_B.MagManlinearization1_o1[i + 12];
391 }
392
393 /* End of Math: '<S5>/Math Function' */
394
395 /* MATLAB Function: '<Root>/Currents to PWM' */
396 /* MATLAB Function 'Currents to PWM': '<S3>:1' */
397 /* conversion from scaling factor to duty cycle of PWM */
398 /* '<S3>:1:3' Currents = max(0, min(1,Currents) ); */
399 /* limits */
400 /* '<S3>:1:4' i = 0.18515*Currents + 0.069587*Currents.^3 + 0.19285*Currents.^(1/3); */
401 /* ratio to current conversion */
402 /* '<S3>:1:5' PWM = i/0.455/2; */
403 /* '<S3>:1:6' PWM(PWM>0.05) = PWM(PWM>0.05) + 0.5; */
404 /* '<S3>:1:7' PWM(PWM<0.5) = 0; */
405 for (xoffset = 0; xoffset < 16; xoffset++) {
406 if ((1.0 <= RaspberryMagmanControl_B.MathFunction[xoffset]) || rtIsNaN
407 (RaspberryMagmanControl_B.MathFunction[xoffset])) {
408 RaspberryMagmanControl_B.lim = 1.0;
409 } else {
410 RaspberryMagmanControl_B.lim =
411 RaspberryMagmanControl_B.MathFunction[xoffset];
412 }
413
414 if ((0.0 >= RaspberryMagmanControl_B.lim) || rtIsNaN
415 (RaspberryMagmanControl_B.lim)) {
416 RaspberryMagmanControl_B.PWM[xoffset] = 0.0;
417 } else {
418 RaspberryMagmanControl_B.PWM[xoffset] = RaspberryMagmanControl_B.lim;
419 }
420
421 RaspberryMagmanControl_B.PWM[xoffset] = ((0.18515 *
422 RaspberryMagmanControl_B.PWM[xoffset] + 0.069587 * rt_powd_snf
423 (RaspberryMagmanControl_B.PWM[xoffset], 3.0)) + 0.19285 * rt_powd_snf
424 (RaspberryMagmanControl_B.PWM[xoffset], 0.33333333333333331)) / 0.455 /
425 2.0;
426 if (RaspberryMagmanControl_B.PWM[xoffset] > 0.05) {
427 RaspberryMagmanControl_B.PWM[xoffset] += 0.5;
428 }
429
430 if (RaspberryMagmanControl_B.PWM[xoffset] < 0.5) {
431 RaspberryMagmanControl_B.PWM[xoffset] = 0.0;
432 }
433 }
434
435 /* End of MATLAB Function: '<Root>/Currents to PWM' */
436
437 /* MATLAB Function: '<S2>/Nonlinearity of magnetic force' */
438 /* MATLAB Function 'Currents to Force/Nonlinearity of magnetic force': '<S12>:1' */
439 /* function F = fcn(u,xmes) */
440 /* Computes vector of magnetic force exerted on the ball in x- and */
441 /* y-direction. Input arguments are excitation factors of coils and measured */
442 /* position of the ball. Contributions of the coils are superposed to get */
443 /* the overall force. */
444 /* Note, that this function only models nonlinearity of the magnetic force, */
445 /* it is not any kind of feedback control. */
446 /* '<S12>:1:10' umin = 1; */
447 /* index of the 1st actuator in one dim */
448 /* '<S12>:1:11' umax = 4; */
449 /* index of tohe last actuator in one dim */
450 /* '<S12>:1:13' F = [0;0]; */
451 rtb_F_idx_0 = 0.0;
452 rtb_F_idx_1 = 0.0;
453
454 /* '<S12>:1:15' u = abs(u); */
455 for (xoffset = 0; xoffset < 16; xoffset++) {
456 RaspberryMagmanControl_B.minval[xoffset] = fabs
457 (RaspberryMagmanControl_B.MathFunction[xoffset]);
458 }
459
460 /* '<S12>:1:16' for cnt = umin:umax */
461 for (xoffset = 0; xoffset < 4; xoffset++) {
462 /* '<S12>:1:17' for cnt2 = umin:umax */
463 for (i = 0; i < 4; i++) {
464 /* '<S12>:1:18' F = F + [dxfce(cnt,cnt2,xmes(1),xmes(2));... */
465 /* '<S12>:1:19' dyfce(cnt,cnt2,xmes(1),xmes(2))] * u(cnt2-umin+1,cnt-umin+1); */
466 /* Compute x-component of force from coil [m,n] exerted on the ball at */
467 /* position [x,y] */
468 /* '<S12>:1:27' out = -48.282*(x-m)./( (x-m).^2 + (y-n).^2 + 0.3303).^3; */
469 RaspberryMagmanControl_B.lim = RaspberryMagmanControl_B.ManualSwitch[0] -
470 (1.0 + (real_T)xoffset);
471 Fv_idx_1 = RaspberryMagmanControl_B.ManualSwitch[1] - (1.0 + (real_T)i);
472
473 /* Compute y-component of force from coil [m,n] exerted on the ball at */
474 /* position [x,y] */
475 /* '<S12>:1:33' out = -48.282*(y-n)./( (x-m).^2 + (y-n).^2 + 0.3303).^3; */
476 RaspberryMagmanControl_B.vy = RaspberryMagmanControl_B.ManualSwitch[0] -
477 (1.0 + (real_T)xoffset);
478 RaspberryMagmanControl_B.d_a = RaspberryMagmanControl_B.ManualSwitch[1] -
479 (1.0 + (real_T)i);
480 rtb_F_idx_0 += (RaspberryMagmanControl_B.ManualSwitch[0] - (1.0 + (real_T)
481 xoffset)) * -48.282 / rt_powd_snf((RaspberryMagmanControl_B.lim *
482 RaspberryMagmanControl_B.lim + Fv_idx_1 * Fv_idx_1) + 0.3303, 3.0) *
483 RaspberryMagmanControl_B.minval[(xoffset << 2) + i];
484 rtb_F_idx_1 += (RaspberryMagmanControl_B.ManualSwitch[1] - (1.0 + (real_T)
485 i)) * -48.282 / rt_powd_snf((RaspberryMagmanControl_B.vy *
486 RaspberryMagmanControl_B.vy + RaspberryMagmanControl_B.d_a *
487 RaspberryMagmanControl_B.d_a) + 0.3303, 3.0) *
488 RaspberryMagmanControl_B.minval[(xoffset << 2) + i];
489 }
490 }
491
492 /* End of MATLAB Function: '<S2>/Nonlinearity of magnetic force' */
493
494 /* Gain: '<S2>/Gain' */
495 RaspberryMagmanControl_B.Gain[0] = RaspberryMagmanControl_P.Gain_Gain *
496 rtb_F_idx_0;
497 RaspberryMagmanControl_B.Gain[1] = RaspberryMagmanControl_P.Gain_Gain *
498 rtb_F_idx_1;
499
500 /* MATLAB Function: '<Root>/proud' */
501 /* MATLAB Function 'proud': '<S9>:1' */
502 /* '<S9>:1:2' i = (max(0, pw-0.5).*(0.455*2))'; */
503 for (xoffset = 0; xoffset < 16; xoffset++) {
504 if ((0.0 >= RaspberryMagmanControl_B.PWM[xoffset] - 0.5) || rtIsNaN
505 (RaspberryMagmanControl_B.PWM[xoffset] - 0.5)) {
506 RaspberryMagmanControl_B.minval[xoffset] = 0.0;
507 } else {
508 RaspberryMagmanControl_B.minval[xoffset] =
509 RaspberryMagmanControl_B.PWM[xoffset] - 0.5;
510 }
511 }
512
513 for (i = 0; i < 4; i++) {
514 RaspberryMagmanControl_B.i[i << 2] = RaspberryMagmanControl_B.minval[i] *
515 0.91;
516 RaspberryMagmanControl_B.i[1 + (i << 2)] = RaspberryMagmanControl_B.minval[i
517 + 4] * 0.91;
518 RaspberryMagmanControl_B.i[2 + (i << 2)] = RaspberryMagmanControl_B.minval[i
519 + 8] * 0.91;
520 RaspberryMagmanControl_B.i[3 + (i << 2)] = RaspberryMagmanControl_B.minval[i
521 + 12] * 0.91;
522 }
523
524 /* '<S9>:1:3' lim = 0; */
525 RaspberryMagmanControl_B.lim = 0.0;
526
527 /* '<S9>:1:4' if sum(sum(abs(i))) < 4 */
528 memcpy(&RaspberryMagmanControl_B.minval[0], &RaspberryMagmanControl_B.i[0],
529 sizeof(real_T) << 4U);
530 for (i = 0; i < 4; i++) {
531 xoffset = i << 2;
532 RaspberryMagmanControl_B.y[i] = ((RaspberryMagmanControl_B.minval[xoffset +
533 1] + RaspberryMagmanControl_B.minval[xoffset]) +
534 RaspberryMagmanControl_B.minval[xoffset + 2]) +
535 RaspberryMagmanControl_B.minval[xoffset + 3];
536 }
537
538 if (((RaspberryMagmanControl_B.y[0] + RaspberryMagmanControl_B.y[1]) +
539 RaspberryMagmanControl_B.y[2]) + RaspberryMagmanControl_B.y[3] < 4.0) {
540 /* '<S9>:1:5' lim = 1; */
541 RaspberryMagmanControl_B.lim = 1.0;
542 }
543
544 /* End of MATLAB Function: '<Root>/proud' */
545
546 /* Switch: '<Root>/Switch1' incorporates:
547 * Constant: '<Root>/Constant2'
548 */
549 if (RaspberryMagmanControl_B.lim != 0.0) {
550 memcpy(&RaspberryMagmanControl_B.Switch1[0], &RaspberryMagmanControl_B.PWM[0],
551 sizeof(real_T) << 4U);
552 } else {
553 memcpy(&RaspberryMagmanControl_B.Switch1[0],
554 &RaspberryMagmanControl_P.Constant2_Value[0], sizeof(real_T) << 4U);
555 }
556
557 /* End of Switch: '<Root>/Switch1' */
558
559 /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */
560 sfcn_setPlatform_Outputs_wrapper(&RaspberryMagmanControl_B.Switch1[0],
561 &RaspberryMagmanControl_DW.MagManCoils_DSTATE);
562
563 /* Sum: '<S6>/SumI1' incorporates:
564 * Gain: '<S6>/Integral Gain'
565 * Gain: '<S6>/Kb'
566 * Sum: '<S6>/Sum2'
567 * Sum: '<S6>/SumI2'
568 */
569 RaspberryMagmanControl_B.SumI1[0] = (RaspberryMagmanControl_B.ManualSwitch1[0]
570 - RaspberryMagmanControl_B.ManualSwitch[0]) *
571 RaspberryMagmanControl_P.PIDController2DOF_I +
572 (RaspberryMagmanControl_B.Saturate[0] - rtb_Sum_idx_0) *
573 RaspberryMagmanControl_P.PIDController2DOF_Kb;
574 RaspberryMagmanControl_B.SumI1[1] = (RaspberryMagmanControl_B.ManualSwitch1[1]
575 - RaspberryMagmanControl_B.ManualSwitch[1]) *
576 RaspberryMagmanControl_P.PIDController2DOF_I +
577 (RaspberryMagmanControl_B.Saturate[1] - Fv_idx_0) *
578 RaspberryMagmanControl_P.PIDController2DOF_Kb;
579
580 /* S-Function "sfcn_getFoilCheck_wrapper" Block: <S4>/MagMan Foil with error */
581 sfcn_getFoilCheck_Update_wrapper
582 ( &RaspberryMagmanControl_B.MagManFoilwitherror_o1,
583 &RaspberryMagmanControl_B.MagManFoilwitherror_o2,
584 &RaspberryMagmanControl_B.MagManFoilwitherror_o3,
585 &RaspberryMagmanControl_B.MagManFoilwitherror_o4,
586 &RaspberryMagmanControl_DW.MagManFoilwitherror_DSTATE);
587
588 /* S-Function "sfcn_posread_shm_wrapper" Block: <S10>/Shared memory camera position */
589 sfcn_posread_shm_Update_wrapper
590 ( &RaspberryMagmanControl_B.Sharedmemorycameraposition_o1,
591 &RaspberryMagmanControl_B.Sharedmemorycameraposition_o2,
592 &RaspberryMagmanControl_DW.Sharedmemorycameraposition_DSTA);
593
594 /* Update for DiscreteIntegrator: '<S6>/Integrator' */
595 RaspberryMagmanControl_DW.Integrator_DSTATE[0] +=
596 RaspberryMagmanControl_P.Integrator_gainval *
597 RaspberryMagmanControl_B.SumI1[0];
598
599 /* Update for DiscreteIntegrator: '<S6>/Filter' */
600 RaspberryMagmanControl_DW.Filter_DSTATE[0] +=
601 RaspberryMagmanControl_P.Filter_gainval *
602 RaspberryMagmanControl_B.FilterCoefficient[0];
603
604 /* Update for DiscreteIntegrator: '<S6>/Integrator' */
605 RaspberryMagmanControl_DW.Integrator_DSTATE[1] +=
606 RaspberryMagmanControl_P.Integrator_gainval *
607 RaspberryMagmanControl_B.SumI1[1];
608
609 /* Update for DiscreteIntegrator: '<S6>/Filter' */
610 RaspberryMagmanControl_DW.Filter_DSTATE[1] +=
611 RaspberryMagmanControl_P.Filter_gainval *
612 RaspberryMagmanControl_B.FilterCoefficient[1];
613
614 /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */
615 sfcn_setPlatform_Update_wrapper(&RaspberryMagmanControl_B.Switch1[0],
616 &RaspberryMagmanControl_DW.MagManCoils_DSTATE);
617
618 /* External mode */
619 rtExtModeUploadCheckTrigger(2);
620
621 { /* Sample time: [0.0s, 0.0s] */
622 rtExtModeUpload(0, RaspberryMagmanControl_M->Timing.t[0]);
623 }
624
625 { /* Sample time: [0.02s, 0.0s] */
626 rtExtModeUpload(1, (((RaspberryMagmanControl_M->Timing.clockTick1+
627 RaspberryMagmanControl_M->Timing.clockTickH1*
628 4294967296.0)) * 0.02));
629 }
630
631 /* signal main to stop simulation */
632 { /* Sample time: [0.0s, 0.0s] */
633 if ((rtmGetTFinal(RaspberryMagmanControl_M)!=-1) &&
634 !((rtmGetTFinal(RaspberryMagmanControl_M)-
635 RaspberryMagmanControl_M->Timing.t[0]) >
636 RaspberryMagmanControl_M->Timing.t[0] * (DBL_EPSILON))) {
637 rtmSetErrorStatus(RaspberryMagmanControl_M, "Simulation finished");
638 }
639
640 if (rtmGetStopRequested(RaspberryMagmanControl_M)) {
641 rtmSetErrorStatus(RaspberryMagmanControl_M, "Simulation finished");
642 }
643 }
644
645 /* Update absolute time for base rate */
646 /* The "clockTick0" counts the number of times the code of this task has
647 * been executed. The absolute time is the multiplication of "clockTick0"
648 * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
649 * overflow during the application lifespan selected.
650 * Timer of this task consists of two 32 bit unsigned integers.
651 * The two integers represent the low bits Timing.clockTick0 and the high bits
652 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
653 */
654 if (!(++RaspberryMagmanControl_M->Timing.clockTick0)) {
655 ++RaspberryMagmanControl_M->Timing.clockTickH0;
656 }
657
658 RaspberryMagmanControl_M->Timing.t[0] =
659 RaspberryMagmanControl_M->Timing.clockTick0 *
660 RaspberryMagmanControl_M->Timing.stepSize0 +
661 RaspberryMagmanControl_M->Timing.clockTickH0 *
662 RaspberryMagmanControl_M->Timing.stepSize0 * 4294967296.0;
663
664 {
665 /* Update absolute timer for sample time: [0.02s, 0.0s] */
666 /* The "clockTick1" counts the number of times the code of this task has
667 * been executed. The resolution of this integer timer is 0.02, which is the step size
668 * of the task. Size of "clockTick1" ensures timer will not overflow during the
669 * application lifespan selected.
670 * Timer of this task consists of two 32 bit unsigned integers.
671 * The two integers represent the low bits Timing.clockTick1 and the high bits
672 * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
673 */
674 RaspberryMagmanControl_M->Timing.clockTick1++;
675 if (!RaspberryMagmanControl_M->Timing.clockTick1) {
676 RaspberryMagmanControl_M->Timing.clockTickH1++;
677 }
678 }
679}
680
681/* Model initialize function */
682void RaspberryMagmanControl_initialize(void)
683{
684 /* Registration code */
685
686 /* initialize non-finites */
687 rt_InitInfAndNaN(sizeof(real_T));
688
689 /* initialize real-time model */
690 (void) memset((void *)RaspberryMagmanControl_M, 0,
691 sizeof(RT_MODEL_RaspberryMagmanControl_T));
692
693 {
694 /* Setup solver object */
695 rtsiSetSimTimeStepPtr(&RaspberryMagmanControl_M->solverInfo,
696 &RaspberryMagmanControl_M->Timing.simTimeStep);
697 rtsiSetTPtr(&RaspberryMagmanControl_M->solverInfo, &rtmGetTPtr
698 (RaspberryMagmanControl_M));
699 rtsiSetStepSizePtr(&RaspberryMagmanControl_M->solverInfo,
700 &RaspberryMagmanControl_M->Timing.stepSize0);
701 rtsiSetErrorStatusPtr(&RaspberryMagmanControl_M->solverInfo,
702 (&rtmGetErrorStatus(RaspberryMagmanControl_M)));
703 rtsiSetRTModelPtr(&RaspberryMagmanControl_M->solverInfo,
704 RaspberryMagmanControl_M);
705 }
706
707 rtsiSetSimTimeStep(&RaspberryMagmanControl_M->solverInfo, MAJOR_TIME_STEP);
708 rtsiSetSolverName(&RaspberryMagmanControl_M->solverInfo,"FixedStepDiscrete");
709 rtmSetTPtr(RaspberryMagmanControl_M, &RaspberryMagmanControl_M->Timing.tArray
710 [0]);
711 rtmSetTFinal(RaspberryMagmanControl_M, -1);
712 RaspberryMagmanControl_M->Timing.stepSize0 = 0.02;
713
714 /* External mode info */
715 RaspberryMagmanControl_M->Sizes.checksums[0] = (51860387U);
716 RaspberryMagmanControl_M->Sizes.checksums[1] = (3512026705U);
717 RaspberryMagmanControl_M->Sizes.checksums[2] = (1923139619U);
718 RaspberryMagmanControl_M->Sizes.checksums[3] = (1747157281U);
719
720 {
721 static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
722 static RTWExtModeInfo rt_ExtModeInfo;
723 static const sysRanDType *systemRan[9];
724 RaspberryMagmanControl_M->extModeInfo = (&rt_ExtModeInfo);
725 rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
726 systemRan[0] = &rtAlwaysEnabled;
727 systemRan[1] = &rtAlwaysEnabled;
728 systemRan[2] = &rtAlwaysEnabled;
729 systemRan[3] = &rtAlwaysEnabled;
730 systemRan[4] = &rtAlwaysEnabled;
731 systemRan[5] = &rtAlwaysEnabled;
732 systemRan[6] = &rtAlwaysEnabled;
733 systemRan[7] = &rtAlwaysEnabled;
734 systemRan[8] = &rtAlwaysEnabled;
735 rteiSetModelMappingInfoPtr(RaspberryMagmanControl_M->extModeInfo,
736 &RaspberryMagmanControl_M->SpecialInfo.mappingInfo);
737 rteiSetChecksumsPtr(RaspberryMagmanControl_M->extModeInfo,
738 RaspberryMagmanControl_M->Sizes.checksums);
739 rteiSetTPtr(RaspberryMagmanControl_M->extModeInfo, rtmGetTPtr
740 (RaspberryMagmanControl_M));
741 }
742
743 /* block I/O */
744 (void) memset(((void *) &RaspberryMagmanControl_B), 0,
745 sizeof(B_RaspberryMagmanControl_T));
746
747 /* states (dwork) */
748 (void) memset((void *)&RaspberryMagmanControl_DW, 0,
749 sizeof(DW_RaspberryMagmanControl_T));
750
751 /* data type transition information */
752 {
753 static DataTypeTransInfo dtInfo;
754 (void) memset((char_T *) &dtInfo, 0,
755 sizeof(dtInfo));
756 RaspberryMagmanControl_M->SpecialInfo.mappingInfo = (&dtInfo);
757 dtInfo.numDataTypes = 14;
758 dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
759 dtInfo.dataTypeNames = &rtDataTypeNames[0];
760
761 /* Block I/O transition table */
762 dtInfo.BTransTable = &rtBTransTable;
763
764 /* Parameters transition table */
765 dtInfo.PTransTable = &rtPTransTable;
766 }
767
768 /* S-Function Block: <S4>/MagMan Foil with error */
769 {
770 real_T initVector[1] = { 0 };
771
772 {
773 int_T i1;
774 for (i1=0; i1 < 1; i1++) {
775 RaspberryMagmanControl_DW.MagManFoilwitherror_DSTATE = initVector[0];
776 }
777 }
778 }
779
780 /* S-Function Block: <S10>/Shared memory camera position */
781 {
782 real_T initVector[1] = { 0 };
783
784 {
785 int_T i1;
786 for (i1=0; i1 < 1; i1++) {
787 RaspberryMagmanControl_DW.Sharedmemorycameraposition_DSTA = initVector[0];
788 }
789 }
790 }
791
792 /* InitializeConditions for DiscreteIntegrator: '<S6>/Integrator' */
793 RaspberryMagmanControl_DW.Integrator_DSTATE[0] =
794 RaspberryMagmanControl_P.Integrator_IC;
795
796 /* InitializeConditions for DiscreteIntegrator: '<S6>/Filter' */
797 RaspberryMagmanControl_DW.Filter_DSTATE[0] =
798 RaspberryMagmanControl_P.Filter_IC;
799
800 /* InitializeConditions for DiscreteIntegrator: '<S6>/Integrator' */
801 RaspberryMagmanControl_DW.Integrator_DSTATE[1] =
802 RaspberryMagmanControl_P.Integrator_IC;
803
804 /* InitializeConditions for DiscreteIntegrator: '<S6>/Filter' */
805 RaspberryMagmanControl_DW.Filter_DSTATE[1] =
806 RaspberryMagmanControl_P.Filter_IC;
807
808 /* S-Function Block: <Root>/MagMan Coils */
809 {
810 real_T initVector[1] = { 0 };
811
812 {
813 int_T i1;
814 for (i1=0; i1 < 1; i1++) {
815 RaspberryMagmanControl_DW.MagManCoils_DSTATE = initVector[0];
816 }
817 }
818 }
819}
820
821/* Model terminate function */
822void RaspberryMagmanControl_terminate(void)
823{
824 /* (no terminate code required) */
825}
826