1/*
2 * File: EV3Control_sil_sil_ec.c
3 *
4 * Code generated for Simulink model 'EV3Control_sil_sil_ec'.
5 *
6 * Model version : 1.2
7 * Simulink Coder version : 9.0 (R2018b) 24-May-2018
8 * C/C++ source code generated on : Tue Oct 29 09:31:45 2019
9 *
10 * Target selection: ert.tlc
11 * Embedded hardware selection: Intel->x86-64 (Windows64)
12 * Code generation objectives:
13 * 1. RAM efficiency
14 * 2. Execution efficiency
15 * 3. ROM efficiency
16 * Validation result: Not run
17 */
18
19#include "EV3Control_sil_sil_ec.h"
20#include "look1_binlx.h"
21
22/* Named constants for Chart: '<S9>/Chart' */
23#define EV3Control_IN_VehicleControlOff ((uint8_T)2U)
24#define EV3Control__IN_VehicleControlOn ((uint8_T)3U)
25#define EV3Control_s_IN_ControlledDrive ((uint8_T)1U)
26#define EV3Control_s_IN_NO_ACTIVE_CHILD ((uint8_T)0U)
27#define EV3Control_sil_IN_EmergencyStop ((uint8_T)1U)
28#define EV3Control_sil_s_IN_ManualDrive ((uint8_T)2U)
29#define EV3Control_sil_s_IN_VehicleIdle ((uint8_T)4U)
30#define EV3Control_sil_sil_ec_IN_FWD ((uint8_T)1U)
31#define EV3Control_sil_sil_ec_IN_REV ((uint8_T)2U)
32#ifndef PORTABLE_WORDSIZES
33#ifndef UCHAR_MAX
34#include <limits.h>
35#endif
36
37#if ( UCHAR_MAX != (0xFFU) ) || ( SCHAR_MAX != (0x7F) )
38#error Code was generated for compiler with different sized uchar/char. \
39Consider adjusting Test hardware word size settings on the \
40Hardware Implementation pane to match your compiler word sizes as \
41defined in limits.h of the compiler. Alternatively, you can \
42select the Test hardware is the same as production hardware option and \
43select the Enable portable word sizes option on the Code Generation > \
44Verification pane for ERT based targets, which will disable the \
45preprocessor word size checks.
46#endif
47
48#if ( USHRT_MAX != (0xFFFFU) ) || ( SHRT_MAX != (0x7FFF) )
49#error Code was generated for compiler with different sized ushort/short. \
50Consider adjusting Test hardware word size settings on the \
51Hardware Implementation pane to match your compiler word sizes as \
52defined in limits.h of the compiler. Alternatively, you can \
53select the Test hardware is the same as production hardware option and \
54select the Enable portable word sizes option on the Code Generation > \
55Verification pane for ERT based targets, which will disable the \
56preprocessor word size checks.
57#endif
58
59#if ( UINT_MAX != (0xFFFFFFFFU) ) || ( INT_MAX != (0x7FFFFFFF) )
60#error Code was generated for compiler with different sized uint/int. \
61Consider adjusting Test hardware word size settings on the \
62Hardware Implementation pane to match your compiler word sizes as \
63defined in limits.h of the compiler. Alternatively, you can \
64select the Test hardware is the same as production hardware option and \
65select the Enable portable word sizes option on the Code Generation > \
66Verification pane for ERT based targets, which will disable the \
67preprocessor word size checks.
68#endif
69
70#if ( ULONG_MAX != (0xFFFFFFFFU) ) || ( LONG_MAX != (0x7FFFFFFF) )
71#error Code was generated for compiler with different sized ulong/long. \
72Consider adjusting Test hardware word size settings on the \
73Hardware Implementation pane to match your compiler word sizes as \
74defined in limits.h of the compiler. Alternatively, you can \
75select the Test hardware is the same as production hardware option and \
76select the Enable portable word sizes option on the Code Generation > \
77Verification pane for ERT based targets, which will disable the \
78preprocessor word size checks.
79#endif
80
81/* Skipping ulong_long/long_long check: insufficient preprocessor integer range. */
82#endif /* PORTABLE_WORDSIZES */
83
84/* Block signals and states (default storage) */
85DW_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_DW;
86
87/* External inputs (root inport signals with default storage) */
88ExtU_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_U;
89
90/* External outputs (root outports fed by signals with default storage) */
91ExtY_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_Y;
92
93/* Real-time model */
94RT_MODEL_EV3Control_sil_sil_e_T EV3Control_sil_sil_ec_M_;
95RT_MODEL_EV3Control_sil_sil_e_T *const EV3Control_sil_sil_ec_M =
96 &EV3Control_sil_sil_ec_M_;
97static void EV3_HonkIfObstacleDetected_Init(real_T *rty_speaker, real_T
98 *rty_speaker_g, P_HonkIfObstacleDetected_EV3C_T *localP);
99static void EV3Contr_HonkIfObstacleDetected(real_T rtu_frequency, real_T
100 rtu_volume, real_T *rty_speaker, real_T *rty_speaker_g);
101
102/*
103 * System initialize for action system:
104 * '<S17>/HonkIfObstacleDetected'
105 * '<S17>/DoNotHonk'
106 */
107static void EV3_HonkIfObstacleDetected_Init(real_T *rty_speaker, real_T
108 *rty_speaker_g, P_HonkIfObstacleDetected_EV3C_T *localP)
109{
110 /* SystemInitialize for Outport: '<S23>/speaker' */
111 *rty_speaker = localP->speaker_Y0;
112 *rty_speaker_g = localP->speaker_Y0;
113}
114
115/*
116 * Output and update for action system:
117 * '<S17>/HonkIfObstacleDetected'
118 * '<S17>/DoNotHonk'
119 */
120static void EV3Contr_HonkIfObstacleDetected(real_T rtu_frequency, real_T
121 rtu_volume, real_T *rty_speaker, real_T *rty_speaker_g)
122{
123 /* SignalConversion: '<S23>/BusConversion_InsertedFor_speaker_at_inport_0' */
124 *rty_speaker = rtu_frequency;
125
126 /* SignalConversion: '<S23>/BusConversion_InsertedFor_speaker_at_inport_0' */
127 *rty_speaker_g = rtu_volume;
128}
129
130/* Model step function */
131void EV3Control_sil_sil_ec_step(void)
132{
133 /* local block i/o variables */
134 real_T rtb_gyroAngleRad;
135 int32_T rtb_driveEncoder;
136 int32_T rtb_steeringEncoder;
137 uint8_T rtb_usSensorFront;
138 uint8_T rtb_usSensorRight;
139 uint8_T rtb_usSensorLeft;
140 int8_T rtb_remoteCtrlCmd;
141 int8_T rtb_steeringMotor;
142 int8_T rtb_driveMotor;
143 boolean_T rtb_ObstacleDetection_o1;
144 boolean_T rtb_obstacleDetection;
145 int32_T k;
146 boolean_T rtb_autopilot;
147 uint8_T rtb_statusLight;
148 int8_T rtb_remoteControl;
149 int8_T steeringMotor;
150 uint16_T statusLight;
151 int32_T r1;
152 real_T rtb_rotMat_tmp;
153 boolean_T exitg1;
154
155 /* Gain: '<S4>/Gain1' incorporates:
156 * Inport: '<Root>/sensorBusRaw'
157 */
158 rtb_driveEncoder = (int32_T)(((int64_T)EV3Control_sil_sil_ec_P.Gain1_Gain_a *
159 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_DriveMotor) >> 30);
160
161 /* Product: '<S4>/conv2rad' incorporates:
162 * Constant: '<S4>/deg2rad'
163 * DataTypeConversion: '<S4>/Data Type Conversion'
164 * Inport: '<Root>/sensorBusRaw'
165 */
166 rtb_gyroAngleRad = (real_T)
167 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_Gyro * rtP_DEG2RAD;
168
169 /* ModelReference: '<S11>/GlobalPosition' incorporates:
170 * Outport: '<Root>/posGlobal'
171 */
172 GlobalPosition_demo_ec(&rtb_driveEncoder, &rtb_gyroAngleRad,
173 &EV3Control_sil_sil_ec_Y.posGlobal[0],
174 &(EV3Control_sil_sil_ec_DW.GlobalPosition_InstanceData.rtdw));
175
176 /* Gain: '<S4>/Gain5' incorporates:
177 * Inport: '<Root>/sensorBusRaw'
178 */
179 rtb_remoteCtrlCmd = (int8_T)((EV3Control_sil_sil_ec_P.Gain5_Gain *
180 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_RemoteControl) >> 6);
181
182 /* DataTypeConversion: '<S5>/Data Type Conversion' */
183 rtb_steeringMotor = rtb_remoteCtrlCmd;
184
185 /* Switch: '<S14>/holdSwitch' incorporates:
186 * DataTypeConversion: '<S14>/DataTypeConversion'
187 * UnitDelay: '<S14>/Unit Delay'
188 */
189 if (rtb_steeringMotor != 0) {
190 rtb_remoteControl = rtb_steeringMotor;
191 } else {
192 rtb_remoteControl = EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE_p;
193 }
194
195 /* End of Switch: '<S14>/holdSwitch' */
196
197 /* Gain: '<S4>/Gain2' incorporates:
198 * Inport: '<Root>/sensorBusRaw'
199 */
200 rtb_usSensorFront = (uint8_T)(((uint32_T)EV3Control_sil_sil_ec_P.Gain2_Gain *
201 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_UltrasoundFront) >> 7);
202
203 /* DataTypeConversion: '<S5>/Data Type Conversion1' */
204 rtb_driveMotor = (int8_T)rtb_usSensorFront;
205
206 /* ModelReference: '<S5>/ObstacleDetection' */
207 ObstacleDetection_demo_ec(&rtb_steeringMotor, &rtb_driveMotor,
208 &rtb_ObstacleDetection_o1, &rtb_obstacleDetection,
209 &(EV3Control_sil_sil_ec_DW.ObstacleDetection_InstanceData.rtdw));
210
211 /* UnitDelay: '<S8>/Unit Delay' */
212 EV3Control_sil_sil_ec_DW.UnitDelay = EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE;
213
214 /* UnitDelay: '<S8>/Unit Delay1' */
215 EV3Control_sil_sil_ec_DW.UnitDelay1 =
216 EV3Control_sil_sil_ec_DW.UnitDelay1_DSTATE;
217
218 /* Chart: '<S9>/Chart' incorporates:
219 * Constant: '<S8>/Constant'
220 * Logic: '<S8>/Logical Operator1'
221 * RelationalOperator: '<S8>/Relational Operator'
222 * RelationalOperator: '<S8>/Relational Operator1'
223 * RelationalOperator: '<S8>/Relational Operator2'
224 * RelationalOperator: '<S8>/Relational Operator3'
225 * UnitDelay: '<S8>/Unit Delay'
226 * UnitDelay: '<S8>/Unit Delay1'
227 * UnitDelay: '<S8>/Unit Delay2'
228 */
229 /* Gateway: EV3Control_sil_sil_ec/VehicleManager/ManageVehicleStates/Chart */
230 /* During: EV3Control_sil_sil_ec/VehicleManager/ManageVehicleStates/Chart */
231 if (EV3Control_sil_sil_ec_DW.is_active_c6_EV3Control_sil_sil == 0U) {
232 /* Entry: EV3Control_sil_sil_ec/VehicleManager/ManageVehicleStates/Chart */
233 EV3Control_sil_sil_ec_DW.is_active_c6_EV3Control_sil_sil = 1U;
234
235 /* Entry Internal: EV3Control_sil_sil_ec/VehicleManager/ManageVehicleStates/Chart */
236 /* Transition: '<S13>:4' */
237 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
238 EV3Control_IN_VehicleControlOff;
239 EV3Control_sil_sil_ec_DW.VehicleControlOff = true;
240
241 /* Entry 'VehicleControlOff': '<S13>:3' */
242 /* '<S13>:3:1' gear = 0; */
243 EV3Control_sil_sil_ec_DW.gear = 0.0;
244
245 /* '<S13>:3:1' autopilot = false; */
246 rtb_autopilot = false;
247 } else {
248 switch (EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec) {
249 case EV3Control_sil_IN_EmergencyStop:
250 rtb_autopilot = false;
251
252 /* During 'EmergencyStop': '<S13>:34' */
253 /* '<S13>:35:1' sf_internal_predicateOutput = remoteControl == REMOTE_STOP; */
254 if (rtb_remoteControl == rtP_REMOTE_STOP) {
255 /* Transition: '<S13>:35' */
256 EV3Control_sil_sil_ec_DW.EmergencyStop = false;
257 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
258 EV3Control_IN_VehicleControlOff;
259 EV3Control_sil_sil_ec_DW.VehicleControlOff = true;
260
261 /* Entry 'VehicleControlOff': '<S13>:3' */
262 /* '<S13>:3:1' gear = 0; */
263 EV3Control_sil_sil_ec_DW.gear = 0.0;
264
265 /* '<S13>:3:1' autopilot = false; */
266 } else {
267 /* '<S13>:56:1' sf_internal_predicateOutput = remoteControl == REMOTE_DRIVE_REV; */
268 if (rtb_remoteControl == rtP_REMOTE_DRIVE_REV) {
269 /* Transition: '<S13>:56' */
270 EV3Control_sil_sil_ec_DW.EmergencyStop = false;
271 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
272 EV3Control__IN_VehicleControlOn;
273 EV3Control_sil_sil_ec_DW.VehicleControlOn = true;
274
275 /* Entry Internal 'VehicleControlOn': '<S13>:5' */
276 /* Transition: '<S13>:47' */
277 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
278 EV3Control_sil_s_IN_ManualDrive;
279
280 /* Entry 'ManualDrive': '<S13>:37' */
281 /* '<S13>:37:1' gear = 0; */
282 /* '<S13>:37:1' autopilot = false; */
283 /* Entry Internal 'ManualDrive': '<S13>:37' */
284 /* Transition: '<S13>:61' */
285 EV3Control_sil_sil_ec_DW.is_ManualDrive = EV3Control_sil_sil_ec_IN_FWD;
286 EV3Control_sil_sil_ec_DW.tp_FWD = 1U;
287
288 /* Entry 'FWD': '<S13>:62' */
289 /* '<S13>:62:1' gear = 1; */
290 EV3Control_sil_sil_ec_DW.gear = 1.0;
291 }
292 }
293 break;
294
295 case EV3Control_IN_VehicleControlOff:
296 rtb_autopilot = false;
297
298 /* During 'VehicleControlOff': '<S13>:3' */
299 /* '<S13>:50:1' sf_internal_predicateOutput = remoteControl == REMOTE_START; */
300 if (rtb_remoteControl == rtP_REMOTE_START) {
301 /* Transition: '<S13>:50' */
302 EV3Control_sil_sil_ec_DW.VehicleControlOff = false;
303 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
304 EV3Control__IN_VehicleControlOn;
305 EV3Control_sil_sil_ec_DW.VehicleControlOn = true;
306
307 /* Entry Internal 'VehicleControlOn': '<S13>:5' */
308 /* Transition: '<S13>:47' */
309 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
310 EV3Control_sil_s_IN_ManualDrive;
311
312 /* Entry 'ManualDrive': '<S13>:37' */
313 /* '<S13>:37:1' gear = 0; */
314 /* '<S13>:37:1' autopilot = false; */
315 /* Entry Internal 'ManualDrive': '<S13>:37' */
316 /* Transition: '<S13>:61' */
317 EV3Control_sil_sil_ec_DW.is_ManualDrive = EV3Control_sil_sil_ec_IN_FWD;
318 EV3Control_sil_sil_ec_DW.tp_FWD = 1U;
319
320 /* Entry 'FWD': '<S13>:62' */
321 /* '<S13>:62:1' gear = 1; */
322 EV3Control_sil_sil_ec_DW.gear = 1.0;
323 } else {
324 /* '<S13>:8:1' sf_internal_predicateOutput = gyroReady == true && ... */
325 /* '<S13>:8:1' remoteControl ~= REMOTE_STOP; */
326 if ((EV3Control_sil_sil_ec_P.Constant_Value_k == rtb_gyroAngleRad) &&
327 (rtb_gyroAngleRad == EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE) &&
328 (rtb_gyroAngleRad == EV3Control_sil_sil_ec_DW.UnitDelay1_DSTATE) &&
329 (rtb_gyroAngleRad == EV3Control_sil_sil_ec_DW.UnitDelay2_DSTATE) &&
330 (rtb_remoteControl != rtP_REMOTE_STOP)) {
331 /* Transition: '<S13>:8' */
332 EV3Control_sil_sil_ec_DW.VehicleControlOff = false;
333 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
334 EV3Control_sil_s_IN_VehicleIdle;
335 EV3Control_sil_sil_ec_DW.VehicleIdle = true;
336
337 /* Entry 'VehicleIdle': '<S13>:29' */
338 /* '<S13>:29:1' gear = 0; */
339 EV3Control_sil_sil_ec_DW.gear = 0.0;
340
341 /* '<S13>:29:1' autopilot = false; */
342 }
343 }
344 break;
345
346 case EV3Control__IN_VehicleControlOn:
347 /* During 'VehicleControlOn': '<S13>:5' */
348 /* '<S13>:9:1' sf_internal_predicateOutput = remoteControl == REMOTE_STOP; */
349 if (rtb_remoteControl == rtP_REMOTE_STOP) {
350 /* Transition: '<S13>:9' */
351 /* Exit Internal 'VehicleControlOn': '<S13>:5' */
352 EV3Control_sil_sil_ec_DW.tp_ControlledDrive = 0U;
353
354 /* Exit Internal 'ManualDrive': '<S13>:37' */
355 EV3Control_sil_sil_ec_DW.tp_FWD = 0U;
356 EV3Control_sil_sil_ec_DW.tp_REV = 0U;
357 EV3Control_sil_sil_ec_DW.is_ManualDrive =
358 EV3Control_s_IN_NO_ACTIVE_CHILD;
359 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
360 EV3Control_s_IN_NO_ACTIVE_CHILD;
361 EV3Control_sil_sil_ec_DW.VehicleControlOn = false;
362 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
363 EV3Control_IN_VehicleControlOff;
364 EV3Control_sil_sil_ec_DW.VehicleControlOff = true;
365
366 /* Entry 'VehicleControlOff': '<S13>:3' */
367 /* '<S13>:3:1' gear = 0; */
368 EV3Control_sil_sil_ec_DW.gear = 0.0;
369
370 /* '<S13>:3:1' autopilot = false; */
371 rtb_autopilot = false;
372 } else {
373 /* '<S13>:21:1' sf_internal_predicateOutput = emergencyStopObstacle == true && ... */
374 /* '<S13>:21:1' remoteControl ~= REMOTE_DRIVE_REV; */
375 if (rtb_ObstacleDetection_o1 && (rtb_remoteControl !=
376 rtP_REMOTE_DRIVE_REV)) {
377 /* Transition: '<S13>:21' */
378 /* Exit Internal 'VehicleControlOn': '<S13>:5' */
379 EV3Control_sil_sil_ec_DW.tp_ControlledDrive = 0U;
380
381 /* Exit Internal 'ManualDrive': '<S13>:37' */
382 EV3Control_sil_sil_ec_DW.tp_FWD = 0U;
383 EV3Control_sil_sil_ec_DW.tp_REV = 0U;
384 EV3Control_sil_sil_ec_DW.is_ManualDrive =
385 EV3Control_s_IN_NO_ACTIVE_CHILD;
386 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
387 EV3Control_s_IN_NO_ACTIVE_CHILD;
388 EV3Control_sil_sil_ec_DW.VehicleControlOn = false;
389 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
390 EV3Control_sil_IN_EmergencyStop;
391 EV3Control_sil_sil_ec_DW.EmergencyStop = true;
392
393 /* Entry 'EmergencyStop': '<S13>:34' */
394 /* '<S13>:34:1' gear = 0; */
395 EV3Control_sil_sil_ec_DW.gear = 0.0;
396
397 /* '<S13>:34:1' autopilot = false; */
398 rtb_autopilot = false;
399 } else if (EV3Control_sil_sil_ec_DW.is_VehicleControlOn ==
400 EV3Control_s_IN_ControlledDrive) {
401 rtb_autopilot = true;
402
403 /* During 'ControlledDrive': '<S13>:45' */
404 /* '<S13>:52:1' sf_internal_predicateOutput = remoteControl == REMOTE_DRIVE_AUTO_OFF; */
405 if (rtb_remoteControl == rtP_REMOTE_DRIVE_AUTO_OFF) {
406 /* Transition: '<S13>:52' */
407 EV3Control_sil_sil_ec_DW.tp_ControlledDrive = 0U;
408 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
409 EV3Control_sil_s_IN_ManualDrive;
410
411 /* Entry 'ManualDrive': '<S13>:37' */
412 /* '<S13>:37:1' gear = 0; */
413 /* '<S13>:37:1' autopilot = false; */
414 rtb_autopilot = false;
415
416 /* Entry Internal 'ManualDrive': '<S13>:37' */
417 /* Transition: '<S13>:61' */
418 EV3Control_sil_sil_ec_DW.is_ManualDrive =
419 EV3Control_sil_sil_ec_IN_FWD;
420 EV3Control_sil_sil_ec_DW.tp_FWD = 1U;
421
422 /* Entry 'FWD': '<S13>:62' */
423 /* '<S13>:62:1' gear = 1; */
424 EV3Control_sil_sil_ec_DW.gear = 1.0;
425 }
426 } else {
427 rtb_autopilot = false;
428
429 /* During 'ManualDrive': '<S13>:37' */
430 /* '<S13>:51:1' sf_internal_predicateOutput = remoteControl == REMOTE_DRIVE_AUTO_ON; */
431 if (rtb_remoteControl == rtP_REMOTE_DRIVE_AUTO_ON) {
432 /* Transition: '<S13>:51' */
433 /* Exit Internal 'ManualDrive': '<S13>:37' */
434 EV3Control_sil_sil_ec_DW.tp_FWD = 0U;
435 EV3Control_sil_sil_ec_DW.tp_REV = 0U;
436 EV3Control_sil_sil_ec_DW.is_ManualDrive =
437 EV3Control_s_IN_NO_ACTIVE_CHILD;
438 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
439 EV3Control_s_IN_ControlledDrive;
440 EV3Control_sil_sil_ec_DW.tp_ControlledDrive = 1U;
441
442 /* Entry 'ControlledDrive': '<S13>:45' */
443 /* '<S13>:45:1' gear = 1; */
444 EV3Control_sil_sil_ec_DW.gear = 1.0;
445
446 /* '<S13>:45:1' autopilot = true; */
447 rtb_autopilot = true;
448 } else if (EV3Control_sil_sil_ec_DW.is_ManualDrive ==
449 EV3Control_sil_sil_ec_IN_FWD) {
450 /* During 'FWD': '<S13>:62' */
451 /* '<S13>:64:1' sf_internal_predicateOutput = remoteControl == REMOTE_DRIVE_REV; */
452 if (rtb_remoteControl == rtP_REMOTE_DRIVE_REV) {
453 /* Transition: '<S13>:64' */
454 EV3Control_sil_sil_ec_DW.tp_FWD = 0U;
455 EV3Control_sil_sil_ec_DW.is_ManualDrive =
456 EV3Control_sil_sil_ec_IN_REV;
457 EV3Control_sil_sil_ec_DW.tp_REV = 1U;
458
459 /* Entry 'REV': '<S13>:63' */
460 /* '<S13>:63:1' gear = -1; */
461 EV3Control_sil_sil_ec_DW.gear = -1.0;
462 }
463 } else {
464 /* During 'REV': '<S13>:63' */
465 /* '<S13>:60:1' sf_internal_predicateOutput = remoteControl == REMOTE_DRIVE_FWD; */
466 if (rtb_remoteControl == rtP_REMOTE_DRIVE_FWD) {
467 /* Transition: '<S13>:60' */
468 EV3Control_sil_sil_ec_DW.tp_REV = 0U;
469 EV3Control_sil_sil_ec_DW.is_ManualDrive =
470 EV3Control_sil_sil_ec_IN_FWD;
471 EV3Control_sil_sil_ec_DW.tp_FWD = 1U;
472
473 /* Entry 'FWD': '<S13>:62' */
474 /* '<S13>:62:1' gear = 1; */
475 EV3Control_sil_sil_ec_DW.gear = 1.0;
476 }
477 }
478 }
479 }
480 break;
481
482 default:
483 rtb_autopilot = false;
484
485 /* During 'VehicleIdle': '<S13>:29' */
486 /* '<S13>:31:1' sf_internal_predicateOutput = remoteControl == REMOTE_START; */
487 if (rtb_remoteControl == rtP_REMOTE_START) {
488 /* Transition: '<S13>:31' */
489 EV3Control_sil_sil_ec_DW.VehicleIdle = false;
490 EV3Control_sil_sil_ec_DW.is_c6_EV3Control_sil_sil_ec =
491 EV3Control__IN_VehicleControlOn;
492 EV3Control_sil_sil_ec_DW.VehicleControlOn = true;
493
494 /* Entry Internal 'VehicleControlOn': '<S13>:5' */
495 /* Transition: '<S13>:47' */
496 EV3Control_sil_sil_ec_DW.is_VehicleControlOn =
497 EV3Control_sil_s_IN_ManualDrive;
498
499 /* Entry 'ManualDrive': '<S13>:37' */
500 /* '<S13>:37:1' gear = 0; */
501 /* '<S13>:37:1' autopilot = false; */
502 /* Entry Internal 'ManualDrive': '<S13>:37' */
503 /* Transition: '<S13>:61' */
504 EV3Control_sil_sil_ec_DW.is_ManualDrive = EV3Control_sil_sil_ec_IN_FWD;
505 EV3Control_sil_sil_ec_DW.tp_FWD = 1U;
506
507 /* Entry 'FWD': '<S13>:62' */
508 /* '<S13>:62:1' gear = 1; */
509 EV3Control_sil_sil_ec_DW.gear = 1.0;
510 }
511 break;
512 }
513 }
514
515 /* End of Chart: '<S9>/Chart' */
516
517 /* Gain: '<S4>/Gain' incorporates:
518 * Inport: '<Root>/sensorBusRaw'
519 */
520 rtb_steeringEncoder = (int32_T)(((int64_T)EV3Control_sil_sil_ec_P.Gain_Gain_a *
521 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_SteeringMotor) >> 30);
522
523 /* If: '<S20>/IfStrnCtrl' incorporates:
524 * Constant: '<S39>/Constant'
525 * Constant: '<S39>/Constant1'
526 * Constant: '<S39>/Constant2'
527 * Constant: '<S85>/Constant2'
528 * Constant: '<S86>/Constant2'
529 * Constant: '<S87>/Constant2'
530 * RelationalOperator: '<S39>/Relational Operator'
531 * RelationalOperator: '<S39>/Relational Operator1'
532 * RelationalOperator: '<S39>/Relational Operator2'
533 */
534 if (rtb_autopilot) {
535 /* Outputs for IfAction SubSystem: '<S20>/AutoSteering' incorporates:
536 * ActionPort: '<S38>/Action Port'
537 */
538 /* MATLAB Function: '<S51>/MATLAB_Function' */
539 /* MATLAB Function 'EV3Control_sil_sil_ec/VehicleManager/VehicleControl/SteeringControl/AutoSteering/PathFollower/RotationMatrix/MATLAB_Function': '<S65>:1' */
540 /* '<S65>:1:3' rotMat = [cos(gyroAngleRad) -sin(gyroAngleRad); ... */
541 /* '<S65>:1:4' sin(gyroAngleRad) cos(gyroAngleRad)]; */
542 rtb_rotMat_tmp = cos(rtb_gyroAngleRad);
543 EV3Control_sil_sil_ec_DW.rotMat[0] = rtb_rotMat_tmp;
544 EV3Control_sil_sil_ec_DW.rtb_rotMat_tmp = sin(rtb_gyroAngleRad);
545 EV3Control_sil_sil_ec_DW.rotMat[2] =
546 -EV3Control_sil_sil_ec_DW.rtb_rotMat_tmp;
547 EV3Control_sil_sil_ec_DW.rotMat[1] = EV3Control_sil_sil_ec_DW.rtb_rotMat_tmp;
548 EV3Control_sil_sil_ec_DW.rotMat[3] = rtb_rotMat_tmp;
549
550 /* MATLAB Function: '<S67>/GoalPointGlobal' incorporates:
551 * Constant: '<S67>/Constant2'
552 * Outport: '<Root>/posGlobal'
553 */
554 /* MATLAB Function 'EV3Control_sil_sil_ec/VehicleManager/VehicleControl/SteeringControl/AutoSteering/PathPlanner/GetNextGoalPointGlobal/GoalPointGlobal': '<S70>:1' */
555 /* '<S70>:1:3' distances = sqrt( (pathCoords(:,1) - posGlobal(1,1)).^2 + (pathCoords(:,2) - posGlobal(2,1)).^2 ); */
556 for (r1 = 0; r1 < 51; r1++) {
557 EV3Control_sil_sil_ec_DW.volume_m = rtP_PATH_COORDS_XY[r1] -
558 EV3Control_sil_sil_ec_Y.posGlobal[0];
559 EV3Control_sil_sil_ec_DW.distances_c = EV3Control_sil_sil_ec_DW.volume_m *
560 EV3Control_sil_sil_ec_DW.volume_m;
561 EV3Control_sil_sil_ec_DW.volume_m = rtP_PATH_COORDS_XY[51 + r1] -
562 EV3Control_sil_sil_ec_Y.posGlobal[1];
563 EV3Control_sil_sil_ec_DW.distances_c += EV3Control_sil_sil_ec_DW.volume_m *
564 EV3Control_sil_sil_ec_DW.volume_m;
565 EV3Control_sil_sil_ec_DW.distances[r1] = sqrt
566 (EV3Control_sil_sil_ec_DW.distances_c);
567 }
568
569 /* '<S70>:1:4' [~, j] = min(distances); */
570 if (!rtIsNaN(EV3Control_sil_sil_ec_DW.distances[0])) {
571 r1 = 0;
572 } else {
573 r1 = -1;
574 k = 2;
575 exitg1 = false;
576 while ((!exitg1) && (k < 52)) {
577 if (!rtIsNaN(EV3Control_sil_sil_ec_DW.distances[k - 1])) {
578 r1 = k - 1;
579 exitg1 = true;
580 } else {
581 k++;
582 }
583 }
584 }
585
586 if (r1 + 1 == 0) {
587 r1 = 0;
588 } else {
589 EV3Control_sil_sil_ec_DW.volume_m = EV3Control_sil_sil_ec_DW.distances[r1];
590 for (k = r1 + 1; k + 1 < 52; k++) {
591 if (EV3Control_sil_sil_ec_DW.volume_m >
592 EV3Control_sil_sil_ec_DW.distances[k]) {
593 EV3Control_sil_sil_ec_DW.volume_m =
594 EV3Control_sil_sil_ec_DW.distances[k];
595 r1 = k;
596 }
597 }
598 }
599
600 /* '<S70>:1:4' ~ */
601 /* j equals index to closest waypoint */
602 /* Move up the path from closest waypoint */
603 /* '<S70>:1:7' distToNext = inf; */
604 EV3Control_sil_sil_ec_DW.volume_m = (rtInf);
605
606 /* '<S70>:1:8' k = j + 20; */
607 k = r1 + 20;
608
609 /* '<S70>:1:9' while distToNext > lookahead */
610 exitg1 = false;
611 while ((!exitg1) && (EV3Control_sil_sil_ec_DW.volume_m >
612 rtP_CTRL_TRK_LOOKAHEAD)) {
613 /* '<S70>:1:10' if k > size(pathCoords,1) */
614 if (k + 1 > 51) {
615 /* '<S70>:1:11' k = size(pathCoords,1); */
616 k = 50;
617 }
618
619 /* '<S70>:1:13' k = k - 1; */
620 k--;
621
622 /* '<S70>:1:14' if k <= 0 */
623 if (k + 1 <= 0) {
624 /* '<S70>:1:15' disp(k) */
625 /* '<S70>:1:16' k = j; */
626 k = r1;
627 exitg1 = true;
628 } else {
629 /* '<S70>:1:19' distToNext = sqrt( (pathCoords(k,1) - posGlobal(1)).^2 + (pathCoords(k,2) - posGlobal(2)).^2 ); */
630 EV3Control_sil_sil_ec_DW.volume_m = rtP_PATH_COORDS_XY[k] -
631 EV3Control_sil_sil_ec_Y.posGlobal[0];
632 EV3Control_sil_sil_ec_DW.distances_c = rtP_PATH_COORDS_XY[51 + k] -
633 EV3Control_sil_sil_ec_Y.posGlobal[1];
634 EV3Control_sil_sil_ec_DW.volume_m = sqrt
635 (EV3Control_sil_sil_ec_DW.volume_m * EV3Control_sil_sil_ec_DW.volume_m
636 + EV3Control_sil_sil_ec_DW.distances_c *
637 EV3Control_sil_sil_ec_DW.distances_c);
638 }
639 }
640
641 /* If: '<S69>/IfObstacle' */
642 /* Goal point in global coordinates */
643 /* '<S70>:1:23' goalPointGlobal = pathCoords(k,:)'; */
644 /* '<S70>:1:24' goalPointGlobal = goalPointGlobal(:); */
645 if (rtb_obstacleDetection) {
646 /* Outputs for IfAction SubSystem: '<S69>/LateralShift' incorporates:
647 * ActionPort: '<S72>/Action Port'
648 */
649 /* Signum: '<S72>/Sign' incorporates:
650 * Constant: '<S67>/Constant2'
651 * MATLAB Function: '<S67>/GoalPointGlobal'
652 * Sum: '<S72>/Add'
653 */
654 EV3Control_sil_sil_ec_DW.volume_m = rtP_PATH_COORDS_XY[51 + k];
655 if (EV3Control_sil_sil_ec_DW.volume_m < 0.0) {
656 EV3Control_sil_sil_ec_DW.distances_c = -1.0;
657 } else if (EV3Control_sil_sil_ec_DW.volume_m > 0.0) {
658 EV3Control_sil_sil_ec_DW.distances_c = 1.0;
659 } else if (EV3Control_sil_sil_ec_DW.volume_m == 0.0) {
660 EV3Control_sil_sil_ec_DW.distances_c = 0.0;
661 } else {
662 EV3Control_sil_sil_ec_DW.distances_c = (rtNaN);
663 }
664
665 /* SignalConversion: '<S72>/Signal Conversion' incorporates:
666 * Constant: '<S67>/Constant2'
667 * Constant: '<S72>/Constant2'
668 * MATLAB Function: '<S67>/GoalPointGlobal'
669 * Product: '<S72>/Product'
670 * Signum: '<S72>/Sign'
671 * Sum: '<S72>/Add'
672 */
673 EV3Control_sil_sil_ec_DW.goalPointGlobal[0] = rtP_PATH_COORDS_XY[k];
674 EV3Control_sil_sil_ec_DW.goalPointGlobal[1] =
675 EV3Control_sil_sil_ec_DW.volume_m + EV3Control_sil_sil_ec_DW.distances_c
676 * rtP_LATERAL_EVADE_INCREMENT;
677
678 /* End of Outputs for SubSystem: '<S69>/LateralShift' */
679 } else {
680 /* Outputs for IfAction SubSystem: '<S69>/NoShift' incorporates:
681 * ActionPort: '<S74>/Action Port'
682 */
683 /* Gain: '<S74>/Gain' incorporates:
684 * Constant: '<S67>/Constant2'
685 * MATLAB Function: '<S67>/GoalPointGlobal'
686 */
687 EV3Control_sil_sil_ec_DW.goalPointGlobal[0] =
688 EV3Control_sil_sil_ec_P.Gain_Gain_f * rtP_PATH_COORDS_XY[k];
689 EV3Control_sil_sil_ec_DW.goalPointGlobal[1] = rtP_PATH_COORDS_XY[k + 51] *
690 EV3Control_sil_sil_ec_P.Gain_Gain_f;
691
692 /* End of Outputs for SubSystem: '<S69>/NoShift' */
693 }
694
695 /* End of If: '<S69>/IfObstacle' */
696
697 /* If: '<S43>/IfTrkMethod' incorporates:
698 * Constant: '<S43>/Constant1'
699 * Constant: '<S49>/Constant'
700 * MATLAB Function: '<S60>/TransformGoalPointToLocal'
701 * Outport: '<Root>/posGlobal'
702 */
703 if (rtP_CTRL_TRK_METHOD) {
704 /* Outputs for IfAction SubSystem: '<S43>/PurePursuitAlgorithm' incorporates:
705 * ActionPort: '<S50>/Action Port'
706 */
707 /* Goal point in vehicle coordinates */
708 /* MATLAB Function 'EV3Control_sil_sil_ec/VehicleManager/VehicleControl/SteeringControl/AutoSteering/PathFollower/PurePursuitAlgorithm/GetGoalPointLocal/TransformGoalPointToLocal': '<S64>:1' */
709 /* '<S64>:1:4' goalPointLocal = rotMat \ (goalPointGlobal - posGlobal); */
710 EV3Control_sil_sil_ec_DW.goalPointGlobal[0] -=
711 EV3Control_sil_sil_ec_Y.posGlobal[0];
712 EV3Control_sil_sil_ec_DW.goalPointGlobal[1] -=
713 EV3Control_sil_sil_ec_Y.posGlobal[1];
714
715 /* MATLAB Function: '<S60>/TransformGoalPointToLocal' incorporates:
716 * Outport: '<Root>/posGlobal'
717 */
718 if (fabs(EV3Control_sil_sil_ec_DW.rtb_rotMat_tmp) > fabs(rtb_rotMat_tmp))
719 {
720 r1 = 1;
721 k = 0;
722 } else {
723 r1 = 0;
724 k = 1;
725 }
726
727 EV3Control_sil_sil_ec_DW.volume_m = EV3Control_sil_sil_ec_DW.rotMat[k] /
728 EV3Control_sil_sil_ec_DW.rotMat[r1];
729
730 /* Product: '<S59>/Divide1' incorporates:
731 * Constant: '<S59>/Constant1'
732 * Constant: '<S59>/Constant2'
733 * Gain: '<S59>/Gain'
734 * MATLAB Function: '<S60>/TransformGoalPointToLocal'
735 * Math: '<S59>/Math Function'
736 * Math: '<S59>/Math Function1'
737 * Product: '<S59>/Divide'
738 *
739 * About '<S59>/Math Function1':
740 * Operator: reciprocal
741 */
742 /* equal to: inv(rotMat) * (goalpoint_g - poscar_g) */
743 rtb_rotMat_tmp = rtP_WHEEL_BASE / (1.0 /
744 ((EV3Control_sil_sil_ec_DW.goalPointGlobal[k] -
745 EV3Control_sil_sil_ec_DW.goalPointGlobal[r1] *
746 EV3Control_sil_sil_ec_DW.volume_m) / (EV3Control_sil_sil_ec_DW.rotMat
747 [2 + k] - EV3Control_sil_sil_ec_DW.rotMat[2 + r1] *
748 EV3Control_sil_sil_ec_DW.volume_m) * EV3Control_sil_sil_ec_P.Gain_Gain /
749 (rtP_CTRL_TRK_LOOKAHEAD * rtP_CTRL_TRK_LOOKAHEAD)));
750
751 /* Trigonometry: '<S59>/Trigonometric Function' */
752 if (rtb_rotMat_tmp > 1.0) {
753 rtb_rotMat_tmp = 1.0;
754 } else {
755 if (rtb_rotMat_tmp < -1.0) {
756 rtb_rotMat_tmp = -1.0;
757 }
758 }
759
760 EV3Control_sil_sil_ec_DW.SteeringAngleCommand = asin(rtb_rotMat_tmp);
761
762 /* End of Trigonometry: '<S59>/Trigonometric Function' */
763 /* End of Outputs for SubSystem: '<S43>/PurePursuitAlgorithm' */
764 } else if (!rtP_CTRL_TRK_METHOD) {
765 /* Outputs for IfAction SubSystem: '<S43>/NoPathFollowing' incorporates:
766 * ActionPort: '<S49>/Action Port'
767 */
768 EV3Control_sil_sil_ec_DW.SteeringAngleCommand =
769 EV3Control_sil_sil_ec_P.Constant_Value;
770
771 /* End of Outputs for SubSystem: '<S43>/NoPathFollowing' */
772 } else {
773 /* '<S56>:1:12' targetHeadingRad = (gyroAngleRad + deltaHeadingRad); */
774 /* MATLAB Function 'EV3Control_sil_sil_ec/VehicleManager/VehicleControl/SteeringControl/AutoSteering/PathFollower/FollowTheCarrotAlgorithm/GetGoalPointLocal/FollowTheCarrotAlgorithm': '<S56>:1' */
775 /* '<S56>:1:3' xaxisCar_g = rotMat * [1; 0]; */
776 /* '<S56>:1:5' vectorToGoalpoint_g = goalPointGlobal - posGlobal; */
777 /* '<S56>:1:7' crossProd = cross([xaxisCar_g;0], [vectorToGoalpoint_g;0]); */
778 /* '<S56>:1:9' deltaHeadingRad = acos( dot(xaxisCar_g, vectorToGoalpoint_g) / (norm(xaxisCar_g) * norm(vectorToGoalpoint_g)) ) ... */
779 /* '<S56>:1:10' * sign(crossProd(3)); */
780 }
781
782 /* End of If: '<S43>/IfTrkMethod' */
783
784 /* Saturate: '<S43>/Saturation' */
785 if (EV3Control_sil_sil_ec_DW.SteeringAngleCommand > rtP_STEER_ANGLE_MAX_RAD)
786 {
787 /* Outport: '<Root>/steeringAngleCmdRad' */
788 EV3Control_sil_sil_ec_Y.steeringAngleCmdRad = rtP_STEER_ANGLE_MAX_RAD;
789 } else if (EV3Control_sil_sil_ec_DW.SteeringAngleCommand <
790 rtP_STEER_ANGLE_MIN_RAD) {
791 /* Outport: '<Root>/steeringAngleCmdRad' */
792 EV3Control_sil_sil_ec_Y.steeringAngleCmdRad = rtP_STEER_ANGLE_MIN_RAD;
793 } else {
794 /* Outport: '<Root>/steeringAngleCmdRad' */
795 EV3Control_sil_sil_ec_Y.steeringAngleCmdRad =
796 EV3Control_sil_sil_ec_DW.SteeringAngleCommand;
797 }
798
799 /* End of Saturate: '<S43>/Saturation' */
800
801 /* Sum: '<S45>/Sum' incorporates:
802 * DataTypeConversion: '<S41>/Data Type Conversion'
803 * Lookup_n-D: '<S41>/1-D Lookup Table'
804 * Outport: '<Root>/steeringAngleCmdRad'
805 */
806 EV3Control_sil_sil_ec_DW.volume_m =
807 EV3Control_sil_sil_ec_Y.steeringAngleCmdRad - look1_binlx
808 (rtb_steeringEncoder, rtP_CL_EST_STEER_ENC_DEG, rtP_CL_EST_STEER_ANGLE_RAD,
809 3U);
810
811 /* If: '<S45>/IfStrnCmd' incorporates:
812 * Constant: '<S45>/Constant'
813 * Constant: '<S45>/Constant1'
814 * Constant: '<S77>/Constant2'
815 * Constant: '<S78>/Constant2'
816 * Constant: '<S80>/Constant2'
817 * Gain: '<S45>/Gain'
818 * RelationalOperator: '<S45>/Relational Operator'
819 * RelationalOperator: '<S45>/Relational Operator1'
820 */
821 if (EV3Control_sil_sil_ec_DW.volume_m >
822 rtP_CTRL_TRK_STEER_ANGLE_DEADZONE_RAD) {
823 /* Outputs for IfAction SubSystem: '<S45>/AutoSteerLeft' incorporates:
824 * ActionPort: '<S77>/Action Port'
825 */
826 steeringMotor = rtP_MOTOR_STEER_AUTO_LEFT;
827
828 /* End of Outputs for SubSystem: '<S45>/AutoSteerLeft' */
829 } else if (EV3Control_sil_sil_ec_DW.volume_m <
830 EV3Control_sil_sil_ec_P.Gain_Gain_k *
831 rtP_CTRL_TRK_STEER_ANGLE_DEADZONE_RAD) {
832 /* Outputs for IfAction SubSystem: '<S45>/AutoSteerRight' incorporates:
833 * ActionPort: '<S78>/Action Port'
834 */
835 steeringMotor = rtP_MOTOR_STEER_AUTO_RIGHT;
836
837 /* End of Outputs for SubSystem: '<S45>/AutoSteerRight' */
838 } else {
839 /* Outputs for IfAction SubSystem: '<S45>/NoSteer' incorporates:
840 * ActionPort: '<S80>/Action Port'
841 */
842 steeringMotor = EV3Control_sil_sil_ec_P.Constant2_Value_n;
843
844 /* End of Outputs for SubSystem: '<S45>/NoSteer' */
845 }
846
847 /* End of If: '<S45>/IfStrnCmd' */
848
849 /* Gain: '<S45>/Gain1' */
850 steeringMotor = (int8_T)floor(EV3Control_sil_sil_ec_P.Gain1_Gain * (real_T)
851 steeringMotor);
852
853 /* End of Outputs for SubSystem: '<S20>/AutoSteering' */
854 } else {
855 /* Outputs for IfAction SubSystem: '<S20>/ManualSteering' incorporates:
856 * ActionPort: '<S39>/Action Port'
857 */
858 /* Outputs for Enabled SubSystem: '<S39>/SteerLeft' incorporates:
859 * EnablePort: '<S86>/Enable'
860 */
861 if (rtb_remoteCtrlCmd == rtP_REMOTE_STEER_LEFT) {
862 EV3Control_sil_sil_ec_DW.Merge_o = rtP_MOTOR_STEER_MANUAL_LEFT;
863 }
864
865 /* End of Outputs for SubSystem: '<S39>/SteerLeft' */
866
867 /* Outputs for Enabled SubSystem: '<S39>/SteerRight' incorporates:
868 * EnablePort: '<S87>/Enable'
869 */
870 if (rtb_remoteCtrlCmd == rtP_REMOTE_STEER_RIGHT) {
871 EV3Control_sil_sil_ec_DW.Merge_o = rtP_MOTOR_STEER_MANUAL_RIGHT;
872 }
873
874 /* End of Outputs for SubSystem: '<S39>/SteerRight' */
875
876 /* Outputs for Enabled SubSystem: '<S39>/NoSteer' incorporates:
877 * EnablePort: '<S85>/Enable'
878 */
879 if (rtb_remoteCtrlCmd == EV3Control_sil_sil_ec_P.Constant2_Value_kp) {
880 EV3Control_sil_sil_ec_DW.Merge_o =
881 EV3Control_sil_sil_ec_P.Constant2_Value_h;
882 }
883
884 /* End of Outputs for SubSystem: '<S39>/NoSteer' */
885
886 /* Saturate: '<S39>/Saturation' incorporates:
887 * Constant: '<S39>/Constant1'
888 * Constant: '<S39>/Constant2'
889 * Constant: '<S85>/Constant2'
890 * Constant: '<S86>/Constant2'
891 * Constant: '<S87>/Constant2'
892 * RelationalOperator: '<S39>/Relational Operator1'
893 * RelationalOperator: '<S39>/Relational Operator2'
894 */
895 if (EV3Control_sil_sil_ec_DW.Merge_o > rtP_MOTOR_STEER_MAX) {
896 steeringMotor = rtP_MOTOR_STEER_MAX;
897 } else if (EV3Control_sil_sil_ec_DW.Merge_o < rtP_MOTOR_STEER_MIN) {
898 steeringMotor = rtP_MOTOR_STEER_MIN;
899 } else {
900 steeringMotor = EV3Control_sil_sil_ec_DW.Merge_o;
901 }
902
903 /* End of Saturate: '<S39>/Saturation' */
904 /* End of Outputs for SubSystem: '<S20>/ManualSteering' */
905 }
906
907 /* End of If: '<S20>/IfStrnCtrl' */
908
909 /* BusCreator: '<S9>/Bus Creator' incorporates:
910 * Outport: '<Root>/VehicleStates'
911 */
912 EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleControlOn =
913 EV3Control_sil_sil_ec_DW.VehicleControlOn;
914 EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleControlOff =
915 EV3Control_sil_sil_ec_DW.VehicleControlOff;
916 EV3Control_sil_sil_ec_Y.VehicleStates_m.EmergencyStop =
917 EV3Control_sil_sil_ec_DW.EmergencyStop;
918 EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleIdle =
919 EV3Control_sil_sil_ec_DW.VehicleIdle;
920
921 /* Outputs for Enabled SubSystem: '<S21>/EnabledDriveController' incorporates:
922 * EnablePort: '<S91>/VehicleControlOn'
923 */
924 /* Outport: '<Root>/VehicleStates' */
925 if (EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleControlOn) {
926 /* Switch: '<S91>/motorSwitch' incorporates:
927 * Constant: '<S91>/Constant1'
928 * Constant: '<S91>/Constant2'
929 * Constant: '<S94>/Constant'
930 * RelationalOperator: '<S94>/Compare'
931 */
932 if (EV3Control_sil_sil_ec_DW.gear >=
933 EV3Control_sil_sil_ec_P.Constant_Value_d) {
934 EV3Control_sil_sil_ec_DW.Merge = rtP_MOTOR_DRIVE_DEFAULT_FWD;
935 } else {
936 EV3Control_sil_sil_ec_DW.Merge = rtP_MOTOR_DRIVE_DEFAULT_REV;
937 }
938
939 /* End of Switch: '<S91>/motorSwitch' */
940 }
941
942 /* End of Outputs for SubSystem: '<S21>/EnabledDriveController' */
943
944 /* Outputs for Enabled SubSystem: '<S21>/NoInput' incorporates:
945 * EnablePort: '<S93>/Enable'
946 */
947 /* Logic: '<S21>/Logical Operator' incorporates:
948 * Constant: '<S93>/Constant1'
949 * Outport: '<Root>/VehicleStates'
950 */
951 if (!EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleControlOn) {
952 EV3Control_sil_sil_ec_DW.Merge = EV3Control_sil_sil_ec_P.Constant1_Value_c;
953 }
954
955 /* End of Logic: '<S21>/Logical Operator' */
956 /* End of Outputs for SubSystem: '<S21>/NoInput' */
957
958 /* Saturate: '<S21>/Saturation' */
959 if (EV3Control_sil_sil_ec_DW.Merge > rtP_MOTOR_DRIVE_MAX) {
960 rtb_driveMotor = rtP_MOTOR_DRIVE_MAX;
961 } else if (EV3Control_sil_sil_ec_DW.Merge < rtP_MOTOR_DRIVE_MIN) {
962 rtb_driveMotor = rtP_MOTOR_DRIVE_MIN;
963 } else {
964 rtb_driveMotor = EV3Control_sil_sil_ec_DW.Merge;
965 }
966
967 /* End of Saturate: '<S21>/Saturation' */
968
969 /* Switch: '<S2>/Switch4' incorporates:
970 * Constant: '<S2>/Constant'
971 * Constant: '<S2>/Constant1'
972 * Outport: '<Root>/VehicleStates'
973 * Switch: '<S2>/Switch1'
974 */
975 if (EV3Control_sil_sil_ec_Y.VehicleStates_m.EmergencyStop) {
976 rtb_driveMotor = EV3Control_sil_sil_ec_P.Constant_Value_a;
977 rtb_steeringMotor = EV3Control_sil_sil_ec_P.Constant1_Value_g;
978 } else {
979 rtb_steeringMotor = steeringMotor;
980 }
981
982 /* End of Switch: '<S2>/Switch4' */
983
984 /* If: '<S17>/IfObstacleDetected' incorporates:
985 * Constant: '<S17>/Constant1'
986 * Constant: '<S17>/Constant2'
987 * Constant: '<S17>/Constant3'
988 */
989 if (rtb_obstacleDetection) {
990 /* Outputs for IfAction SubSystem: '<S17>/HonkIfObstacleDetected' incorporates:
991 * ActionPort: '<S23>/Action Port'
992 */
993 EV3Contr_HonkIfObstacleDetected(EV3Control_sil_sil_ec_P.Constant2_Value_k,
994 EV3Control_sil_sil_ec_P.Constant3_Value,
995 &EV3Control_sil_sil_ec_DW.frequency_c, &EV3Control_sil_sil_ec_DW.volume_k);
996
997 /* End of Outputs for SubSystem: '<S17>/HonkIfObstacleDetected' */
998 } else {
999 /* Outputs for IfAction SubSystem: '<S17>/DoNotHonk' incorporates:
1000 * ActionPort: '<S22>/Action Port'
1001 */
1002 EV3Contr_HonkIfObstacleDetected(EV3Control_sil_sil_ec_P.Constant2_Value_k,
1003 EV3Control_sil_sil_ec_P.Constant1_Value,
1004 &EV3Control_sil_sil_ec_DW.frequency_c, &EV3Control_sil_sil_ec_DW.volume_k);
1005
1006 /* End of Outputs for SubSystem: '<S17>/DoNotHonk' */
1007 }
1008
1009 /* End of If: '<S17>/IfObstacleDetected' */
1010
1011 /* Outputs for Atomic SubSystem: '<S11>/StatusLightControl' */
1012 /* If: '<S19>/IfLightInput' incorporates:
1013 * Constant: '<S2>/Constant2'
1014 * Outport: '<Root>/VehicleStates'
1015 * Switch: '<S2>/Switch2'
1016 */
1017 if (EV3Control_sil_sil_ec_Y.VehicleStates_m.EmergencyStop) {
1018 EV3Control_sil_sil_ec_DW.volume_m = EV3Control_sil_sil_ec_P.Constant2_Value;
1019
1020 /* Outputs for IfAction SubSystem: '<S19>/StatusLightEmergency' incorporates:
1021 * ActionPort: '<S30>/Action Port'
1022 */
1023 /* Gain: '<S30>/Gain' incorporates:
1024 * Constant: '<S19>/Constant4'
1025 * Constant: '<S2>/Constant2'
1026 */
1027 statusLight = (uint16_T)((uint32_T)EV3Control_sil_sil_ec_P.Gain_Gain_j *
1028 rtP_STATUS_LIGHT_EMERGENCY_STOP);
1029
1030 /* End of Outputs for SubSystem: '<S19>/StatusLightEmergency' */
1031 } else {
1032 EV3Control_sil_sil_ec_DW.volume_m = EV3Control_sil_sil_ec_DW.volume_k;
1033 if (rtb_obstacleDetection) {
1034 /* Outputs for IfAction SubSystem: '<S19>/StatusLightObstacle' incorporates:
1035 * ActionPort: '<S32>/Action Port'
1036 */
1037 /* Gain: '<S32>/Gain' incorporates:
1038 * Constant: '<S19>/Constant1'
1039 */
1040 statusLight = (uint16_T)((uint32_T)EV3Control_sil_sil_ec_P.Gain_Gain_b *
1041 rtP_STATUS_LIGHT_OBSTACLE_DETECTED);
1042
1043 /* End of Outputs for SubSystem: '<S19>/StatusLightObstacle' */
1044 } else if (EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleIdle) {
1045 /* Outputs for IfAction SubSystem: '<S19>/StatusLightIdle' incorporates:
1046 * ActionPort: '<S31>/Action Port'
1047 */
1048 /* Gain: '<S31>/Gain' incorporates:
1049 * Constant: '<S19>/Constant3'
1050 */
1051 statusLight = (uint16_T)((uint32_T)EV3Control_sil_sil_ec_P.Gain_Gain_ke *
1052 rtP_STATUS_LIGHT_IDLE);
1053
1054 /* End of Outputs for SubSystem: '<S19>/StatusLightIdle' */
1055 } else if (EV3Control_sil_sil_ec_Y.VehicleStates_m.VehicleControlOff) {
1056 /* Outputs for IfAction SubSystem: '<S19>/StatusLightCtrlOff' incorporates:
1057 * ActionPort: '<S28>/Action Port'
1058 */
1059 /* Gain: '<S28>/Gain' incorporates:
1060 * Constant: '<S19>/Constant5'
1061 */
1062 statusLight = (uint16_T)((uint32_T)EV3Control_sil_sil_ec_P.Gain_Gain_d *
1063 rtP_STATUS_LIGHT_VEHICLE_CTRL_OFF);
1064
1065 /* End of Outputs for SubSystem: '<S19>/StatusLightCtrlOff' */
1066 } else {
1067 /* Outputs for IfAction SubSystem: '<S19>/StatusLightDefault' incorporates:
1068 * ActionPort: '<S29>/Action Port'
1069 */
1070 /* Gain: '<S29>/Gain' incorporates:
1071 * Constant: '<S19>/Constant2'
1072 */
1073 statusLight = (uint16_T)((uint32_T)EV3Control_sil_sil_ec_P.Gain_Gain_n *
1074 rtP_STATUS_LIGHT_DEFAULT);
1075
1076 /* End of Outputs for SubSystem: '<S19>/StatusLightDefault' */
1077 }
1078 }
1079
1080 /* End of If: '<S19>/IfLightInput' */
1081 /* End of Outputs for SubSystem: '<S11>/StatusLightControl' */
1082
1083 /* DataTypeConversion: '<S2>/defInt' */
1084 rtb_statusLight = (uint8_T)((uint32_T)statusLight >> 7);
1085
1086 /* Switch: '<S2>/Switch3' incorporates:
1087 * Constant: '<S2>/Constant3'
1088 * Outport: '<Root>/VehicleStates'
1089 */
1090 if (EV3Control_sil_sil_ec_Y.VehicleStates_m.EmergencyStop) {
1091 rtb_statusLight = rtP_STATUS_LIGHT_EMERGENCY_STOP;
1092 }
1093
1094 /* End of Switch: '<S2>/Switch3' */
1095
1096 /* BusCreator: '<S2>/Bus Creator' incorporates:
1097 * Outport: '<Root>/actuatorBusOut'
1098 */
1099 EV3Control_sil_sil_ec_Y.actuatorBusOut_k.driveMotor = rtb_driveMotor;
1100 EV3Control_sil_sil_ec_Y.actuatorBusOut_k.steeringMotor = rtb_steeringMotor;
1101 EV3Control_sil_sil_ec_Y.actuatorBusOut_k.volume =
1102 EV3Control_sil_sil_ec_DW.volume_m;
1103 EV3Control_sil_sil_ec_Y.actuatorBusOut_k.frequency =
1104 EV3Control_sil_sil_ec_DW.frequency_c;
1105 EV3Control_sil_sil_ec_Y.actuatorBusOut_k.statusLight = rtb_statusLight;
1106
1107 /* Gain: '<S4>/Gain3' incorporates:
1108 * Inport: '<Root>/sensorBusRaw'
1109 */
1110 rtb_usSensorRight = (uint8_T)(((uint32_T)EV3Control_sil_sil_ec_P.Gain3_Gain *
1111 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_UltrasoundRight) >> 7);
1112
1113 /* Gain: '<S4>/Gain4' incorporates:
1114 * Inport: '<Root>/sensorBusRaw'
1115 */
1116 rtb_usSensorLeft = (uint8_T)(((uint32_T)EV3Control_sil_sil_ec_P.Gain4_Gain *
1117 EV3Control_sil_sil_ec_U.sensorBusRaw_m.sensorRaw_UltrasoundLeft) >> 7);
1118
1119 /* Update for UnitDelay: '<S14>/Unit Delay' */
1120 EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE_p = rtb_remoteControl;
1121
1122 /* Update for UnitDelay: '<S8>/Unit Delay' */
1123 EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE = rtb_gyroAngleRad;
1124
1125 /* Update for UnitDelay: '<S8>/Unit Delay1' */
1126 EV3Control_sil_sil_ec_DW.UnitDelay1_DSTATE =
1127 EV3Control_sil_sil_ec_DW.UnitDelay;
1128
1129 /* Update for UnitDelay: '<S8>/Unit Delay2' */
1130 EV3Control_sil_sil_ec_DW.UnitDelay2_DSTATE =
1131 EV3Control_sil_sil_ec_DW.UnitDelay1;
1132}
1133
1134/* Model initialize function */
1135void EV3Control_sil_sil_ec_initialize(void)
1136{
1137 /* Registration code */
1138
1139 /* initialize non-finites */
1140 rt_InitInfAndNaN(sizeof(real_T));
1141
1142 /* Model Initialize function for ModelReference Block: '<S5>/ObstacleDetection' */
1143 ObstacleDetection_de_initialize(rtmGetErrorStatusPointer
1144 (EV3Control_sil_sil_ec_M),
1145 &(EV3Control_sil_sil_ec_DW.ObstacleDetection_InstanceData.rtm));
1146
1147 /* Model Initialize function for ModelReference Block: '<S11>/GlobalPosition' */
1148 GlobalPosition_demo__initialize(rtmGetErrorStatusPointer
1149 (EV3Control_sil_sil_ec_M),
1150 &(EV3Control_sil_sil_ec_DW.GlobalPosition_InstanceData.rtm));
1151
1152 {
1153 real_T frequency;
1154 real_T volume;
1155
1156 /* InitializeConditions for UnitDelay: '<S14>/Unit Delay' */
1157 EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE_p =
1158 EV3Control_sil_sil_ec_P.UnitDelay_InitialCondition_f;
1159
1160 /* InitializeConditions for UnitDelay: '<S8>/Unit Delay' */
1161 EV3Control_sil_sil_ec_DW.UnitDelay_DSTATE =
1162 EV3Control_sil_sil_ec_P.UnitDelay_InitialCondition;
1163
1164 /* InitializeConditions for UnitDelay: '<S8>/Unit Delay1' */
1165 EV3Control_sil_sil_ec_DW.UnitDelay1_DSTATE =
1166 EV3Control_sil_sil_ec_P.UnitDelay1_InitialCondition;
1167
1168 /* InitializeConditions for UnitDelay: '<S8>/Unit Delay2' */
1169 EV3Control_sil_sil_ec_DW.UnitDelay2_DSTATE =
1170 EV3Control_sil_sil_ec_P.UnitDelay2_InitialCondition;
1171
1172 /* SystemInitialize for ModelReference: '<S11>/GlobalPosition' incorporates:
1173 * Outport: '<Root>/posGlobal'
1174 */
1175 GlobalPosition_demo_ec_Init
1176 (&(EV3Control_sil_sil_ec_DW.GlobalPosition_InstanceData.rtdw));
1177
1178 /* SystemInitialize for ModelReference: '<S5>/ObstacleDetection' */
1179 ObstacleDetection_demo_ec_Init
1180 (&(EV3Control_sil_sil_ec_DW.ObstacleDetection_InstanceData.rtdw));
1181
1182 /* SystemInitialize for IfAction SubSystem: '<S20>/AutoSteering' */
1183 /* SystemInitialize for IfAction SubSystem: '<S43>/PurePursuitAlgorithm' */
1184 /* SystemInitialize for Outport: '<S50>/steeringAngleCmd' */
1185 EV3Control_sil_sil_ec_DW.SteeringAngleCommand =
1186 EV3Control_sil_sil_ec_P.steeringAngleCmd_Y0;
1187
1188 /* End of SystemInitialize for SubSystem: '<S43>/PurePursuitAlgorithm' */
1189
1190 /* SystemInitialize for IfAction SubSystem: '<S43>/NoPathFollowing' */
1191 /* SystemInitialize for Outport: '<S49>/steeringAngleCmd' */
1192 EV3Control_sil_sil_ec_DW.SteeringAngleCommand =
1193 EV3Control_sil_sil_ec_P.steeringAngleCmd_Y0_l;
1194
1195 /* End of SystemInitialize for SubSystem: '<S43>/NoPathFollowing' */
1196
1197 /* SystemInitialize for Merge: '<S43>/Merge' */
1198 EV3Control_sil_sil_ec_DW.SteeringAngleCommand =
1199 EV3Control_sil_sil_ec_P.Merge_InitialOutput_j;
1200
1201 /* SystemInitialize for Outport: '<Root>/steeringAngleCmdRad' incorporates:
1202 * Outport: '<S38>/steeringAngleCmdRad'
1203 */
1204 EV3Control_sil_sil_ec_Y.steeringAngleCmdRad =
1205 EV3Control_sil_sil_ec_P.steeringAngleCmdRad_Y0;
1206
1207 /* End of SystemInitialize for SubSystem: '<S20>/AutoSteering' */
1208
1209 /* SystemInitialize for IfAction SubSystem: '<S20>/ManualSteering' */
1210 /* SystemInitialize for Merge: '<S39>/Merge' */
1211 EV3Control_sil_sil_ec_DW.Merge_o =
1212 EV3Control_sil_sil_ec_P.Merge_InitialOutput_m;
1213
1214 /* End of SystemInitialize for SubSystem: '<S20>/ManualSteering' */
1215
1216 /* SystemInitialize for Merge: '<S21>/Merge' */
1217 EV3Control_sil_sil_ec_DW.Merge =
1218 EV3Control_sil_sil_ec_P.Merge_InitialOutput_h;
1219
1220 /* SystemInitialize for IfAction SubSystem: '<S17>/HonkIfObstacleDetected' */
1221 EV3_HonkIfObstacleDetected_Init(&frequency, &volume,
1222 &EV3Control_sil_sil_ec_P.HonkIfObstacleDetected);
1223
1224 /* End of SystemInitialize for SubSystem: '<S17>/HonkIfObstacleDetected' */
1225
1226 /* SystemInitialize for IfAction SubSystem: '<S17>/DoNotHonk' */
1227 EV3_HonkIfObstacleDetected_Init(&frequency, &volume,
1228 &EV3Control_sil_sil_ec_P.DoNotHonk);
1229
1230 /* End of SystemInitialize for SubSystem: '<S17>/DoNotHonk' */
1231
1232 /* SystemInitialize for Merge: '<S17>/Merge' */
1233 volume = EV3Control_sil_sil_ec_P.Merge_2_InitialOutput;
1234 frequency = EV3Control_sil_sil_ec_P.Merge_1_InitialOutput;
1235 }
1236}
1237
1238/* Model terminate function */
1239void EV3Control_sil_sil_ec_terminate(void)
1240{
1241 /* (no terminate code required) */
1242}
1243
1244/*
1245 * File trailer for generated code.
1246 *
1247 * [EOF]
1248 */
1249