controller.sa 1.8 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273
  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. input ports armature_current -> lazy.obj_detected,
  9. passenger_up -> lazy.passenger_up,
  10. passenger_down -> lazy.passenger_down,
  11. passenger_stop -> lazy.passenger_stop,
  12. driver_up -> lazy.driver_up,
  13. driver_down -> lazy.driver_down,
  14. driver_stop -> lazy.driver_stop
  15. output ports u,
  16. d
  17. param RTOL := 0.0001,
  18. ATOL := 1e-8,
  19. T := 5.0,
  20. INIT_V := 0.0;
  21. control var c := false,
  22. p_v := INIT_V;
  23. control rules {
  24. var step_size := H;
  25. var aux_obj_detected := false;
  26. var crossedTooFar := false;
  27. if ((not is_close(p_v, T, RTOL, ATOL) and p_v < T)
  28. and (not is_close(f_v, T, RTOL, ATOL) and f_v > T)) {
  29. crossedTooFar := true;
  30. var negative_value := p_v - T;
  31. var positive_value := f_v - T;
  32. step_size := (H * (- negative_value)) / (positive_value - negative_value);
  33. } else {
  34. if ((not is_close(p_v, T, RTOL, ATOL) and p_v < T)
  35. and is_close(f_v, T, RTOL, ATOL )) {
  36. c := true;
  37. }
  38. }
  39. if (not crossedTooFar){
  40. step_size := do_step(lazy, t, H);
  41. }
  42. if (is_close(step_size, H, RTOL, ATOL)) {
  43. p_v := f_v;
  44. }
  45. return step_size;
  46. }
  47. in var f_v := INIT_V;
  48. in rules {
  49. true -> {
  50. f_v := controller_sa.armature_current;
  51. } --> {
  52. lazy.obj_detected := c;
  53. };
  54. }
  55. out rules {
  56. lazy.up -> { } --> {controller_sa.u := 1.0; };
  57. not lazy.up -> { } --> {controller_sa.u := 0.0; };
  58. lazy.down -> { } --> {controller_sa.d := 1.0; };
  59. not lazy.down -> { } --> {controller_sa.d := 0.0; };
  60. lazy.stop -> { } --> {controller_sa.u := 0.0 ; controller_sa.d := 0.0; };
  61. }