window_obstacle_sa_loop_BASE_new.sa 674 B

12345678910111213141516171819202122232425262728
  1. //outdated
  2. semantic adaptation window_obstacle_sa_loop
  3. for fmu window_sa, obstacle
  4. input ports motor_speed
  5. output ports reaction_torque
  6. param MAXITER = 100;
  7. param REL_TOL = 1e-8;
  8. param ABS_TOL = 1e-8;
  9. control rules {
  10. var prev_height;
  11. var window_state;
  12. var obstacle_state;
  13. for (var iter in 0 .. MAXITER) {
  14. window_state := get_state(window_sa);
  15. obstacle_state := get_state(obstacle);
  16. prev_height := height;
  17. do_step(obstacle, H); // with delayed input for height
  18. do_step(window_sa, H);
  19. if (is_close(prev_height, height, REL_TOL, ABS_TOL)) {
  20. break;
  21. } else {
  22. set_state(window_sa, window_state);
  23. set_state(obstacle, obstacle_state);
  24. }
  25. }
  26. return H;
  27. }