1 | /* |
2 | * File: sensorsharp.c |
3 | * |
4 | * Code generated for Simulink model 'sensorsharp'. |
5 | * |
6 | * Model version : 1.47 |
7 | * Simulink Coder version : 8.12 (R2017a) 16-Feb-2017 |
8 | * C/C++ source code generated on : Tue Jun 18 18:54:30 2019 |
9 | * |
10 | * Target selection: ert.tlc |
11 | * Embedded hardware selection: Atmel->AVR |
12 | * Code generation objectives: Unspecified |
13 | * Validation result: Not run |
14 | */ |
15 | |
16 | #include "sensorsharp.h" |
17 | #include "sensorsharp_private.h" |
18 | #include "sensorsharp_dt.h" |
19 | |
20 | /* Block signals (auto storage) */ |
21 | B_sensorsharp_T sensorsharp_B; |
22 | |
23 | /* Block states (auto storage) */ |
24 | DW_sensorsharp_T sensorsharp_DW; |
25 | |
26 | /* Real-time model */ |
27 | RT_MODEL_sensorsharp_T sensorsharp_M_; |
28 | RT_MODEL_sensorsharp_T *const sensorsharp_M = &sensorsharp_M_; |
29 | static void rate_monotonic_scheduler(void); |
30 | |
31 | /* |
32 | * Set which subrates need to run this base step (base rate always runs). |
33 | * This function must be called prior to calling the model step function |
34 | * in order to "remember" which rates need to run this base step. The |
35 | * buffering of events allows for overlapping preemption. |
36 | */ |
37 | void sensorsharp_SetEventsForThisBaseStep(boolean_T *eventFlags) |
38 | { |
39 | /* Task runs when its counter is zero, computed via rtmStepTask macro */ |
40 | eventFlags[1] = ((boolean_T)rtmStepTask(sensorsharp_M, 1)); |
41 | } |
42 | |
43 | /* |
44 | * This function updates active task flag for each subrate |
45 | * and rate transition flags for tasks that exchange data. |
46 | * The function assumes rate-monotonic multitasking scheduler. |
47 | * The function must be called at model base rate so that |
48 | * the generated code self-manages all its subrates and rate |
49 | * transition flags. |
50 | */ |
51 | static void rate_monotonic_scheduler(void) |
52 | { |
53 | /* Compute which subrates run during the next base time step. Subrates |
54 | * are an integer multiple of the base rate counter. Therefore, the subtask |
55 | * counter is reset when it reaches its limit (zero means run). |
56 | */ |
57 | (sensorsharp_M->Timing.TaskCounters.TID[1])++; |
58 | if ((sensorsharp_M->Timing.TaskCounters.TID[1]) > 4) {/* Sample time: [0.01s, 0.0s] */ |
59 | sensorsharp_M->Timing.TaskCounters.TID[1] = 0; |
60 | } |
61 | } |
62 | |
63 | /* Model step function for TID0 */ |
64 | void sensorsharp_step0(void) /* Sample time: [0.002s, 0.0s] */ |
65 | { |
66 | { /* Sample time: [0.002s, 0.0s] */ |
67 | rate_monotonic_scheduler(); |
68 | } |
69 | |
70 | /* External mode */ |
71 | rtExtModeUploadCheckTrigger(2); |
72 | rtExtModeUpload(0, sensorsharp_M->Timing.taskTime0); |
73 | |
74 | /* signal main to stop simulation */ |
75 | { /* Sample time: [0.002s, 0.0s] */ |
76 | if ((rtmGetTFinal(sensorsharp_M)!=-1) && |
77 | !((rtmGetTFinal(sensorsharp_M)-sensorsharp_M->Timing.taskTime0) > |
78 | sensorsharp_M->Timing.taskTime0 * (DBL_EPSILON))) { |
79 | rtmSetErrorStatus(sensorsharp_M, "Simulation finished"); |
80 | } |
81 | |
82 | if (rtmGetStopRequested(sensorsharp_M)) { |
83 | rtmSetErrorStatus(sensorsharp_M, "Simulation finished"); |
84 | } |
85 | } |
86 | |
87 | /* Update absolute time */ |
88 | /* The "clockTick0" counts the number of times the code of this task has |
89 | * been executed. The absolute time is the multiplication of "clockTick0" |
90 | * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not |
91 | * overflow during the application lifespan selected. |
92 | */ |
93 | sensorsharp_M->Timing.taskTime0 = |
94 | (++sensorsharp_M->Timing.clockTick0) * sensorsharp_M->Timing.stepSize0; |
95 | } |
96 | |
97 | /* Model step function for TID1 */ |
98 | void sensorsharp_step1(void) /* Sample time: [0.01s, 0.0s] */ |
99 | { |
100 | uint16_T rtb_AnalogImputSensor_0; |
101 | int16_T j; |
102 | int16_T k; |
103 | int16_T a; |
104 | int16_T b_j; |
105 | int16_T b_k; |
106 | real_T rtb_error; |
107 | real_T rtb_TSamp; |
108 | real_T u0; |
109 | uint8_T tmp; |
110 | boolean_T exitg1; |
111 | |
112 | /* MATLAB Function: '<Root>/MATLAB Function 1' incorporates: |
113 | * S-Function (sfix_udelay): '<Root>/Delay' |
114 | */ |
115 | /* MATLAB Function 'MATLAB Function 1': '<S1>:1' */ |
116 | /* '<S1>:1:2' sensorfilter=0.0; */ |
117 | rtb_error = 0.0; |
118 | |
119 | /* sortedValues=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; */ |
120 | /* '<S1>:1:4' sortedValues=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; */ |
121 | memset(&sensorsharp_B.sortedValues[0], 0, 20U * sizeof(real_T)); |
122 | |
123 | /* '<S1>:1:5' for a=1:20 */ |
124 | for (a = 0; a < 20; a++) { |
125 | /* medidas */ |
126 | /* '<S1>:1:6' if(sensor(a)<sortedValues(1) || a==1) */ |
127 | if (sensorsharp_DW.Delay_X[a] < sensorsharp_B.sortedValues[0]) { |
128 | /* '<S1>:1:7' j=1; */ |
129 | j = 0; |
130 | |
131 | /* inserta en la primera posicion */ |
132 | } else if (1 + a == 1) { |
133 | /* '<S1>:1:7' j=1; */ |
134 | j = 0; |
135 | |
136 | /* inserta en la primera posicion */ |
137 | } else { |
138 | /* '<S1>:1:8' else */ |
139 | /* '<S1>:1:9' for j=2:a */ |
140 | j = 1; |
141 | b_j = 0; |
142 | exitg1 = false; |
143 | while ((!exitg1) && (b_j <= a - 1)) { |
144 | j = b_j + 1; |
145 | |
146 | /* '<S1>:1:10' if(sortedValues(j-1)<=sensor(a)&& sortedValues(j)>=sensor(a)) */ |
147 | if ((sensorsharp_B.sortedValues[b_j] <= sensorsharp_DW.Delay_X[a]) && |
148 | (sensorsharp_B.sortedValues[b_j + 1] >= sensorsharp_DW.Delay_X[a])) |
149 | { |
150 | /* j is insert position */ |
151 | exitg1 = true; |
152 | } else { |
153 | b_j++; |
154 | } |
155 | } |
156 | } |
157 | |
158 | /* '<S1>:1:16' for k=a:-1:j+1 */ |
159 | b_j = (int16_T)((real_T)(j - a) / -1.0); |
160 | for (b_k = 0; b_k < b_j; b_k++) { |
161 | k = a - b_k; |
162 | |
163 | /* movemos los valores una posición */ |
164 | /* '<S1>:1:17' sortedValues(k)=sortedValues(k-1); */ |
165 | sensorsharp_B.sortedValues[k] = sensorsharp_B.sortedValues[k - 1]; |
166 | } |
167 | |
168 | /* '<S1>:1:19' sortedValues(j)=sensor(a); */ |
169 | sensorsharp_B.sortedValues[j] = sensorsharp_DW.Delay_X[a]; |
170 | |
171 | /* se inserta lectura actual */ |
172 | } |
173 | |
174 | /* '<S1>:1:21' for a=8:12 */ |
175 | for (a = 0; a < 5; a++) { |
176 | /* hacemos media de los intermedios */ |
177 | /* '<S1>:1:22' sensorfilter=sensorfilter+sortedValues(a); */ |
178 | rtb_error += sensorsharp_B.sortedValues[a + 7]; |
179 | } |
180 | |
181 | /* '<S1>:1:24' sensorfilter=sensorfilter/5; */ |
182 | rtb_error /= 5.0; |
183 | |
184 | /* MATLAB Function: '<Root>/MATLAB Function 2' incorporates: |
185 | * MATLAB Function: '<Root>/MATLAB Function 1' |
186 | */ |
187 | /* MATLAB Function 'MATLAB Function 2': '<S2>:1' */ |
188 | /* '<S2>:1:3' poscm=-0.1138*posv+77.958; */ |
189 | sensorsharp_B.poscm = -0.1138 * rtb_error + 77.958; |
190 | |
191 | /* DataTypeConversion: '<Root>/Data Type Conversion1' */ |
192 | rtb_error = floor(sensorsharp_B.poscm); |
193 | if (rtIsNaN(rtb_error) || rtIsInf(rtb_error)) { |
194 | rtb_error = 0.0; |
195 | } else { |
196 | rtb_error = fmod(rtb_error, 65536.0); |
197 | } |
198 | |
199 | sensorsharp_B.DataTypeConversion1 = rtb_error < 0.0 ? (uint16_T)-(int16_T) |
200 | (uint16_T)-rtb_error : (uint16_T)rtb_error; |
201 | |
202 | /* End of DataTypeConversion: '<Root>/Data Type Conversion1' */ |
203 | |
204 | /* S-Function (Lcd): '<Root>/S-Function Builder' */ |
205 | Lcd_Outputs_wrapper(&sensorsharp_B.DataTypeConversion1, |
206 | &sensorsharp_DW.SFunctionBuilder_DSTATE); |
207 | |
208 | /* Constant: '<Root>/Constant2' */ |
209 | sensorsharp_B.Constant2 = sensorsharp_P.Constant2_Value; |
210 | |
211 | /* Sum: '<Root>/Sum1' */ |
212 | rtb_error = sensorsharp_B.Constant2 - sensorsharp_B.poscm; |
213 | |
214 | /* SampleTimeMath: '<S7>/TSamp' incorporates: |
215 | * Gain: '<S4>/Derivative Gain' |
216 | * |
217 | * About '<S7>/TSamp': |
218 | * y = u * K where K = 1 / ( w * Ts ) |
219 | */ |
220 | rtb_TSamp = sensorsharp_P.PIDController_D * rtb_error * |
221 | sensorsharp_P.TSamp_WtEt; |
222 | |
223 | /* Sum: '<S4>/Sum' incorporates: |
224 | * Delay: '<S7>/UD' |
225 | * DiscreteIntegrator: '<S4>/Integrator' |
226 | * Gain: '<S4>/Proportional Gain' |
227 | * Sum: '<S7>/Diff' |
228 | */ |
229 | sensorsharp_B.Sum = (sensorsharp_P.PIDController_P * rtb_error + |
230 | sensorsharp_DW.Integrator_DSTATE) + (rtb_TSamp - |
231 | sensorsharp_DW.UD_DSTATE); |
232 | |
233 | /* MATLAB Function: '<Root>/MATLAB Function 3' */ |
234 | /* MATLAB Function 'MATLAB Function 3': '<S3>:1' */ |
235 | /* '<S3>:1:3' grados = ((pid - (-180))*(180 - 0))/(180 - (-180)) + 0; */ |
236 | u0 = (sensorsharp_B.Sum - -180.0) * 180.0 / 360.0; |
237 | |
238 | /* Saturate: '<Root>/Saturation' */ |
239 | if (u0 > sensorsharp_P.Saturation_UpperSat) { |
240 | sensorsharp_B.Saturation = sensorsharp_P.Saturation_UpperSat; |
241 | } else if (u0 < sensorsharp_P.Saturation_LowerSat) { |
242 | sensorsharp_B.Saturation = sensorsharp_P.Saturation_LowerSat; |
243 | } else { |
244 | sensorsharp_B.Saturation = u0; |
245 | } |
246 | |
247 | /* End of Saturate: '<Root>/Saturation' */ |
248 | /* DataTypeConversion: '<S6>/Data Type Conversion' */ |
249 | if (sensorsharp_B.Saturation < 256.0) { |
250 | if (sensorsharp_B.Saturation >= 0.0) { |
251 | tmp = (uint8_T)sensorsharp_B.Saturation; |
252 | } else { |
253 | tmp = 0U; |
254 | } |
255 | } else { |
256 | tmp = MAX_uint8_T; |
257 | } |
258 | |
259 | /* End of DataTypeConversion: '<S6>/Data Type Conversion' */ |
260 | |
261 | /* S-Function (arduinoservowrite_sfcn): '<S6>/Servo Write' */ |
262 | MW_servoWrite(sensorsharp_P.ServoWrite_p1, tmp); |
263 | |
264 | /* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Imput Sensor' */ |
265 | rtb_AnalogImputSensor_0 = MW_analogRead(sensorsharp_P.AnalogImputSensor_p1); |
266 | |
267 | /* DataTypeConversion: '<S5>/Data Type Conversion' incorporates: |
268 | * Constant: '<Root>/Constant1' |
269 | */ |
270 | if (sensorsharp_P.Constant1_Value < 256.0) { |
271 | if (sensorsharp_P.Constant1_Value >= 0.0) { |
272 | tmp = (uint8_T)sensorsharp_P.Constant1_Value; |
273 | } else { |
274 | tmp = 0U; |
275 | } |
276 | } else { |
277 | tmp = MAX_uint8_T; |
278 | } |
279 | |
280 | /* End of DataTypeConversion: '<S5>/Data Type Conversion' */ |
281 | |
282 | /* S-Function (arduinoanalogoutput_sfcn): '<S5>/PWM' */ |
283 | MW_analogWrite(sensorsharp_P.PWM_pinNumber, tmp); |
284 | |
285 | /* Update for S-Function (sfix_udelay): '<Root>/Delay' incorporates: |
286 | * DataTypeConversion: '<Root>/Data Type Conversion3' |
287 | * S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Imput Sensor' |
288 | */ |
289 | for (a = 0; a < 19; a++) { |
290 | sensorsharp_DW.Delay_X[a] = sensorsharp_DW.Delay_X[a + 1]; |
291 | } |
292 | |
293 | sensorsharp_DW.Delay_X[19] = rtb_AnalogImputSensor_0; |
294 | |
295 | /* End of Update for S-Function (sfix_udelay): '<Root>/Delay' */ |
296 | |
297 | /* Update for S-Function (Lcd): '<Root>/S-Function Builder' */ |
298 | |
299 | /* S-Function "Lcd_wrapper" Block: <Root>/S-Function Builder */ |
300 | Lcd_Update_wrapper(&sensorsharp_B.DataTypeConversion1, |
301 | &sensorsharp_DW.SFunctionBuilder_DSTATE); |
302 | |
303 | /* Update for DiscreteIntegrator: '<S4>/Integrator' incorporates: |
304 | * Gain: '<S4>/Integral Gain' |
305 | */ |
306 | sensorsharp_DW.Integrator_DSTATE += sensorsharp_P.PIDController_I * rtb_error * |
307 | sensorsharp_P.Integrator_gainval; |
308 | |
309 | /* Update for Delay: '<S7>/UD' */ |
310 | sensorsharp_DW.UD_DSTATE = rtb_TSamp; |
311 | rtExtModeUpload(1, ((sensorsharp_M->Timing.clockTick1) * 0.01)); |
312 | |
313 | /* Update absolute time */ |
314 | /* The "clockTick1" counts the number of times the code of this task has |
315 | * been executed. The resolution of this integer timer is 0.01, which is the step size |
316 | * of the task. Size of "clockTick1" ensures timer will not overflow during the |
317 | * application lifespan selected. |
318 | */ |
319 | sensorsharp_M->Timing.clockTick1++; |
320 | } |
321 | |
322 | /* Model initialize function */ |
323 | void sensorsharp_initialize(void) |
324 | { |
325 | /* Registration code */ |
326 | |
327 | /* initialize non-finites */ |
328 | rt_InitInfAndNaN(sizeof(real_T)); |
329 | |
330 | /* initialize real-time model */ |
331 | (void) memset((void *)sensorsharp_M, 0, |
332 | sizeof(RT_MODEL_sensorsharp_T)); |
333 | rtmSetTFinal(sensorsharp_M, -1); |
334 | sensorsharp_M->Timing.stepSize0 = 0.002; |
335 | |
336 | /* External mode info */ |
337 | sensorsharp_M->Sizes.checksums[0] = (1368376521U); |
338 | sensorsharp_M->Sizes.checksums[1] = (2124501209U); |
339 | sensorsharp_M->Sizes.checksums[2] = (2996990022U); |
340 | sensorsharp_M->Sizes.checksums[3] = (827725802U); |
341 | |
342 | { |
343 | static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE; |
344 | static RTWExtModeInfo rt_ExtModeInfo; |
345 | static const sysRanDType *systemRan[4]; |
346 | sensorsharp_M->extModeInfo = (&rt_ExtModeInfo); |
347 | rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan); |
348 | systemRan[0] = &rtAlwaysEnabled; |
349 | systemRan[1] = &rtAlwaysEnabled; |
350 | systemRan[2] = &rtAlwaysEnabled; |
351 | systemRan[3] = &rtAlwaysEnabled; |
352 | rteiSetModelMappingInfoPtr(sensorsharp_M->extModeInfo, |
353 | &sensorsharp_M->SpecialInfo.mappingInfo); |
354 | rteiSetChecksumsPtr(sensorsharp_M->extModeInfo, |
355 | sensorsharp_M->Sizes.checksums); |
356 | rteiSetTPtr(sensorsharp_M->extModeInfo, rtmGetTPtr(sensorsharp_M)); |
357 | } |
358 | |
359 | /* block I/O */ |
360 | (void) memset(((void *) &sensorsharp_B), 0, |
361 | sizeof(B_sensorsharp_T)); |
362 | |
363 | /* states (dwork) */ |
364 | (void) memset((void *)&sensorsharp_DW, 0, |
365 | sizeof(DW_sensorsharp_T)); |
366 | |
367 | /* data type transition information */ |
368 | { |
369 | static DataTypeTransInfo dtInfo; |
370 | (void) memset((char_T *) &dtInfo, 0, |
371 | sizeof(dtInfo)); |
372 | sensorsharp_M->SpecialInfo.mappingInfo = (&dtInfo); |
373 | dtInfo.numDataTypes = 14; |
374 | dtInfo.dataTypeSizes = &rtDataTypeSizes[0]; |
375 | dtInfo.dataTypeNames = &rtDataTypeNames[0]; |
376 | |
377 | /* Block I/O transition table */ |
378 | dtInfo.BTransTable = &rtBTransTable; |
379 | |
380 | /* Parameters transition table */ |
381 | dtInfo.PTransTable = &rtPTransTable; |
382 | } |
383 | |
384 | { |
385 | int16_T i; |
386 | |
387 | /* Start for S-Function (arduinoservowrite_sfcn): '<S6>/Servo Write' */ |
388 | MW_servoAttach(sensorsharp_P.ServoWrite_p1, |
389 | sensorsharp_P.ServoWrite_pinNumber); |
390 | |
391 | /* Start for S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Imput Sensor' */ |
392 | MW_pinModeAnalogInput(sensorsharp_P.AnalogImputSensor_p1); |
393 | |
394 | /* Start for S-Function (arduinoanalogoutput_sfcn): '<S5>/PWM' */ |
395 | MW_pinModeOutput(sensorsharp_P.PWM_pinNumber); |
396 | |
397 | /* InitializeConditions for S-Function (sfix_udelay): '<Root>/Delay' */ |
398 | for (i = 0; i < 20; i++) { |
399 | sensorsharp_DW.Delay_X[i] = sensorsharp_P.Delay_vinit; |
400 | } |
401 | |
402 | /* End of InitializeConditions for S-Function (sfix_udelay): '<Root>/Delay' */ |
403 | |
404 | /* InitializeConditions for S-Function (Lcd): '<Root>/S-Function Builder' */ |
405 | |
406 | /* S-Function Block: <Root>/S-Function Builder */ |
407 | { |
408 | real_T initVector[1] = { 0 }; |
409 | |
410 | { |
411 | int_T i1; |
412 | for (i1=0; i1 < 1; i1++) { |
413 | sensorsharp_DW.SFunctionBuilder_DSTATE = initVector[0]; |
414 | } |
415 | } |
416 | } |
417 | |
418 | /* InitializeConditions for DiscreteIntegrator: '<S4>/Integrator' */ |
419 | sensorsharp_DW.Integrator_DSTATE = sensorsharp_P.Integrator_IC; |
420 | |
421 | /* InitializeConditions for Delay: '<S7>/UD' */ |
422 | sensorsharp_DW.UD_DSTATE = sensorsharp_P.UD_InitialCondition; |
423 | } |
424 | } |
425 | |
426 | /* Model terminate function */ |
427 | void sensorsharp_terminate(void) |
428 | { |
429 | /* (no terminate code required) */ |
430 | } |
431 | |
432 | /* |
433 | * File trailer for generated code. |
434 | * |
435 | * [EOF] |
436 | */ |
437 | |