1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980 |
- module Controller_SA
- semantic adaptation reactive moore ControllerSA controller_sa
- at "./path/to/ControllerSA.fmu"
- for inner fmu LazySA lazy
- at "./path/to/LazySA.fmu"
- with input ports obj_detected, passenger_up, passenger_down, passenger_stop, driver_up, driver_down, driver_stop
- with output ports up, down, stop
- /*
- At some point in the future, we support for simple wildcars in the input ports.
- According to Casper and Kenneth, some FMUs have thousands of ports...
- */
- input ports armature_current -> lazy.obj_detected,
- passenger_up -> lazy.passenger_up, // You could write passenger_* here.
- passenger_down -> lazy.passenger_down,
- passenger_stop -> lazy.passenger_stop,
- driver_up -> lazy.driver_up, // You could write driver_* here.
- driver_down -> lazy.driver_down,
- driver_stop -> lazy.driver_stop
- output ports u,
- d
- param REL_TOL := 0.0001,
- ABS_TOL := 1e-8,
- CROSSING := 5.0,
- INIT_ARMATURE_CURRENT := 0.0;
- control var aux_obj_detected := 0,
- previous_arm_current := INIT_ARMATURE_CURRENT;
- control rules {
- var step_size := H;
- var aux_obj_detected := false;
- var crossedTooFar := false;
- if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
- and (not is_close(future_arm_current, CROSSING, REL_TOL, ABS_TOL) and future_arm_current > CROSSING)) { // crossing, but not within tolerance
- crossedTooFar := true;
- var negative_value := previous_arm_current - CROSSING;
- var positive_value := future_arm_current - CROSSING;
- step_size := (H * (- negative_value)) / (positive_value - negative_value);
- } else {
- if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
- and is_close(future_arm_current, CROSSING, REL_TOL, ABS_TOL )) { // crossing within tolerance found
- aux_obj_detected := true;
- }
- }
-
- if (not crossedTooFar){
- step_size := do_step(lazy, t, H);
- }
-
- if (is_close(step_size, H, REL_TOL, ABS_TOL)) {
- // Step accepted, so store the known input.
- // We cannot store the armature current at the in rules because we may not accept the step and because they may be called multiple times.
- // If that happens, we want to still have the old value of armature current, to compare it with the new one.
- previous_arm_current := future_arm_current;
- }
- return step_size;
- }
- in var future_arm_current := INIT_ARMATURE_CURRENT;
- in rules {
- true -> {
- future_arm_current := armature_current;
- } --> {
- obj_detected := aux_obj_detected; // Sets this input to the FMU
- };
- }
- out rules {
- lazy.up -> { } --> {u := 1.0; };
- not lazy.up -> { } --> {u := 0.0; };
-
- lazy.down -> { } --> {d := 1.0; };
- not lazy.down -> { } --> {d := 0.0; };
-
- lazy.stop -> { } --> {u := 0.0 ; d := 0.0; };
- }
|