001/**
002 * Copyright 2024 The Space Cookies : Girl Scout Troop #62868 and FRC Team #1868
003 * Open Source Software; you may modify and/or share it under the terms of
004 * the 3-Clause BSD License found in the root directory of this project.
005 */
006
007package tagalong.commands.base;
008
009import tagalong.commands.TagalongCommand;
010import tagalong.measurements.Height;
011import tagalong.subsystems.TagalongSubsystemBase;
012import tagalong.subsystems.micro.Elevator;
013import tagalong.subsystems.micro.augments.ElevatorAugment;
014
015/**
016 * Command that raises elevator by a target height based on specified
017 * parameters.
018 */
019public class ElevateXCmd<T extends TagalongSubsystemBase & ElevatorAugment>
020    extends TagalongCommand {
021  /**
022   * Elevator microsystem
023   */
024  private final Elevator _elevator;
025  /**
026   * Desired additional movement for the elevator relative to its current position
027   */
028  private final double _relativeMovementM;
029  /**
030   * Lower tolerance in meters
031   */
032  private final double _lowerToleranceM;
033  /**
034   * Upper tolerance in meters
035   */
036  private final double _upperToleranceM;
037  /**
038   * Whether or not the elevator must be in tolerance for the command to end
039   */
040  private final boolean _requireInTolerance;
041  /**
042   * Duration that the elevator must be in tolerance for the command to end
043   */
044  private final double _requiredInToleranceDurationS;
045
046  /**
047   * Lower bound of the goal tolerance
048   */
049  private double _lowerBoundM;
050  /**
051   * Upper bound of the goal tolerance
052   */
053  private double _upperBoundM;
054  /**
055   * Whether or not the elevator should holds its position after the command ends
056   */
057  private boolean _holdPositionAfter;
058  /**
059   * True if the command has started to move yet
060   * False if waiting for a safe state before moving
061   */
062  private boolean _startedMovement;
063  /**
064   * Max velocity of the elevator for this specific command
065   */
066  private double _maxVelocityMPS;
067  /**
068   * Starting height of the elevator when the command executes
069   */
070  private double _startHeightM;
071  /**
072   * Actual goal height of the elevator when the command executes
073   */
074  private double _goalPositionM;
075
076  @Override
077  public void initialize() {
078    _startedMovement = false;
079    _startHeightM = _elevator.getElevatorHeightM();
080    _goalPositionM = _elevator.clampElevatorPosition(_startHeightM + _relativeMovementM);
081    _lowerBoundM = _goalPositionM - _lowerToleranceM;
082    _upperBoundM = _goalPositionM + _upperToleranceM;
083  }
084
085  @Override
086  public void execute() {
087    if (!_startedMovement) {
088      if (_elevator.isSafeToMove()) {
089        _elevator.setElevatorProfile(_goalPositionM, 0.0, _maxVelocityMPS);
090        _startedMovement = true;
091        _elevator.followLastProfile();
092      }
093    } else {
094      _elevator.followLastProfile();
095    }
096  }
097
098  @Override
099  public void end(boolean interrupted) {
100    _elevator.setFollowProfile(_holdPositionAfter);
101  }
102
103  @Override
104  public boolean isFinished() {
105    // Command is finished when the profile is finished AND
106    // Either the tolerance is bypassed or in tolerance for the desired duration
107    return _elevator.isProfileFinished()
108        && (!_requireInTolerance
109            || _elevator.checkToleranceTime(
110                _elevator.isElevatorInTolerance(_lowerBoundM, _upperBoundM),
111                _requiredInToleranceDurationS
112            ));
113  }
114
115  /**
116   * Constructor that creates the command with the below parameters.
117   *
118   * @param elevator          Tagalong Subsystem containing an elevator
119   *                          microsystem
120   * @param relativeMovementM the target amount to move, in meters
121   */
122  public ElevateXCmd(T elevator, Height relativeMovementM) {
123    this(elevator, relativeMovementM, true);
124  }
125
126  /**
127   * Constructor that creates the command with the below parameters.
128   *
129   * @param id                Tagalong Subsystem containing an elevator
130   *                          microsystem
131   * @param elevator          Tagalong Subsystem containing an elevator
132   *                          microsystem
133   * @param relativeMovementM the target amount to move, in meters
134   */
135  public ElevateXCmd(int id, T elevator, Height relativeMovementM) {
136    this(id, elevator, relativeMovementM, true);
137  }
138
139  /**
140   * Constructor that creates the command with the below parameters.
141   *
142   * @param elevator          Tagalong Subsystem containing an elevator
143   *                          microsystem
144   * @param relativeMovementM the target amount to move, in meters
145   * @param holdPositionAfter whether or not the elevator must hold position after
146   *                          reaching target
147   */
148  public ElevateXCmd(T elevator, Height relativeMovementM, boolean holdPositionAfter) {
149    this(elevator, relativeMovementM, holdPositionAfter, elevator.getElevator()._maxVelocityMPS);
150  }
151
152  /**
153   * Constructor that creates the command with the below parameters.
154   *
155   * @param id                Tagalong Subsystem containing an elevator
156   *                          microsystem
157   * @param elevator          Tagalong Subsystem containing an elevator
158   *                          microsystem
159   * @param relativeMovementM the target amount to move, in meters
160   * @param holdPositionAfter whether or not the elevator must hold position after
161   *                          reaching target
162   */
163  public ElevateXCmd(int id, T elevator, Height relativeMovementM, boolean holdPositionAfter) {
164    this(
165        id, elevator, relativeMovementM, holdPositionAfter, elevator.getElevator(id)._maxVelocityMPS
166    );
167  }
168
169  /**
170   * Constructor that creates the command with the below parameters.
171   *
172   * @param elevator          Tagalong Subsystem containing an elevator
173   *                          microsystem
174   * @param relativeMovementM the target amount to move, in meters
175   * @param holdPositionAfter whether or not the elevator must hold position after
176   *                          reaching target
177   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
178   *                          can move
179   */
180  public ElevateXCmd(
181      T elevator, Height relativeMovementM, boolean holdPositionAfter, double maxVelocityMPS
182  ) {
183    this(
184        elevator,
185        relativeMovementM,
186        holdPositionAfter,
187        maxVelocityMPS,
188        elevator.getElevator()._defaultElevatorLowerToleranceM,
189        elevator.getElevator()._defaultElevatorUpperToleranceM
190    );
191  }
192
193  /**
194   * Constructor that creates the command with the below parameters.
195   *
196   * @param id                Tagalong Subsystem containing an elevator
197   *                          microsystem
198   * @param elevator          Tagalong Subsystem containing an elevator
199   *                          microsystem
200   * @param relativeMovementM the target amount to move, in meters
201   * @param holdPositionAfter whether or not the elevator must hold position after
202   *                          reaching target
203   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
204   *                          can move
205   */
206  public ElevateXCmd(
207      int id, T elevator, Height relativeMovementM, boolean holdPositionAfter, double maxVelocityMPS
208  ) {
209    this(
210        id,
211        elevator,
212        relativeMovementM,
213        holdPositionAfter,
214        maxVelocityMPS,
215        elevator.getElevator(id)._defaultElevatorLowerToleranceM,
216        elevator.getElevator(id)._defaultElevatorUpperToleranceM
217    );
218  }
219
220  /**
221   * Constructor that creates the command with the below parameters.
222   *
223   * @param elevator          Tagalong Subsystem containing an elevator
224   *                          microsystem
225   * @param relativeMovementM the target amount to move, in meters
226   * @param holdPositionAfter whether or not the elevator must hold position after
227   *                          reaching target
228   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
229   *                          can move
230   * @param toleranceM        the desired tolerance for height, in meters
231   */
232  public ElevateXCmd(
233      T elevator,
234      Height relativeMovementM,
235      boolean holdPositionAfter,
236      double maxVelocityMPS,
237      double toleranceM
238  ) {
239    this(elevator, relativeMovementM, holdPositionAfter, maxVelocityMPS, toleranceM, toleranceM);
240  }
241
242  /**
243   * Constructor that creates the command with the below parameters.
244   *
245   * @param id                Tagalong Subsystem containing an elevator
246   *                          microsystem
247   * @param elevator          Tagalong Subsystem containing an elevator
248   *                          microsystem
249   * @param relativeMovementM the target amount to move, in meters
250   * @param holdPositionAfter whether or not the elevator must hold position after
251   *                          reaching target
252   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
253   *                          can move
254   * @param toleranceM        the desired tolerance for height, in meters
255   */
256  public ElevateXCmd(
257      int id,
258      T elevator,
259      Height relativeMovementM,
260      boolean holdPositionAfter,
261      double maxVelocityMPS,
262      double toleranceM
263  ) {
264    this(
265        id, elevator, relativeMovementM, holdPositionAfter, maxVelocityMPS, toleranceM, toleranceM
266    );
267  }
268
269  /**
270   * Constructor that creates the command with the below parameters.
271   *
272   * @param elevator          Tagalong Subsystem containing an elevator
273   *                          microsystem
274   * @param relativeMovementM the target amount to move, in meters
275   * @param holdPositionAfter whether or not the elevator must hold position after
276   *                          reaching target
277   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
278   *                          can move
279   * @param lowerToleranceM   the lower bound for height tolerance, in meters
280   * @param upperToleranceM   the upper bound for height tolerance, in meters
281   */
282  public ElevateXCmd(
283      T elevator,
284      Height relativeMovementM,
285      boolean holdPositionAfter,
286      double maxVelocityMPS,
287      double lowerToleranceM,
288      double upperToleranceM
289  ) {
290    this(
291        elevator,
292        relativeMovementM,
293        holdPositionAfter,
294        maxVelocityMPS,
295        lowerToleranceM,
296        upperToleranceM,
297        -1.0
298    );
299  }
300
301  /**
302   * Constructor that creates the command with the below parameters.
303   *
304   * @param id                Tagalong Subsystem containing an elevator
305   *                          microsystem
306   * @param elevator          Tagalong Subsystem containing an elevator
307   *                          microsystem
308   * @param relativeMovementM the target amount to move, in meters
309   * @param holdPositionAfter whether or not the elevator must hold position after
310   *                          reaching target
311   * @param maxVelocityMPS    maximum velocity, in meters per second, the elevator
312   *                          can move
313   * @param lowerToleranceM   the lower bound for height tolerance, in meters
314   * @param upperToleranceM   the upper bound for height tolerance, in meters
315   */
316  public ElevateXCmd(
317      int id,
318      T elevator,
319      Height relativeMovementM,
320      boolean holdPositionAfter,
321      double maxVelocityMPS,
322      double lowerToleranceM,
323      double upperToleranceM
324  ) {
325    this(
326        id,
327        elevator,
328        relativeMovementM,
329        holdPositionAfter,
330        maxVelocityMPS,
331        lowerToleranceM,
332        upperToleranceM,
333        -1.0
334    );
335  }
336
337  /**
338   * Constructor that creates the command with the below parameters.
339   *
340   * @param elevator                     Tagalong Subsystem containing an elevator
341   *                                     microsystem
342   * @param relativeMovement             the target amount to move
343   * @param holdPositionAfter            whether or not the elevator must hold
344   *                                     position after reaching target
345   * @param maxVelocityMPS               maximum velocity, in meters per second,
346   *                                     the elevator can move
347   * @param lowerToleranceM              the lower bound for height tolerance, in
348   *                                     meters
349   * @param upperToleranceM              the upper bound for height tolerance, in
350   *                                     meters
351   * @param requiredInToleranceDurationS the duration (in seconds) the elevator
352   *                                     must stay in
353   *                                     tolerance
354   */
355  public ElevateXCmd(
356      T elevator,
357      Height relativeMovement,
358      boolean holdPositionAfter,
359      double maxVelocityMPS,
360      double lowerToleranceM,
361      double upperToleranceM,
362      double requiredInToleranceDurationS
363  ) {
364    this(
365        elevator,
366        relativeMovement.getHeightM(),
367        holdPositionAfter,
368        maxVelocityMPS,
369        lowerToleranceM,
370        upperToleranceM,
371        requiredInToleranceDurationS
372    );
373  }
374
375  /**
376   * Constructor that creates the command with the below parameters.
377   *
378   * @param id                           Tagalong Subsystem containing an elevator
379   *                                     microsystem
380   * @param elevator                     Tagalong Subsystem containing an elevator
381   *                                     microsystem
382   * @param relativeMovement             the target amount to move
383   * @param holdPositionAfter            whether or not the elevator must hold
384   *                                     position after reaching target
385   * @param maxVelocityMPS               maximum velocity, in meters per second,
386   *                                     the elevator can move
387   * @param lowerToleranceM              the lower bound for height tolerance, in
388   *                                     meters
389   * @param upperToleranceM              the upper bound for height tolerance, in
390   *                                     meters
391   * @param requiredInToleranceDurationS the duration (in seconds) the elevator
392   *                                     must stay in
393   *                                     tolerance
394   */
395  public ElevateXCmd(
396      int id,
397      T elevator,
398      Height relativeMovement,
399      boolean holdPositionAfter,
400      double maxVelocityMPS,
401      double lowerToleranceM,
402      double upperToleranceM,
403      double requiredInToleranceDurationS
404  ) {
405    this(
406        id,
407        elevator,
408        relativeMovement.getHeightM(),
409        holdPositionAfter,
410        maxVelocityMPS,
411        lowerToleranceM,
412        upperToleranceM,
413        requiredInToleranceDurationS
414    );
415  }
416
417  /**
418   * Full constructor, allows for double for goal in rare cases of advance use
419   * needing a direct way to interact
420   *
421   * @param elevator                     Tagalong Subsystem containing an elevator
422   *                                     microsystem
423   * @param relativeMovementM            the target amount to move, in meters
424   * @param holdPositionAfter            whether or not the elevator must hold
425   *                                     position after reaching target
426   * @param maxVelocityMPS               maximum velocity, in meters per second,
427   *                                     the elevator can move
428   * @param lowerToleranceM              the lower bound for height tolerance, in
429   *                                     meters
430   * @param upperToleranceM              the upper bound for height tolerance, in
431   *                                     meters
432   * @param requiredInToleranceDurationS the duration (in seconds) the elevator
433   *                                     must stay in
434   *                                     tolerance
435   */
436  public ElevateXCmd(
437      T elevator,
438      double relativeMovementM,
439      boolean holdPositionAfter,
440      double maxVelocityMPS,
441      double lowerToleranceM,
442      double upperToleranceM,
443      double requiredInToleranceDurationS
444  ) {
445    _elevator = elevator.getElevator();
446    _relativeMovementM = relativeMovementM;
447    _holdPositionAfter = holdPositionAfter;
448    _lowerToleranceM = Math.abs(lowerToleranceM);
449    _upperToleranceM = Math.abs(upperToleranceM);
450    _maxVelocityMPS = maxVelocityMPS;
451    _requiredInToleranceDurationS = requiredInToleranceDurationS;
452    _requireInTolerance = _requiredInToleranceDurationS >= 0.0;
453
454    addRequirements(elevator);
455  }
456
457  /**
458   * Full constructor, allows for double for goal in rare cases of advance use
459   * needing a direct way to interact
460   *
461   * @param id                           Tagalong Subsystem containing an elevator
462   *                                     microsystem
463   * @param elevator                     Tagalong Subsystem containing an elevator
464   *                                     microsystem
465   * @param relativeMovementM            the target amount to move, in meters
466   * @param holdPositionAfter            whether or not the elevator must hold
467   *                                     position after reaching target
468   * @param maxVelocityMPS               maximum velocity, in meters per second,
469   *                                     the elevator can move
470   * @param lowerToleranceM              the lower bound for height tolerance, in
471   *                                     meters
472   * @param upperToleranceM              the upper bound for height tolerance, in
473   *                                     meters
474   * @param requiredInToleranceDurationS the duration (in seconds) the elevator
475   *                                     must stay in
476   *                                     tolerance
477   */
478  public ElevateXCmd(
479      int id,
480      T elevator,
481      double relativeMovementM,
482      boolean holdPositionAfter,
483      double maxVelocityMPS,
484      double lowerToleranceM,
485      double upperToleranceM,
486      double requiredInToleranceDurationS
487  ) {
488    _elevator = elevator.getElevator(id);
489    _relativeMovementM = relativeMovementM;
490    _holdPositionAfter = holdPositionAfter;
491    _lowerToleranceM = _goalPositionM - Math.abs(lowerToleranceM);
492    _upperToleranceM = _goalPositionM + Math.abs(upperToleranceM);
493    _maxVelocityMPS = maxVelocityMPS;
494    _requiredInToleranceDurationS = requiredInToleranceDurationS;
495    _requireInTolerance = _requiredInToleranceDurationS >= 0.0;
496
497    addRequirements(elevator);
498  }
499}