controller.sa 1.7 KB

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