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) */ |
25 | B_RaspberryMagmanControl_T RaspberryMagmanControl_B; |
26 | |
27 | /* Block states (auto storage) */ |
28 | DW_RaspberryMagmanControl_T RaspberryMagmanControl_DW; |
29 | |
30 | /* Real-time model */ |
31 | RT_MODEL_RaspberryMagmanControl_T RaspberryMagmanControl_M_; |
32 | RT_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 | */ |
40 | void 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 | |
59 | real_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 */ |
106 | void 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 */ |
682 | void 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 */ |
822 | void RaspberryMagmanControl_terminate(void) |
823 | { |
824 | /* (no terminate code required) */ |
825 | } |
826 | |