1 | /* |
2 | * Academic License - for use in teaching, academic research, and meeting |
3 | * course requirements at degree granting institutions only. Not for |
4 | * government, commercial, or other organizational use. |
5 | * |
6 | * File: ActuatorControl.c |
7 | * |
8 | * Code generated for Simulink model 'ActuatorControl'. |
9 | * |
10 | * Model version : 1.68 |
11 | * Simulink Coder version : 9.3 (R2020a) 18-Nov-2019 |
12 | * C/C++ source code generated on : Fri Jul 10 14:45:14 2020 |
13 | * |
14 | * Target selection: ert.tlc |
15 | * Embedded hardware selection: Intel->x86-32 (Windows32) |
16 | * Code generation objectives: |
17 | * 1. Execution efficiency |
18 | * 2. RAM efficiency |
19 | * Validation result: Not run |
20 | */ |
21 | |
22 | #include "ActuatorControl.h" |
23 | |
24 | /* Block states (default storage) */ |
25 | DW_ActuatorControl_f_T ActuatorControl_DW; |
26 | extern void ActuatorC_ActuatorLoop1_Disable(void); |
27 | extern void ActuatorControl_ActuatorLoop1(real_T rtu_U, real_T *rty_Y); |
28 | extern void ActuatorC_ActuatorLoop2_Disable(void); |
29 | extern void ActuatorControl_ActuatorLoop2(real_T rtu_U, real_T *rty_Y); |
30 | extern void ActuatorC_ActuatorLoop3_Disable(void); |
31 | extern void ActuatorControl_ActuatorLoop3(real_T rtu_U, real_T *rty_Y); |
32 | |
33 | /* Disable for Simulink Function: '<S1>/ActuatorLoop1' */ |
34 | void ActuatorC_ActuatorLoop1_Disable(void) |
35 | { |
36 | /* Disable for DiscreteIntegrator: '<S2>/Integrator' */ |
37 | ActuatorControl_DW.Integrator_DSTATE_h = ActuatorControl_DW.Integrator_fa; |
38 | } |
39 | |
40 | /* Output and update for Simulink Function: '<S1>/ActuatorLoop1' */ |
41 | void ActuatorControl_ActuatorLoop1(real_T rtu_U, real_T *rty_Y) |
42 | { |
43 | /* DiscreteIntegrator: '<S2>/Integrator' */ |
44 | ActuatorControl_DW.Integrator_fa = ActuatorControl_DW.Integrator_DSTATE_h; |
45 | |
46 | /* SignalConversion generated from: '<S2>/Y' incorporates: |
47 | * Gain: '<S2>/Gain' |
48 | * Gain: '<S2>/Gain1' |
49 | * Gain: '<S2>/Gain3' |
50 | * Gain: '<S2>/Gain4' |
51 | * SignalConversion generated from: '<S2>/U' |
52 | * Sum: '<S2>/Sum1' |
53 | * Sum: '<S5>/Diff' |
54 | * UnitDelay: '<S5>/UD' |
55 | * |
56 | * Block description for '<S5>/Diff': |
57 | * |
58 | * Add in CPU |
59 | * |
60 | * Block description for '<S5>/UD': |
61 | * |
62 | * Store in Global RAM |
63 | */ |
64 | *rty_Y = ((0.001 * ActuatorControl_DW.Integrator_fa) + (0.339 * rtu_U)) - |
65 | (((rtu_U - ActuatorControl_DW.UD_DSTATE_j) * 0.00272) * 0.001); |
66 | |
67 | /* Update for DiscreteIntegrator: '<S2>/Integrator' incorporates: |
68 | * Gain: '<S2>/Gain2' |
69 | * SignalConversion generated from: '<S2>/U' |
70 | */ |
71 | ActuatorControl_DW.Integrator_DSTATE_h += 2.73 * rtu_U; |
72 | if (ActuatorControl_DW.Integrator_DSTATE_h >= 0.1) { |
73 | ActuatorControl_DW.Integrator_DSTATE_h = 0.1; |
74 | } else { |
75 | if (ActuatorControl_DW.Integrator_DSTATE_h <= -0.1) { |
76 | ActuatorControl_DW.Integrator_DSTATE_h = -0.1; |
77 | } |
78 | } |
79 | |
80 | /* End of Update for DiscreteIntegrator: '<S2>/Integrator' */ |
81 | |
82 | /* Update for UnitDelay: '<S5>/UD' incorporates: |
83 | * SignalConversion generated from: '<S2>/U' |
84 | * |
85 | * Block description for '<S5>/UD': |
86 | * |
87 | * Store in Global RAM |
88 | */ |
89 | ActuatorControl_DW.UD_DSTATE_j = rtu_U; |
90 | } |
91 | |
92 | /* Disable for Simulink Function: '<S1>/ActuatorLoop2' */ |
93 | void ActuatorC_ActuatorLoop2_Disable(void) |
94 | { |
95 | /* Disable for DiscreteIntegrator: '<S3>/Integrator' */ |
96 | ActuatorControl_DW.Integrator_DSTATE_p = ActuatorControl_DW.Integrator_f; |
97 | } |
98 | |
99 | /* Output and update for Simulink Function: '<S1>/ActuatorLoop2' */ |
100 | void ActuatorControl_ActuatorLoop2(real_T rtu_U, real_T *rty_Y) |
101 | { |
102 | /* DiscreteIntegrator: '<S3>/Integrator' */ |
103 | ActuatorControl_DW.Integrator_f = ActuatorControl_DW.Integrator_DSTATE_p; |
104 | |
105 | /* SignalConversion generated from: '<S3>/Y' incorporates: |
106 | * Gain: '<S3>/Gain' |
107 | * Gain: '<S3>/Gain1' |
108 | * Gain: '<S3>/Gain3' |
109 | * Gain: '<S3>/Gain4' |
110 | * SignalConversion generated from: '<S3>/U' |
111 | * Sum: '<S3>/Sum1' |
112 | * Sum: '<S6>/Diff' |
113 | * UnitDelay: '<S6>/UD' |
114 | * |
115 | * Block description for '<S6>/Diff': |
116 | * |
117 | * Add in CPU |
118 | * |
119 | * Block description for '<S6>/UD': |
120 | * |
121 | * Store in Global RAM |
122 | */ |
123 | *rty_Y = ((0.001 * ActuatorControl_DW.Integrator_f) + (0.339 * rtu_U)) - |
124 | (((rtu_U - ActuatorControl_DW.UD_DSTATE_d) * 0.00272) * 0.001); |
125 | |
126 | /* Update for DiscreteIntegrator: '<S3>/Integrator' incorporates: |
127 | * Gain: '<S3>/Gain2' |
128 | * SignalConversion generated from: '<S3>/U' |
129 | */ |
130 | ActuatorControl_DW.Integrator_DSTATE_p += 2.73 * rtu_U; |
131 | if (ActuatorControl_DW.Integrator_DSTATE_p >= 0.1) { |
132 | ActuatorControl_DW.Integrator_DSTATE_p = 0.1; |
133 | } else { |
134 | if (ActuatorControl_DW.Integrator_DSTATE_p <= -0.1) { |
135 | ActuatorControl_DW.Integrator_DSTATE_p = -0.1; |
136 | } |
137 | } |
138 | |
139 | /* End of Update for DiscreteIntegrator: '<S3>/Integrator' */ |
140 | |
141 | /* Update for UnitDelay: '<S6>/UD' incorporates: |
142 | * SignalConversion generated from: '<S3>/U' |
143 | * |
144 | * Block description for '<S6>/UD': |
145 | * |
146 | * Store in Global RAM |
147 | */ |
148 | ActuatorControl_DW.UD_DSTATE_d = rtu_U; |
149 | } |
150 | |
151 | /* Disable for Simulink Function: '<S1>/ActuatorLoop3' */ |
152 | void ActuatorC_ActuatorLoop3_Disable(void) |
153 | { |
154 | /* Disable for DiscreteIntegrator: '<S4>/Integrator' */ |
155 | ActuatorControl_DW.Integrator_DSTATE = ActuatorControl_DW.Integrator; |
156 | } |
157 | |
158 | /* Output and update for Simulink Function: '<S1>/ActuatorLoop3' */ |
159 | void ActuatorControl_ActuatorLoop3(real_T rtu_U, real_T *rty_Y) |
160 | { |
161 | /* DiscreteIntegrator: '<S4>/Integrator' */ |
162 | ActuatorControl_DW.Integrator = ActuatorControl_DW.Integrator_DSTATE; |
163 | |
164 | /* SignalConversion generated from: '<S4>/Y' incorporates: |
165 | * Gain: '<S4>/Gain' |
166 | * Gain: '<S4>/Gain1' |
167 | * Gain: '<S4>/Gain3' |
168 | * Gain: '<S4>/Gain4' |
169 | * SignalConversion generated from: '<S4>/U' |
170 | * Sum: '<S4>/Sum1' |
171 | * Sum: '<S7>/Diff' |
172 | * UnitDelay: '<S7>/UD' |
173 | * |
174 | * Block description for '<S7>/Diff': |
175 | * |
176 | * Add in CPU |
177 | * |
178 | * Block description for '<S7>/UD': |
179 | * |
180 | * Store in Global RAM |
181 | */ |
182 | *rty_Y = ((0.001 * ActuatorControl_DW.Integrator) + (0.339 * rtu_U)) - |
183 | (((rtu_U - ActuatorControl_DW.UD_DSTATE) * 0.00272) * 0.001); |
184 | |
185 | /* Update for DiscreteIntegrator: '<S4>/Integrator' incorporates: |
186 | * Gain: '<S4>/Gain2' |
187 | * SignalConversion generated from: '<S4>/U' |
188 | */ |
189 | ActuatorControl_DW.Integrator_DSTATE += 2.73 * rtu_U; |
190 | if (ActuatorControl_DW.Integrator_DSTATE >= 0.1) { |
191 | ActuatorControl_DW.Integrator_DSTATE = 0.1; |
192 | } else { |
193 | if (ActuatorControl_DW.Integrator_DSTATE <= -0.1) { |
194 | ActuatorControl_DW.Integrator_DSTATE = -0.1; |
195 | } |
196 | } |
197 | |
198 | /* End of Update for DiscreteIntegrator: '<S4>/Integrator' */ |
199 | |
200 | /* Update for UnitDelay: '<S7>/UD' incorporates: |
201 | * SignalConversion generated from: '<S4>/U' |
202 | * |
203 | * Block description for '<S7>/UD': |
204 | * |
205 | * Store in Global RAM |
206 | */ |
207 | ActuatorControl_DW.UD_DSTATE = rtu_U; |
208 | } |
209 | |
210 | /* Disable for referenced model: 'ActuatorControl' */ |
211 | void ActuatorControl_Disable(void) |
212 | { |
213 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<Root>/Actuator_Control' incorporates: |
214 | * SubSystem: '<Root>/Actuator_Control' |
215 | */ |
216 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop3' incorporates: |
217 | * SubSystem: '<S1>/ActuatorLoop3' |
218 | */ |
219 | ActuatorC_ActuatorLoop3_Disable(); |
220 | |
221 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop3' */ |
222 | |
223 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop2' incorporates: |
224 | * SubSystem: '<S1>/ActuatorLoop2' |
225 | */ |
226 | ActuatorC_ActuatorLoop2_Disable(); |
227 | |
228 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop2' */ |
229 | |
230 | /* Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop1' incorporates: |
231 | * SubSystem: '<S1>/ActuatorLoop1' |
232 | */ |
233 | ActuatorC_ActuatorLoop1_Disable(); |
234 | |
235 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<S1>/ActuatorLoop1' */ |
236 | /* End of Disable for S-Function (sfun_private_function_caller) generated from: '<Root>/Actuator_Control' */ |
237 | } |
238 | |
239 | /* Output and update for referenced model: 'ActuatorControl' */ |
240 | void Actuator_Control(const int16_T rtu_Act_Pos1, const real_T |
241 | rtu_actuator_command_1, const int16_T rtu_Act_Pos2, const |
242 | real_T rtu_actuator_command_2, const int16_T rtu_Act_Pos3, |
243 | const real_T rtu_actuator_command_3, int16_T |
244 | *rty_Actuator1, int16_T *rty_Actuator2, int16_T |
245 | *rty_Actuator3) |
246 | { |
247 | real_T rtb_Scaling1; |
248 | |
249 | /* Outputs for Function Call SubSystem: '<Root>/Actuator_Control' */ |
250 | /* Sum: '<S1>/Sum2' incorporates: |
251 | * DataTypeConversion: '<S1>/TypeConversion4' |
252 | * Gain: '<S1>/Scaling4' |
253 | * SignalConversion generated from: '<S1>/Act_Pos3' |
254 | * SignalConversion generated from: '<S1>/actuator_command_3' |
255 | */ |
256 | rtb_Scaling1 = rtu_actuator_command_3 - (3.0518509475997192E-6 * ((real_T) |
257 | rtu_Act_Pos3)); |
258 | |
259 | /* FunctionCaller: '<S1>/ActuatorLoop3_caller' */ |
260 | ActuatorControl_ActuatorLoop3(rtb_Scaling1, &rtb_Scaling1); |
261 | |
262 | /* Saturate: '<S1>/Saturation2' */ |
263 | if (rtb_Scaling1 > 0.3) { |
264 | rtb_Scaling1 = 0.3; |
265 | } else { |
266 | if (rtb_Scaling1 < -0.3) { |
267 | rtb_Scaling1 = -0.3; |
268 | } |
269 | } |
270 | |
271 | /* End of Saturate: '<S1>/Saturation2' */ |
272 | |
273 | /* Gain: '<S1>/Scaling5' */ |
274 | rtb_Scaling1 *= 109223.33333333334; |
275 | |
276 | /* SignalConversion generated from: '<S1>/Actuator3' incorporates: |
277 | * DataTypeConversion: '<S1>/TypeConversion5' |
278 | */ |
279 | *rty_Actuator3 = (int16_T)floor(rtb_Scaling1); |
280 | |
281 | /* Sum: '<S1>/Sum1' incorporates: |
282 | * DataTypeConversion: '<S1>/TypeConversion2' |
283 | * Gain: '<S1>/Scaling2' |
284 | * SignalConversion generated from: '<S1>/Act_Pos2' |
285 | * SignalConversion generated from: '<S1>/actuator_command_2' |
286 | */ |
287 | rtb_Scaling1 = rtu_actuator_command_2 - (3.0518509475997192E-6 * ((real_T) |
288 | rtu_Act_Pos2)); |
289 | |
290 | /* FunctionCaller: '<S1>/ActuatorLoop2_caller' */ |
291 | ActuatorControl_ActuatorLoop2(rtb_Scaling1, &rtb_Scaling1); |
292 | |
293 | /* Saturate: '<S1>/Saturation1' */ |
294 | if (rtb_Scaling1 > 0.3) { |
295 | rtb_Scaling1 = 0.3; |
296 | } else { |
297 | if (rtb_Scaling1 < -0.3) { |
298 | rtb_Scaling1 = -0.3; |
299 | } |
300 | } |
301 | |
302 | /* End of Saturate: '<S1>/Saturation1' */ |
303 | |
304 | /* Gain: '<S1>/Scaling3' */ |
305 | rtb_Scaling1 *= 109223.33333333334; |
306 | |
307 | /* SignalConversion generated from: '<S1>/Actuator2' incorporates: |
308 | * DataTypeConversion: '<S1>/TypeConversion3' |
309 | */ |
310 | *rty_Actuator2 = (int16_T)floor(rtb_Scaling1); |
311 | |
312 | /* Sum: '<S1>/Sum' incorporates: |
313 | * DataTypeConversion: '<S1>/TypeConversion' |
314 | * Gain: '<S1>/Scaling' |
315 | * SignalConversion generated from: '<S1>/Act_Pos1' |
316 | * SignalConversion generated from: '<S1>/actuator_command_1' |
317 | */ |
318 | rtb_Scaling1 = rtu_actuator_command_1 - (3.0518509475997192E-6 * ((real_T) |
319 | rtu_Act_Pos1)); |
320 | |
321 | /* FunctionCaller: '<S1>/ActuatorLoop1_caller' */ |
322 | ActuatorControl_ActuatorLoop1(rtb_Scaling1, &rtb_Scaling1); |
323 | |
324 | /* Saturate: '<S1>/Saturation' */ |
325 | if (rtb_Scaling1 > 0.3) { |
326 | rtb_Scaling1 = 0.3; |
327 | } else { |
328 | if (rtb_Scaling1 < -0.3) { |
329 | rtb_Scaling1 = -0.3; |
330 | } |
331 | } |
332 | |
333 | /* End of Saturate: '<S1>/Saturation' */ |
334 | |
335 | /* Gain: '<S1>/Scaling1' */ |
336 | rtb_Scaling1 *= 109223.33333333334; |
337 | |
338 | /* SignalConversion generated from: '<S1>/Actuator1' incorporates: |
339 | * DataTypeConversion: '<S1>/TypeConversion1' |
340 | */ |
341 | *rty_Actuator1 = (int16_T)floor(rtb_Scaling1); |
342 | |
343 | /* End of Outputs for SubSystem: '<Root>/Actuator_Control' */ |
344 | } |
345 | |
346 | /* |
347 | * File trailer for generated code. |
348 | * |
349 | * [EOF] |
350 | */ |
351 | |