1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162 |
- import PowerWindowModel
- module Controller_SA
- semantic adaptation controller_sa
- for fmu controller
- input ports armature_current -> obj_detected, passenger_up, passenger_down, driver_up, driver_down
- output ports up, down
- param REL_TOL = 0.0001;
- param ABS_TOL = 1e-8;
- param CROSSING = 5;
- param init_armature_current = CROSSING;
- param init_up = 0;
- param init_down = 0;
- control var internal_transition;
- control rules { // overwriting master! What to do in case of roll back? How to reuse generic algorithm
- if (not is_set(internal_transition)) { // only at start of simulation
- var state := get_state(controller);
- internal_transition := do_step(controller, MAX) + t; // may return internal transition time or MAX
- set_state(controller, state);
- }
- var step_size; // may be used to determine roll-back
- if (signal == true or t >= internal_transition) {
- step_size := do_step(controller, H); // do a step, then decide next internal transition
- var state := get_state(controller);
- internal_transition := do_step(controller, MAX) + t;
- set_state(controller, state);
- }
- return step_size;
- }
- in var stored_armature_current := init_armature_current;
- in rules {
- true -> {
- obj_detected := false;
- if (not (not is_close(stored_armature_current, CROSSING, REL_TOL, ABS_TOL) and stored_armature_current > CROSSING)
- and (not is_close(armature_current, CROSSING, REL_TOL, ABS_TOL) and armature_current > CROSSING)) { // crossing, but not within tolerance
- var negative_value := stored_armature_current - CROSSING;
- var positive_value := armature_current - CROSSING;
- var new_step_size := (H * (- negative_value)) / (positive_value - negative_value);
- discard(new_step_size);
- } else {
- if (not (not is_close(stored_armature_current, CROSSING, REL_TOL, ABS_TOL) and stored_armature_current > CROSSING)
- and is_close(armature_current, CROSSING, REL_TOL, ABS_TOL)) { // crossing within tolerance found
- obj_detected := true;
- }
- }
- stored_armature_current := armature_current;
- } --> { };
- }
- out var stored_up := init_up;
- out var stored_down := init_down;
- out rules {
- up == true -> { up := 1; } --> { };
- otherwise up -> { } --> { };
- down == true -> { down := 1; } --> { };
- otherwise down -> { } --> { };
- stop == true -> { up := 0; down := 0; } --> { };
- otherwise stop -> { } --> { };
- true -> { up := stored_up; stored_up := up; } --> { };
- true -> { down := stored_down; stored_down := down; } --> { };
- }
|