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) */
21B_sensorsharp_T sensorsharp_B;
22
23/* Block states (auto storage) */
24DW_sensorsharp_T sensorsharp_DW;
25
26/* Real-time model */
27RT_MODEL_sensorsharp_T sensorsharp_M_;
28RT_MODEL_sensorsharp_T *const sensorsharp_M = &sensorsharp_M_;
29static 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 */
37void 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 */
51static 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 */
64void 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 */
98void 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 */
323void 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 */
427void sensorsharp_terminate(void)
428{
429 /* (no terminate code required) */
430}
431
432/*
433 * File trailer for generated code.
434 *
435 * [EOF]
436 */
437