loop.sa 1.0 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546
  1. semantic adaptation reactive moore LoopSA loop_sa
  2. at "./path/to/LoopSA.fmu"
  3. for inner fmu WindowSA window_sa
  4. at "./path/to/WindowSA.fmu"
  5. with input ports displacement (rad), speed (rad/s), reaction_force (N)
  6. with output ports disp (m), tau (N.m)
  7. for inner fmu Obstacle obstacle
  8. at "./path/to/Obstacle.fmu"
  9. with input ports disp (m)
  10. with output ports reaction_force (m)
  11. coupled as window_sa.disp -> obstacle.disp,
  12. obstacle.reaction_force -> window_sa.reaction_force
  13. output ports tau <- window_sa.tau
  14. param MAXITER := 10,
  15. REL_TOL := 1e-05,
  16. ABS_TOL := 1e-05;
  17. control var prev_disp := 0.0;
  18. control rules {
  19. var repeat := false;
  20. for (var iter in 0 .. MAXITER) {
  21. save_state(obstacle);
  22. save_state(window_sa);
  23. obstacle.disp := prev_disp;
  24. do_step(obstacle,t,H);
  25. do_step(window_sa,t,H);
  26. repeat := is_close(prev_disp, window_sa.disp, REL_TOL, ABS_TOL);
  27. prev_disp := window_sa.disp;
  28. if (repeat) {
  29. break;
  30. } else {
  31. rollback(obstacle);
  32. rollback(window_sa);
  33. }
  34. }
  35. return H;
  36. }