controller_sa_commented.sa 2.7 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980
  1. module Controller_SA
  2. semantic adaptation reactive moore ControllerSA controller_sa
  3. at "./path/to/ControllerSA.fmu"
  4. for inner fmu LazySA lazy
  5. at "./path/to/LazySA.fmu"
  6. with input ports obj_detected, passenger_up, passenger_down, passenger_stop, driver_up, driver_down, driver_stop
  7. with output ports up, down, stop
  8. /*
  9. At some point in the future, we support for simple wildcars in the input ports.
  10. According to Casper and Kenneth, some FMUs have thousands of ports...
  11. */
  12. input ports armature_current -> lazy.obj_detected,
  13. passenger_up -> lazy.passenger_up, // You could write passenger_* here.
  14. passenger_down -> lazy.passenger_down,
  15. passenger_stop -> lazy.passenger_stop,
  16. driver_up -> lazy.driver_up, // You could write driver_* here.
  17. driver_down -> lazy.driver_down,
  18. driver_stop -> lazy.driver_stop
  19. output ports u,
  20. d
  21. param REL_TOL := 0.0001,
  22. ABS_TOL := 1e-8,
  23. CROSSING := 5.0,
  24. INIT_ARMATURE_CURRENT := 0.0;
  25. control var aux_obj_detected := 0,
  26. previous_arm_current := INIT_ARMATURE_CURRENT;
  27. control rules {
  28. var step_size := H;
  29. var aux_obj_detected := false;
  30. var crossedTooFar := false;
  31. if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
  32. and (not is_close(future_arm_current, CROSSING, REL_TOL, ABS_TOL) and future_arm_current > CROSSING)) { // crossing, but not within tolerance
  33. crossedTooFar := true;
  34. var negative_value := previous_arm_current - CROSSING;
  35. var positive_value := future_arm_current - CROSSING;
  36. step_size := (H * (- negative_value)) / (positive_value - negative_value);
  37. } else {
  38. if ((not is_close(previous_arm_current, CROSSING, REL_TOL, ABS_TOL) and previous_arm_current < CROSSING)
  39. and is_close(future_arm_current, CROSSING, REL_TOL, ABS_TOL )) { // crossing within tolerance found
  40. aux_obj_detected := true;
  41. }
  42. }
  43. if (not crossedTooFar){
  44. step_size := do_step(lazy, t, H);
  45. }
  46. if (is_close(step_size, H, REL_TOL, ABS_TOL)) {
  47. // Step accepted, so store the known input.
  48. // 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.
  49. // If that happens, we want to still have the old value of armature current, to compare it with the new one.
  50. previous_arm_current := future_arm_current;
  51. }
  52. return step_size;
  53. }
  54. in var future_arm_current := INIT_ARMATURE_CURRENT;
  55. in rules {
  56. true -> {
  57. future_arm_current := armature_current;
  58. } --> {
  59. obj_detected := aux_obj_detected; // Sets this input to the FMU
  60. };
  61. }
  62. out rules {
  63. lazy.up -> { } --> {u := 1.0; };
  64. not lazy.up -> { } --> {u := 0.0; };
  65. lazy.down -> { } --> {d := 1.0; };
  66. not lazy.down -> { } --> {d := 0.0; };
  67. lazy.stop -> { } --> {u := 0.0 ; d := 0.0; };
  68. }