1 | /* |
2 | * RaspberryPrediction.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 "RaspberryPrediction". |
8 | * |
9 | * Model version : 1.13 |
10 | * Simulink Coder version : 8.11 (R2016b) 25-Aug-2016 |
11 | * C source code generated on : Tue Dec 20 13:49:58 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 "RaspberryPrediction.h" |
21 | #include "RaspberryPrediction_private.h" |
22 | #include "RaspberryPrediction_dt.h" |
23 | |
24 | /* Block signals (auto storage) */ |
25 | B_RaspberryPrediction_T RaspberryPrediction_B; |
26 | |
27 | /* Continuous states */ |
28 | X_RaspberryPrediction_T RaspberryPrediction_X; |
29 | |
30 | /* Block states (auto storage) */ |
31 | DW_RaspberryPrediction_T RaspberryPrediction_DW; |
32 | |
33 | /* Real-time model */ |
34 | RT_MODEL_RaspberryPrediction_T RaspberryPrediction_M_; |
35 | RT_MODEL_RaspberryPrediction_T *const RaspberryPrediction_M = |
36 | &RaspberryPrediction_M_; |
37 | |
38 | /* |
39 | * This function updates continuous states using the ODE1 fixed-step |
40 | * solver algorithm |
41 | */ |
42 | static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si ) |
43 | { |
44 | time_T tnew = rtsiGetSolverStopTime(si); |
45 | time_T h = rtsiGetStepSize(si); |
46 | real_T *x = rtsiGetContStates(si); |
47 | ODE1_IntgData *id = (ODE1_IntgData *)rtsiGetSolverData(si); |
48 | real_T *f0 = id->f[0]; |
49 | int_T i; |
50 | int_T nXc = 4; |
51 | rtsiSetSimTimeStep(si,MINOR_TIME_STEP); |
52 | rtsiSetdX(si, f0); |
53 | RaspberryPrediction_derivatives(); |
54 | rtsiSetT(si, tnew); |
55 | for (i = 0; i < nXc; ++i) { |
56 | x[i] += h * f0[i]; |
57 | } |
58 | |
59 | rtsiSetSimTimeStep(si,MAJOR_TIME_STEP); |
60 | } |
61 | |
62 | /* |
63 | * Output and update for atomic system: |
64 | * '<S1>/Position transformation1' |
65 | * '<S4>/Position transformation' |
66 | */ |
67 | void Raspber_Positiontransformation1(real_T rtu_x, real_T rtu_y, const real_T |
68 | rtu_km4[9], B_Positiontransformation1_Ras_T *localB) |
69 | { |
70 | /* MATLAB Function 'Camera position/Position transformation1': '<S7>:1' */ |
71 | /* out=km2*[x;y;x^2;y^2;x*y;1]; */ |
72 | /* '<S7>:1:4' xa = [x,y]; */ |
73 | /* '<S7>:1:5' H = km4(:); */ |
74 | /* '<S7>:1:6' xa = [xa,ones(size(xa,1),1)]; */ |
75 | /* homogeneous coordinates */ |
76 | /* '<S7>:1:7' out = [ (xa*H(1:3))./(xa*H(7:9)), (xa*H(4:6))./(xa*H(7:9)) ]; */ |
77 | /* '<S7>:1:9' X=out(1); */ |
78 | localB->X = ((rtu_x * rtu_km4[0] + rtu_y * rtu_km4[1]) + rtu_km4[2]) / ((rtu_x |
79 | * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]); |
80 | |
81 | /* '<S7>:1:10' Y=out(2); */ |
82 | localB->Y = ((rtu_x * rtu_km4[3] + rtu_y * rtu_km4[4]) + rtu_km4[5]) / ((rtu_x |
83 | * rtu_km4[6] + rtu_y * rtu_km4[7]) + rtu_km4[8]); |
84 | } |
85 | |
86 | real_T rt_powd_snf(real_T u0, real_T u1) |
87 | { |
88 | real_T y; |
89 | real_T tmp; |
90 | real_T tmp_0; |
91 | if (rtIsNaN(u0) || rtIsNaN(u1)) { |
92 | y = (rtNaN); |
93 | } else { |
94 | tmp = fabs(u0); |
95 | tmp_0 = fabs(u1); |
96 | if (rtIsInf(u1)) { |
97 | if (tmp == 1.0) { |
98 | y = (rtNaN); |
99 | } else if (tmp > 1.0) { |
100 | if (u1 > 0.0) { |
101 | y = (rtInf); |
102 | } else { |
103 | y = 0.0; |
104 | } |
105 | } else if (u1 > 0.0) { |
106 | y = 0.0; |
107 | } else { |
108 | y = (rtInf); |
109 | } |
110 | } else if (tmp_0 == 0.0) { |
111 | y = 1.0; |
112 | } else if (tmp_0 == 1.0) { |
113 | if (u1 > 0.0) { |
114 | y = u0; |
115 | } else { |
116 | y = 1.0 / u0; |
117 | } |
118 | } else if (u1 == 2.0) { |
119 | y = u0 * u0; |
120 | } else if ((u1 == 0.5) && (u0 >= 0.0)) { |
121 | y = sqrt(u0); |
122 | } else if ((u0 < 0.0) && (u1 > floor(u1))) { |
123 | y = (rtNaN); |
124 | } else { |
125 | y = pow(u0, u1); |
126 | } |
127 | } |
128 | |
129 | return y; |
130 | } |
131 | |
132 | /* Model step function */ |
133 | void RaspberryPrediction_step(void) |
134 | { |
135 | int_T iy; |
136 | int32_T ix; |
137 | real_T d; |
138 | real_T vy; |
139 | real_T b_d; |
140 | real_T Fv_idx_0; |
141 | real_T Fv_idx_1; |
142 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
143 | /* set solver stop time */ |
144 | if (!(RaspberryPrediction_M->Timing.clockTick0+1)) { |
145 | rtsiSetSolverStopTime(&RaspberryPrediction_M->solverInfo, |
146 | ((RaspberryPrediction_M->Timing.clockTickH0 + 1) * |
147 | RaspberryPrediction_M->Timing.stepSize0 * 4294967296.0)); |
148 | } else { |
149 | rtsiSetSolverStopTime(&RaspberryPrediction_M->solverInfo, |
150 | ((RaspberryPrediction_M->Timing.clockTick0 + 1) * |
151 | RaspberryPrediction_M->Timing.stepSize0 + |
152 | RaspberryPrediction_M->Timing.clockTickH0 * |
153 | RaspberryPrediction_M->Timing.stepSize0 * 4294967296.0)); |
154 | } |
155 | } /* end MajorTimeStep */ |
156 | |
157 | /* Update absolute time of base rate at minor time step */ |
158 | if (rtmIsMinorTimeStep(RaspberryPrediction_M)) { |
159 | RaspberryPrediction_M->Timing.t[0] = rtsiGetT |
160 | (&RaspberryPrediction_M->solverInfo); |
161 | } |
162 | |
163 | /* StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
164 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] = 0.0; |
165 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] = 0.0; |
166 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] += |
167 | RaspberryPrediction_P.Linearmodeloftherollingball2i_h[0] * |
168 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[1]; |
169 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] += |
170 | RaspberryPrediction_P.Linearmodeloftherollingball2i_h[1] * |
171 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[3]; |
172 | |
173 | /* MATLAB Function: '<S5>/Embedded MATLAB Function' */ |
174 | /* MATLAB Function 'Force to Currents/Embedded MATLAB Function': '<S10>:1' */ |
175 | /* '<S10>:1:2' Ao=zeros(2,16); */ |
176 | memset(&RaspberryPrediction_B.Ao[0], 0, sizeof(real_T) << 5U); |
177 | |
178 | /* '<S10>:1:4' for iy=1:4 */ |
179 | for (iy = 0; iy < 4; iy++) { |
180 | /* '<S10>:1:5' for ix=1:4 */ |
181 | for (ix = 0; ix < 4; ix++) { |
182 | /* '<S10>:1:6' [cFx, cFy] = fForce(x, y, ix, iy); */ |
183 | /* '<S10>:1:14' d=dist(x,y, ix, iy); */ |
184 | /* '<S10>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
185 | d = RaspberryPrediction_B.Linearmodeloftherollingball2ind[0] - (1.0 + |
186 | (real_T)ix); |
187 | Fv_idx_1 = RaspberryPrediction_B.Linearmodeloftherollingball2ind[1] - (1.0 |
188 | + (real_T)iy); |
189 | d = sqrt(d * d + Fv_idx_1 * Fv_idx_1); |
190 | |
191 | /* '<S10>:1:15' if d~=0 */ |
192 | if (d != 0.0) { |
193 | /* '<S10>:1:16' F=gForce(d); */ |
194 | /* '<S10>:1:43' a = 622.06; */ |
195 | /* '<S10>:1:44' b = 9.5032; */ |
196 | /* '<S10>:1:45' c = 3.1884; */ |
197 | /* '<S10>:1:46' F = x.*a./(b.*x.^2+ c).^3; */ |
198 | d = d * 622.06 / rt_powd_snf(d * d * 9.5032 + 3.1884, 3.0); |
199 | |
200 | /* '<S10>:1:17' Fv=F*vector(x,y,ix,iy); */ |
201 | /* '<S10>:1:32' vx=ix-x; */ |
202 | Fv_idx_1 = (1.0 + (real_T)ix) - |
203 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[0]; |
204 | |
205 | /* '<S10>:1:33' vy=iy-y; */ |
206 | vy = (1.0 + (real_T)iy) - |
207 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[1]; |
208 | |
209 | /* '<S10>:1:35' if(vx==0 && vy ==0) */ |
210 | if ((Fv_idx_1 == 0.0) && (vy == 0.0)) { |
211 | /* '<S10>:1:36' v=[0 0]; */ |
212 | Fv_idx_0 = 0.0; |
213 | Fv_idx_1 = 0.0; |
214 | } else { |
215 | /* '<S10>:1:37' else */ |
216 | /* '<S10>:1:38' v = [vx vy]/dist(vx, vy, 0 ,0); */ |
217 | /* '<S10>:1:28' d=sqrt((x-ix).^2+(y-iy).^2); */ |
218 | b_d = sqrt(Fv_idx_1 * Fv_idx_1 + vy * vy); |
219 | Fv_idx_0 = Fv_idx_1 / b_d; |
220 | Fv_idx_1 = vy / b_d; |
221 | } |
222 | |
223 | Fv_idx_0 *= d; |
224 | Fv_idx_1 *= d; |
225 | |
226 | /* '<S10>:1:18' Fx=Fv(1); */ |
227 | d = Fv_idx_0; |
228 | |
229 | /* '<S10>:1:19' Fy=Fv(2); */ |
230 | } else { |
231 | /* '<S10>:1:20' else */ |
232 | /* '<S10>:1:21' Fx=0; */ |
233 | d = 0.0; |
234 | |
235 | /* '<S10>:1:22' Fy=0; */ |
236 | Fv_idx_1 = 0.0; |
237 | } |
238 | |
239 | /* '<S10>:1:7' Ao(1,(iy-1)*4+ix) = cFx; */ |
240 | RaspberryPrediction_B.Ao[((iy << 2) + ix) << 1] = d; |
241 | |
242 | /* '<S10>:1:8' Ao(2,(iy-1)*4+ix) = cFy; */ |
243 | RaspberryPrediction_B.Ao[1 + (((iy << 2) + ix) << 1)] = Fv_idx_1; |
244 | } |
245 | } |
246 | |
247 | /* End of MATLAB Function: '<S5>/Embedded MATLAB Function' */ |
248 | |
249 | /* S-Function (cvxfasd): '<S5>/MagMan linearization1' */ |
250 | cvxfasd_Outputs_wrapper(&RaspberryPrediction_B.Ao[0], |
251 | &RaspberryPrediction_P.ReferenceForce_Value[0], |
252 | &RaspberryPrediction_P.ReferenceForce_Value[1], |
253 | &RaspberryPrediction_B.MagManlinearization1_o1[0], |
254 | &RaspberryPrediction_B.MagManlinearization1_o2 ); |
255 | |
256 | /* Math: '<S5>/Math Function' */ |
257 | for (iy = 0; iy < 4; iy++) { |
258 | RaspberryPrediction_B.MathFunction[iy << 2] = |
259 | RaspberryPrediction_B.MagManlinearization1_o1[iy]; |
260 | RaspberryPrediction_B.MathFunction[1 + (iy << 2)] = |
261 | RaspberryPrediction_B.MagManlinearization1_o1[iy + 4]; |
262 | RaspberryPrediction_B.MathFunction[2 + (iy << 2)] = |
263 | RaspberryPrediction_B.MagManlinearization1_o1[iy + 8]; |
264 | RaspberryPrediction_B.MathFunction[3 + (iy << 2)] = |
265 | RaspberryPrediction_B.MagManlinearization1_o1[iy + 12]; |
266 | } |
267 | |
268 | /* End of Math: '<S5>/Math Function' */ |
269 | |
270 | /* MATLAB Function: '<Root>/Currents to PWM1' */ |
271 | /* MATLAB Function 'Currents to PWM': '<S3>:1' */ |
272 | /* conversion from scaling factor to duty cycle of PWM */ |
273 | /* '<S3>:1:3' Currents = max(0, min(1,Currents) ); */ |
274 | /* limits */ |
275 | /* '<S3>:1:4' i = 0.18515*Currents + 0.069587*Currents.^3 + 0.19285*Currents.^(1/3); */ |
276 | /* ratio to current conversion */ |
277 | /* '<S3>:1:5' PWM = i/0.455/2; */ |
278 | /* '<S3>:1:6' PWM(PWM>0.05) = PWM(PWM>0.05) + 0.5; */ |
279 | /* '<S3>:1:7' PWM(PWM<0.5) = 0; */ |
280 | for (iy = 0; iy < 16; iy++) { |
281 | if ((1.0 <= RaspberryPrediction_B.MathFunction[iy]) || rtIsNaN |
282 | (RaspberryPrediction_B.MathFunction[iy])) { |
283 | d = 1.0; |
284 | } else { |
285 | d = RaspberryPrediction_B.MathFunction[iy]; |
286 | } |
287 | |
288 | if ((0.0 >= d) || rtIsNaN(d)) { |
289 | RaspberryPrediction_B.PWM[iy] = 0.0; |
290 | } else { |
291 | RaspberryPrediction_B.PWM[iy] = d; |
292 | } |
293 | |
294 | RaspberryPrediction_B.PWM[iy] = ((0.18515 * RaspberryPrediction_B.PWM[iy] + |
295 | 0.069587 * rt_powd_snf(RaspberryPrediction_B.PWM[iy], 3.0)) + 0.19285 * |
296 | rt_powd_snf(RaspberryPrediction_B.PWM[iy], 0.33333333333333331)) / 0.455 / |
297 | 2.0; |
298 | if (RaspberryPrediction_B.PWM[iy] > 0.05) { |
299 | RaspberryPrediction_B.PWM[iy] += 0.5; |
300 | } |
301 | |
302 | if (RaspberryPrediction_B.PWM[iy] < 0.5) { |
303 | RaspberryPrediction_B.PWM[iy] = 0.0; |
304 | } |
305 | } |
306 | |
307 | /* End of MATLAB Function: '<Root>/Currents to PWM1' */ |
308 | |
309 | /* S-Function (sfcn_setPlatform): '<Root>/MagMan Coils' */ |
310 | sfcn_setPlatform_Outputs_wrapper(&RaspberryPrediction_B.PWM[0], |
311 | &RaspberryPrediction_DW.MagManCoils_DSTATE); |
312 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
313 | /* S-Function (sfcn_getFoilCheck): '<S4>/MagMan Foil with error' */ |
314 | sfcn_getFoilCheck_Outputs_wrapper |
315 | ( &RaspberryPrediction_B.MagManFoilwitherror_o1, |
316 | &RaspberryPrediction_B.MagManFoilwitherror_o2, |
317 | &RaspberryPrediction_B.MagManFoilwitherror_o3, |
318 | &RaspberryPrediction_B.MagManFoilwitherror_o4, |
319 | &RaspberryPrediction_DW.MagManFoilwitherror_DSTATE); |
320 | |
321 | /* MATLAB Function: '<S4>/Position transformation' incorporates: |
322 | * Constant: '<S4>/Sentinel Calibration matrix' |
323 | */ |
324 | Raspber_Positiontransformation1(RaspberryPrediction_B.MagManFoilwitherror_o1, |
325 | RaspberryPrediction_B.MagManFoilwitherror_o2, RaspberryPrediction_P.km4pi, |
326 | &RaspberryPrediction_B.sf_Positiontransformation); |
327 | } |
328 | |
329 | /* S-Function (sfcn_posread_shm): '<S6>/Shared memory camera position' */ |
330 | sfcn_posread_shm_Outputs_wrapper |
331 | ( &RaspberryPrediction_B.Sharedmemorycameraposition_o1, |
332 | &RaspberryPrediction_B.Sharedmemorycameraposition_o2, |
333 | &RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA); |
334 | |
335 | /* DataTypeConversion: '<S6>/Data Type Conversion1' */ |
336 | RaspberryPrediction_B.DataTypeConversion1 = |
337 | RaspberryPrediction_B.Sharedmemorycameraposition_o1; |
338 | |
339 | /* DataTypeConversion: '<S6>/Data Type Conversion2' */ |
340 | RaspberryPrediction_B.DataTypeConversion2 = |
341 | RaspberryPrediction_B.Sharedmemorycameraposition_o2; |
342 | |
343 | /* MATLAB Function: '<S1>/Position transformation1' incorporates: |
344 | * Constant: '<S1>/Sentinel Calibration matrix1' |
345 | */ |
346 | Raspber_Positiontransformation1(RaspberryPrediction_B.DataTypeConversion1, |
347 | RaspberryPrediction_B.DataTypeConversion2, RaspberryPrediction_P.km4piCam, |
348 | &RaspberryPrediction_B.sf_Positiontransformation1); |
349 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
350 | /* Delay: '<Root>/Delay' */ |
351 | RaspberryPrediction_B.Delay[0] = RaspberryPrediction_DW.Delay_DSTATE[0]; |
352 | RaspberryPrediction_B.Delay[1] = RaspberryPrediction_DW.Delay_DSTATE[1]; |
353 | } |
354 | |
355 | /* Sum: '<Root>/Sum1' incorporates: |
356 | * Constant: '<Root>/Reference Force' |
357 | * Gain: '<Root>/L' |
358 | * Sum: '<Root>/Sum' |
359 | */ |
360 | RaspberryPrediction_B.Sum1[0] = RaspberryPrediction_P.ReferenceForce_Value[0] |
361 | - (RaspberryPrediction_B.Delay[0] - |
362 | RaspberryPrediction_B.sf_Positiontransformation1.X) * |
363 | RaspberryPrediction_P.L_Gain; |
364 | RaspberryPrediction_B.Sum1[1] = RaspberryPrediction_P.ReferenceForce_Value[1] |
365 | - (RaspberryPrediction_B.Delay[1] - |
366 | RaspberryPrediction_B.sf_Positiontransformation1.Y) * |
367 | RaspberryPrediction_P.L_Gain; |
368 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
369 | /* S-Function "sfcn_setPlatform_wrapper" Block: <Root>/MagMan Coils */ |
370 | sfcn_setPlatform_Update_wrapper(&RaspberryPrediction_B.PWM[0], |
371 | &RaspberryPrediction_DW.MagManCoils_DSTATE); |
372 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
373 | /* S-Function "sfcn_getFoilCheck_wrapper" Block: <S4>/MagMan Foil with error */ |
374 | sfcn_getFoilCheck_Update_wrapper |
375 | ( &RaspberryPrediction_B.MagManFoilwitherror_o1, |
376 | &RaspberryPrediction_B.MagManFoilwitherror_o2, |
377 | &RaspberryPrediction_B.MagManFoilwitherror_o3, |
378 | &RaspberryPrediction_B.MagManFoilwitherror_o4, |
379 | &RaspberryPrediction_DW.MagManFoilwitherror_DSTATE); |
380 | } |
381 | |
382 | /* S-Function "sfcn_posread_shm_wrapper" Block: <S6>/Shared memory camera position */ |
383 | sfcn_posread_shm_Update_wrapper |
384 | ( &RaspberryPrediction_B.Sharedmemorycameraposition_o1, |
385 | &RaspberryPrediction_B.Sharedmemorycameraposition_o2, |
386 | &RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA); |
387 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
388 | /* Update for Delay: '<Root>/Delay' */ |
389 | RaspberryPrediction_DW.Delay_DSTATE[0] = |
390 | RaspberryPrediction_DW.Delay_DSTATE[2]; |
391 | RaspberryPrediction_DW.Delay_DSTATE[1] = |
392 | RaspberryPrediction_DW.Delay_DSTATE[3]; |
393 | RaspberryPrediction_DW.Delay_DSTATE[2] = |
394 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[0]; |
395 | RaspberryPrediction_DW.Delay_DSTATE[3] = |
396 | RaspberryPrediction_B.Linearmodeloftherollingball2ind[1]; |
397 | } |
398 | |
399 | /* External mode */ |
400 | rtExtModeUploadCheckTrigger(2); |
401 | |
402 | { /* Sample time: [0.0s, 0.0s] */ |
403 | rtExtModeUpload(0, RaspberryPrediction_M->Timing.t[0]); |
404 | } |
405 | |
406 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) {/* Sample time: [0.02s, 0.0s] */ |
407 | rtExtModeUpload(1, (((RaspberryPrediction_M->Timing.clockTick1+ |
408 | RaspberryPrediction_M->Timing.clockTickH1* |
409 | 4294967296.0)) * 0.02)); |
410 | } |
411 | } /* end MajorTimeStep */ |
412 | |
413 | if (rtmIsMajorTimeStep(RaspberryPrediction_M)) { |
414 | /* signal main to stop simulation */ |
415 | { /* Sample time: [0.0s, 0.0s] */ |
416 | if ((rtmGetTFinal(RaspberryPrediction_M)!=-1) && |
417 | !((rtmGetTFinal(RaspberryPrediction_M)- |
418 | (((RaspberryPrediction_M->Timing.clockTick1+ |
419 | RaspberryPrediction_M->Timing.clockTickH1* 4294967296.0)) * 0.02)) |
420 | > (((RaspberryPrediction_M->Timing.clockTick1+ |
421 | RaspberryPrediction_M->Timing.clockTickH1* 4294967296.0)) * |
422 | 0.02) * (DBL_EPSILON))) { |
423 | rtmSetErrorStatus(RaspberryPrediction_M, "Simulation finished"); |
424 | } |
425 | |
426 | if (rtmGetStopRequested(RaspberryPrediction_M)) { |
427 | rtmSetErrorStatus(RaspberryPrediction_M, "Simulation finished"); |
428 | } |
429 | } |
430 | |
431 | rt_ertODEUpdateContinuousStates(&RaspberryPrediction_M->solverInfo); |
432 | |
433 | /* Update absolute time for base rate */ |
434 | /* The "clockTick0" counts the number of times the code of this task has |
435 | * been executed. The absolute time is the multiplication of "clockTick0" |
436 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
437 | * overflow during the application lifespan selected. |
438 | * Timer of this task consists of two 32 bit unsigned integers. |
439 | * The two integers represent the low bits Timing.clockTick0 and the high bits |
440 | * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment. |
441 | */ |
442 | if (!(++RaspberryPrediction_M->Timing.clockTick0)) { |
443 | ++RaspberryPrediction_M->Timing.clockTickH0; |
444 | } |
445 | |
446 | RaspberryPrediction_M->Timing.t[0] = rtsiGetSolverStopTime |
447 | (&RaspberryPrediction_M->solverInfo); |
448 | |
449 | { |
450 | /* Update absolute timer for sample time: [0.02s, 0.0s] */ |
451 | /* The "clockTick1" counts the number of times the code of this task has |
452 | * been executed. The resolution of this integer timer is 0.02, which is the step size |
453 | * of the task. Size of "clockTick1" ensures timer will not overflow during the |
454 | * application lifespan selected. |
455 | * Timer of this task consists of two 32 bit unsigned integers. |
456 | * The two integers represent the low bits Timing.clockTick1 and the high bits |
457 | * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment. |
458 | */ |
459 | RaspberryPrediction_M->Timing.clockTick1++; |
460 | if (!RaspberryPrediction_M->Timing.clockTick1) { |
461 | RaspberryPrediction_M->Timing.clockTickH1++; |
462 | } |
463 | } |
464 | } /* end MajorTimeStep */ |
465 | } |
466 | |
467 | /* Derivatives for root system: '<Root>' */ |
468 | void RaspberryPrediction_derivatives(void) |
469 | { |
470 | XDot_RaspberryPrediction_T *_rtXdot; |
471 | _rtXdot = ((XDot_RaspberryPrediction_T *) RaspberryPrediction_M->derivs); |
472 | |
473 | /* Derivatives for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
474 | _rtXdot->Linearmodeloftherollingball2ind[0] = 0.0; |
475 | _rtXdot->Linearmodeloftherollingball2ind[1] = 0.0; |
476 | _rtXdot->Linearmodeloftherollingball2ind[2] = 0.0; |
477 | _rtXdot->Linearmodeloftherollingball2ind[3] = 0.0; |
478 | _rtXdot->Linearmodeloftherollingball2ind[1] += |
479 | RaspberryPrediction_P.Linearmodeloftherollingball2ind[0] * |
480 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[0]; |
481 | _rtXdot->Linearmodeloftherollingball2ind[3] += |
482 | RaspberryPrediction_P.Linearmodeloftherollingball2ind[1] * |
483 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[2]; |
484 | _rtXdot->Linearmodeloftherollingball2ind[0] += |
485 | RaspberryPrediction_P.Linearmodeloftherollingball2i_l[0] * |
486 | RaspberryPrediction_B.Sum1[0]; |
487 | _rtXdot->Linearmodeloftherollingball2ind[2] += |
488 | RaspberryPrediction_P.Linearmodeloftherollingball2i_l[1] * |
489 | RaspberryPrediction_B.Sum1[1]; |
490 | } |
491 | |
492 | /* Model initialize function */ |
493 | void RaspberryPrediction_initialize(void) |
494 | { |
495 | /* Registration code */ |
496 | |
497 | /* initialize non-finites */ |
498 | rt_InitInfAndNaN(sizeof(real_T)); |
499 | |
500 | /* initialize real-time model */ |
501 | (void) memset((void *)RaspberryPrediction_M, 0, |
502 | sizeof(RT_MODEL_RaspberryPrediction_T)); |
503 | |
504 | { |
505 | /* Setup solver object */ |
506 | rtsiSetSimTimeStepPtr(&RaspberryPrediction_M->solverInfo, |
507 | &RaspberryPrediction_M->Timing.simTimeStep); |
508 | rtsiSetTPtr(&RaspberryPrediction_M->solverInfo, &rtmGetTPtr |
509 | (RaspberryPrediction_M)); |
510 | rtsiSetStepSizePtr(&RaspberryPrediction_M->solverInfo, |
511 | &RaspberryPrediction_M->Timing.stepSize0); |
512 | rtsiSetdXPtr(&RaspberryPrediction_M->solverInfo, |
513 | &RaspberryPrediction_M->derivs); |
514 | rtsiSetContStatesPtr(&RaspberryPrediction_M->solverInfo, (real_T **) |
515 | &RaspberryPrediction_M->contStates); |
516 | rtsiSetNumContStatesPtr(&RaspberryPrediction_M->solverInfo, |
517 | &RaspberryPrediction_M->Sizes.numContStates); |
518 | rtsiSetNumPeriodicContStatesPtr(&RaspberryPrediction_M->solverInfo, |
519 | &RaspberryPrediction_M->Sizes.numPeriodicContStates); |
520 | rtsiSetPeriodicContStateIndicesPtr(&RaspberryPrediction_M->solverInfo, |
521 | &RaspberryPrediction_M->periodicContStateIndices); |
522 | rtsiSetPeriodicContStateRangesPtr(&RaspberryPrediction_M->solverInfo, |
523 | &RaspberryPrediction_M->periodicContStateRanges); |
524 | rtsiSetErrorStatusPtr(&RaspberryPrediction_M->solverInfo, |
525 | (&rtmGetErrorStatus(RaspberryPrediction_M))); |
526 | rtsiSetRTModelPtr(&RaspberryPrediction_M->solverInfo, RaspberryPrediction_M); |
527 | } |
528 | |
529 | rtsiSetSimTimeStep(&RaspberryPrediction_M->solverInfo, MAJOR_TIME_STEP); |
530 | RaspberryPrediction_M->intgData.f[0] = RaspberryPrediction_M->odeF[0]; |
531 | RaspberryPrediction_M->contStates = ((X_RaspberryPrediction_T *) |
532 | &RaspberryPrediction_X); |
533 | rtsiSetSolverData(&RaspberryPrediction_M->solverInfo, (void *) |
534 | &RaspberryPrediction_M->intgData); |
535 | rtsiSetSolverName(&RaspberryPrediction_M->solverInfo,"ode1"); |
536 | rtmSetTPtr(RaspberryPrediction_M, &RaspberryPrediction_M->Timing.tArray[0]); |
537 | rtmSetTFinal(RaspberryPrediction_M, -1); |
538 | RaspberryPrediction_M->Timing.stepSize0 = 0.02; |
539 | |
540 | /* External mode info */ |
541 | RaspberryPrediction_M->Sizes.checksums[0] = (3595132348U); |
542 | RaspberryPrediction_M->Sizes.checksums[1] = (4069635331U); |
543 | RaspberryPrediction_M->Sizes.checksums[2] = (3308004464U); |
544 | RaspberryPrediction_M->Sizes.checksums[3] = (2480287856U); |
545 | |
546 | { |
547 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
548 | static RTWExtModeInfo rt_ExtModeInfo; |
549 | static const sysRanDType *systemRan[5]; |
550 | RaspberryPrediction_M->extModeInfo = (&rt_ExtModeInfo); |
551 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
552 | systemRan[0] = &rtAlwaysEnabled; |
553 | systemRan[1] = &rtAlwaysEnabled; |
554 | systemRan[2] = &rtAlwaysEnabled; |
555 | systemRan[3] = &rtAlwaysEnabled; |
556 | systemRan[4] = &rtAlwaysEnabled; |
557 | rteiSetModelMappingInfoPtr(RaspberryPrediction_M->extModeInfo, |
558 | &RaspberryPrediction_M->SpecialInfo.mappingInfo); |
559 | rteiSetChecksumsPtr(RaspberryPrediction_M->extModeInfo, |
560 | RaspberryPrediction_M->Sizes.checksums); |
561 | rteiSetTPtr(RaspberryPrediction_M->extModeInfo, rtmGetTPtr |
562 | (RaspberryPrediction_M)); |
563 | } |
564 | |
565 | /* block I/O */ |
566 | (void) memset(((void *) &RaspberryPrediction_B), 0, |
567 | sizeof(B_RaspberryPrediction_T)); |
568 | |
569 | /* states (continuous) */ |
570 | { |
571 | (void) memset((void *)&RaspberryPrediction_X, 0, |
572 | sizeof(X_RaspberryPrediction_T)); |
573 | } |
574 | |
575 | /* states (dwork) */ |
576 | (void) memset((void *)&RaspberryPrediction_DW, 0, |
577 | sizeof(DW_RaspberryPrediction_T)); |
578 | |
579 | /* data type transition information */ |
580 | { |
581 | static DataTypeTransInfo dtInfo; |
582 | (void) memset((char_T *) &dtInfo, 0, |
583 | sizeof(dtInfo)); |
584 | RaspberryPrediction_M->SpecialInfo.mappingInfo = (&dtInfo); |
585 | dtInfo.numDataTypes = 14; |
586 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
587 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
588 | |
589 | /* Block I/O transition table */ |
590 | dtInfo.BTransTable = &rtBTransTable; |
591 | |
592 | /* Parameters transition table */ |
593 | dtInfo.PTransTable = &rtPTransTable; |
594 | } |
595 | |
596 | /* InitializeConditions for StateSpace: '<Root>/Linear model of the rolling ball (2 independent double integrators)' */ |
597 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[0] = |
598 | RaspberryPrediction_P.Linearmodeloftherollingball2i_a[0]; |
599 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[1] = |
600 | RaspberryPrediction_P.Linearmodeloftherollingball2i_a[1]; |
601 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[2] = |
602 | RaspberryPrediction_P.Linearmodeloftherollingball2i_a[2]; |
603 | RaspberryPrediction_X.Linearmodeloftherollingball2ind[3] = |
604 | RaspberryPrediction_P.Linearmodeloftherollingball2i_a[3]; |
605 | |
606 | /* S-Function Block: <Root>/MagMan Coils */ |
607 | { |
608 | real_T initVector[1] = { 0 }; |
609 | |
610 | { |
611 | int_T i1; |
612 | for (i1=0; i1 < 1; i1++) { |
613 | RaspberryPrediction_DW.MagManCoils_DSTATE = initVector[0]; |
614 | } |
615 | } |
616 | } |
617 | |
618 | /* S-Function Block: <S4>/MagMan Foil with error */ |
619 | { |
620 | real_T initVector[1] = { 0 }; |
621 | |
622 | { |
623 | int_T i1; |
624 | for (i1=0; i1 < 1; i1++) { |
625 | RaspberryPrediction_DW.MagManFoilwitherror_DSTATE = initVector[0]; |
626 | } |
627 | } |
628 | } |
629 | |
630 | /* S-Function Block: <S6>/Shared memory camera position */ |
631 | { |
632 | real_T initVector[1] = { 0 }; |
633 | |
634 | { |
635 | int_T i1; |
636 | for (i1=0; i1 < 1; i1++) { |
637 | RaspberryPrediction_DW.Sharedmemorycameraposition_DSTA = initVector[0]; |
638 | } |
639 | } |
640 | } |
641 | |
642 | /* InitializeConditions for Delay: '<Root>/Delay' */ |
643 | RaspberryPrediction_DW.Delay_DSTATE[0] = |
644 | RaspberryPrediction_P.Delay_InitialCondition; |
645 | RaspberryPrediction_DW.Delay_DSTATE[1] = |
646 | RaspberryPrediction_P.Delay_InitialCondition; |
647 | RaspberryPrediction_DW.Delay_DSTATE[2] = |
648 | RaspberryPrediction_P.Delay_InitialCondition; |
649 | RaspberryPrediction_DW.Delay_DSTATE[3] = |
650 | RaspberryPrediction_P.Delay_InitialCondition; |
651 | } |
652 | |
653 | /* Model terminate function */ |
654 | void RaspberryPrediction_terminate(void) |
655 | { |
656 | /* (no terminate code required) */ |
657 | } |
658 | |