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. \ |
39 | Consider adjusting Test hardware word size settings on the \ |
40 | Hardware Implementation pane to match your compiler word sizes as \ |
41 | defined in limits.h of the compiler. Alternatively, you can \ |
42 | select the Test hardware is the same as production hardware option and \ |
43 | select the Enable portable word sizes option on the Code Generation > \ |
44 | Verification pane for ERT based targets, which will disable the \ |
45 | preprocessor 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. \ |
50 | Consider adjusting Test hardware word size settings on the \ |
51 | Hardware Implementation pane to match your compiler word sizes as \ |
52 | defined in limits.h of the compiler. Alternatively, you can \ |
53 | select the Test hardware is the same as production hardware option and \ |
54 | select the Enable portable word sizes option on the Code Generation > \ |
55 | Verification pane for ERT based targets, which will disable the \ |
56 | preprocessor 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. \ |
61 | Consider adjusting Test hardware word size settings on the \ |
62 | Hardware Implementation pane to match your compiler word sizes as \ |
63 | defined in limits.h of the compiler. Alternatively, you can \ |
64 | select the Test hardware is the same as production hardware option and \ |
65 | select the Enable portable word sizes option on the Code Generation > \ |
66 | Verification pane for ERT based targets, which will disable the \ |
67 | preprocessor 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. \ |
72 | Consider adjusting Test hardware word size settings on the \ |
73 | Hardware Implementation pane to match your compiler word sizes as \ |
74 | defined in limits.h of the compiler. Alternatively, you can \ |
75 | select the Test hardware is the same as production hardware option and \ |
76 | select the Enable portable word sizes option on the Code Generation > \ |
77 | Verification pane for ERT based targets, which will disable the \ |
78 | preprocessor 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) */ |
85 | DW_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_DW; |
86 | |
87 | /* External inputs (root inport signals with default storage) */ |
88 | ExtU_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_U; |
89 | |
90 | /* External outputs (root outports fed by signals with default storage) */ |
91 | ExtY_EV3Control_sil_sil_ec_T EV3Control_sil_sil_ec_Y; |
92 | |
93 | /* Real-time model */ |
94 | RT_MODEL_EV3Control_sil_sil_e_T EV3Control_sil_sil_ec_M_; |
95 | RT_MODEL_EV3Control_sil_sil_e_T *const EV3Control_sil_sil_ec_M = |
96 | &EV3Control_sil_sil_ec_M_; |
97 | static void EV3_HonkIfObstacleDetected_Init(real_T *rty_speaker, real_T |
98 | *rty_speaker_g, P_HonkIfObstacleDetected_EV3C_T *localP); |
99 | static 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 | */ |
107 | static 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 | */ |
120 | static 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 */ |
131 | void 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 */ |
1135 | void 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 */ |
1239 | void 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 | |