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.math.AlgebraicUtils;
011import tagalong.measurements.Angle;
012import tagalong.subsystems.TagalongSubsystemBase;
013import tagalong.subsystems.micro.Pivot;
014import tagalong.subsystems.micro.augments.PivotAugment;
015
016/**
017 * PivotToCommand with optimal pathing
018 */
019public class PivotToAbsoluteCmd<T extends TagalongSubsystemBase & PivotAugment>
020    extends TagalongCommand {
021  /**
022   * pivot object
023   */
024  private final Pivot _pivot;
025  /**
026   * goal position in rotations
027   */
028  private final double _goalPositionRot;
029  /**
030   * whether or not the elevator must hold position after reaching target
031   */
032  private final boolean _holdPositionAfter;
033  /**
034   * lower pivot bound in rotations
035   */
036  private final double _lowerBoundRot;
037  /**
038   * upper pivot bound in rotations
039   */
040  private final double _upperBoundRot;
041  /**
042   * whether or not to require pivot to be in tolerance
043   */
044  private final boolean _requireInTolerance;
045  /**
046   * how long pivot must be in tolerance for in seconds
047   */
048  private final double _requiredInToleranceDurationS;
049
050  /**
051   *The maximum velocity of the pivot, in rotations per second, during this command
052   */
053  private double _maxVelocityRPS;
054  /**
055   * whether or not the pivot has started moving
056   */
057  private boolean _startedMovement;
058
059  /**
060   * closest wrapped goal angle in rotations
061   */
062  private double _scopedGoalPositionRot;
063
064  @Override
065  public void initialize() {
066    _pivot.setHoldPosition(false);
067    _startedMovement = false;
068    _pivot.resetToleranceTimer();
069  }
070
071  @Override
072  public void execute() {
073    if (!_startedMovement && _pivot.isSafeToMove()) {
074      _startedMovement = true;
075      _scopedGoalPositionRot =
076          AlgebraicUtils.placeInScopeRot(_pivot.getPivotPosition(), _goalPositionRot);
077      if (_scopedGoalPositionRot < _pivot._minPositionRot) {
078        _scopedGoalPositionRot += 1.0;
079      }
080      if (_scopedGoalPositionRot > _pivot._maxPositionRot) {
081        _scopedGoalPositionRot -= 1.0;
082      }
083
084      _pivot.setPivotProfile(
085          _pivot.clampPivotPosition(_scopedGoalPositionRot), 0.0, _maxVelocityRPS
086      );
087    }
088
089    if (_startedMovement) {
090      _pivot.followLastProfile();
091    }
092  }
093
094  @Override
095  public void end(boolean interrupted) {
096    _pivot.setHoldPosition(_holdPositionAfter);
097  }
098
099  @Override
100  public boolean isFinished() {
101    // Command is finished when the profile is finished AND
102    // Either the tolerance is bypassed or in tolerance for the desired duration
103    return _pivot.isProfileFinished()
104        && (!_requireInTolerance
105            || _pivot.checkToleranceTime(
106                _pivot.isPivotInAbsoluteTolerance(_lowerBoundRot, _upperBoundRot),
107                _requiredInToleranceDurationS
108            ));
109  }
110
111  /**
112   * Constructor that creates the command with the below parameters.
113   *
114   * @param pivot        Tagalong Subsystem containing a pivot microsystem
115   * @param goalPosition The goal position to reach
116   */
117  public PivotToAbsoluteCmd(T pivot, Angle goalPosition) {
118    this(pivot, goalPosition, true);
119  }
120
121  /**
122   * Constructor that creates the command with the below parameters.
123   *
124   * @param id           The pivot integer ID
125   * @param pivot        Tagalong Subsystem containing a pivot microsystem
126   * @param goalPosition The goal position to reach
127   */
128  public PivotToAbsoluteCmd(int id, T pivot, Angle goalPosition) {
129    this(id, pivot, goalPosition, true);
130  }
131
132  /**
133   * Constructor that creates the command with the below parameters.
134   *
135   * @param pivot             Tagalong Subsystem containing a pivot microsystem
136   * @param goalPosition      The goal position to reach
137   * @param holdPositionAfter If the pivot should hold position when the command
138   *                          completes
139   *
140   */
141  public PivotToAbsoluteCmd(T pivot, Angle goalPosition, boolean holdPositionAfter) {
142    this(pivot, goalPosition, holdPositionAfter, pivot.getPivot()._maxVelocityRPS);
143  }
144
145  /**
146   * Constructor that creates the command with the below parameters.
147   *
148   * @param id                The pivot integer ID
149   * @param pivot             Tagalong Subsystem containing a pivot microsystem
150   * @param goalPosition      The goal position to reach
151   * @param holdPositionAfter If the pivot should hold position when the command
152   *                          completes
153   *
154   */
155  public PivotToAbsoluteCmd(int id, T pivot, Angle goalPosition, boolean holdPositionAfter) {
156    this(id, pivot, goalPosition, holdPositionAfter, pivot.getPivot(id)._maxVelocityRPS);
157  }
158
159  /**
160   * Constructor that creates the command with the below parameters.
161   *
162   * @param pivot             Tagalong Subsystem containing a pivot microsystem
163   * @param goalPosition      The goal position to reach
164   * @param holdPositionAfter If the pivot should hold position when the command
165   *                          completes
166   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
167   *                          second, during this command
168   */
169
170  public PivotToAbsoluteCmd(
171      T pivot, Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS
172  ) {
173    this(
174        pivot,
175        goalPosition,
176        holdPositionAfter,
177        maxVelocityRPS,
178        pivot.getPivot()._defaultPivotLowerToleranceRot,
179        pivot.getPivot()._defaultPivotUpperToleranceRot
180    );
181  }
182
183  /**
184   * Constructor that creates the command with the below parameters.
185   *
186   * @param id                The pivot integer ID
187   * @param pivot             Tagalong Subsystem containing a pivot microsystem
188   * @param goalPosition      The goal position to reach
189   * @param holdPositionAfter If the pivot should hold position when the command
190   *                          completes
191   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
192   *                          second, during this command
193   */
194
195  public PivotToAbsoluteCmd(
196      int id, T pivot, Angle goalPosition, boolean holdPositionAfter, double maxVelocityRPS
197  ) {
198    this(
199        id,
200        pivot,
201        goalPosition,
202        holdPositionAfter,
203        maxVelocityRPS,
204        pivot.getPivot(id)._defaultPivotLowerToleranceRot,
205        pivot.getPivot(id)._defaultPivotUpperToleranceRot
206    );
207  }
208
209  /**
210   * Constructor that creates the command with the below parameters.
211   *
212   * @param pivot             Tagalong Subsystem containing a pivot microsystem
213   * @param goalPosition      The goal position to reach
214   * @param holdPositionAfter If the pivot should hold position when the command
215   *                          completes
216   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
217   *                          second, during this command
218   * @param toleranceRot      The number of rotations below or beyond the target
219   *                          position the pivot can
220   *                          be while still being considered in tolerance
221   */
222
223  public PivotToAbsoluteCmd(
224      T pivot,
225      Angle goalPosition,
226      boolean holdPositionAfter,
227      double maxVelocityRPS,
228      double toleranceRot
229  ) {
230    this(pivot, goalPosition, holdPositionAfter, maxVelocityRPS, toleranceRot, toleranceRot);
231  }
232
233  /**
234   * Constructor that creates the command with the below parameters.
235   *
236   * @param id                The pivot integer ID
237   * @param pivot             Tagalong Subsystem containing a pivot microsystem
238   * @param goalPosition      The goal position to reach
239   * @param holdPositionAfter If the pivot should hold position when the command
240   *                          completes
241   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
242   *                          second, during this command
243   * @param toleranceRot      The number of rotations below or beyond the target
244   *                          position the pivot can
245   *                          be while still being considered in tolerance
246   */
247  public PivotToAbsoluteCmd(
248      int id,
249      T pivot,
250      Angle goalPosition,
251      boolean holdPositionAfter,
252      double maxVelocityRPS,
253      double toleranceRot
254  ) {
255    this(id, pivot, goalPosition, holdPositionAfter, maxVelocityRPS, toleranceRot, toleranceRot);
256  }
257
258  /**
259   * Constructor with no in tolerance duration requirement
260   *
261   * @param pivot             Tagalong Subsystem containing a pivot microsystem
262   * @param goalPosition      The goal position to reach
263   * @param holdPositionAfter If the pivot should hold position when the command
264   *                          completes
265   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
266   *                          second, during this command
267   * @param lowerToleranceRot The number of rotations below the target position
268   *                          the pivot can be
269   *                          while still being considered in tolerance
270   * @param upperToleranceRot The number of rotations beyond the target position
271   *                          the pivot can be
272   *                          while still being considered in tolerance
273   */
274
275  public PivotToAbsoluteCmd(
276      T pivot,
277      Angle goalPosition,
278      boolean holdPositionAfter,
279      double maxVelocityRPS,
280      double lowerToleranceRot,
281      double upperToleranceRot
282  ) {
283    this(
284        pivot,
285        goalPosition,
286        holdPositionAfter,
287        maxVelocityRPS,
288        lowerToleranceRot,
289        upperToleranceRot,
290        -1.0
291    );
292  }
293
294  /**
295   * Constructor with ID for specifying pivot but no in tolerance duration requirement
296   *
297   * @param id                The pivot integer ID
298   * @param pivot             Tagalong Subsystem containing a pivot microsystem
299   * @param goalPosition      The goal position to reach
300   * @param holdPositionAfter If the pivot should hold position when the command
301   *                          completes
302   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per
303   *                          second, during this command
304   * @param lowerToleranceRot The number of rotations below the target position
305   *                          the pivot can be
306   *                          while still being considered in tolerance
307   * @param upperToleranceRot The number of rotations beyond the target position
308   *                          the pivot can be
309   *                          while still being considered in tolerance
310   */
311
312  public PivotToAbsoluteCmd(
313      int id,
314      T pivot,
315      Angle goalPosition,
316      boolean holdPositionAfter,
317      double maxVelocityRPS,
318      double lowerToleranceRot,
319      double upperToleranceRot
320  ) {
321    this(
322        id,
323        pivot,
324        goalPosition,
325        holdPositionAfter,
326        maxVelocityRPS,
327        lowerToleranceRot,
328        upperToleranceRot,
329        -1.0
330    );
331  }
332
333  /**
334   * Full constructor
335   *
336   * @param pivot                        Tagalong Subsystem containing a pivot
337   *                                     microsystem
338   * @param goalPosition                 The goal position to reach
339   * @param holdPositionAfter            If the pivot should hold position when
340   *                                     the command completes
341   * @param maxVelocityRPS               The maximum velocity of the pivot, in
342   *                                     rotations per second, during this
343   *                                     command
344   * @param upperToleranceRot            The number of rotations beyond the target
345   *                                     position the pivot can be
346   *                                     while still being considered in tolerance
347   * @param lowerToleranceRot            The number of rotations below the target
348   *                                     position the pivot can be
349   *                                     while still being considered in tolerance
350   * @param requiredInToleranceDurationS the duration (in seconds) the pivot must
351   *                                     stay in
352   *                                     tolerance
353   */
354  public PivotToAbsoluteCmd(
355      T pivot,
356      Angle goalPosition,
357      boolean holdPositionAfter,
358      double maxVelocityRPS,
359      double lowerToleranceRot,
360      double upperToleranceRot,
361      double requiredInToleranceDurationS
362  ) {
363    this(
364        pivot,
365        goalPosition.getRotations(),
366        holdPositionAfter,
367        maxVelocityRPS,
368        lowerToleranceRot,
369        upperToleranceRot,
370        requiredInToleranceDurationS
371    );
372  }
373
374  /**
375   * Full constructor with ID for specifying pivot
376   *
377   * @param id                           The pivot integer ID
378   * @param pivot                        Tagalong Subsystem containing a pivot
379   *                                     microsystem
380   * @param goalPosition                 The goal position to reach
381   * @param holdPositionAfter            If the pivot should hold position when
382   *                                     the command completes
383   * @param maxVelocityRPS               The maximum velocity of the pivot, in
384   *                                     rotations per second, during this
385   *                                     command
386   * @param lowerToleranceRot            The number of rotations below the target
387   *                                     position the pivot can be
388   *                                     while still being considered in tolerance
389   * @param upperToleranceRot            The number of rotations beyond the target
390   *                                     position the pivot can be
391   *                                     while still being considered in tolerance
392   * @param requiredInToleranceDurationS the duration (in seconds) the pivot must
393   *                                     stay in
394   *                                     tolerance
395   */
396  public PivotToAbsoluteCmd(
397      int id,
398      T pivot,
399      Angle goalPosition,
400      boolean holdPositionAfter,
401      double maxVelocityRPS,
402      double lowerToleranceRot,
403      double upperToleranceRot,
404      double requiredInToleranceDurationS
405  ) {
406    this(
407        id,
408        pivot,
409        goalPosition.getRotations(),
410        holdPositionAfter,
411        maxVelocityRPS,
412        lowerToleranceRot,
413        upperToleranceRot,
414        requiredInToleranceDurationS
415    );
416  }
417
418  /**
419   * Full constructor, allows for double for goal position (rotations) in rare cases of advance use
420   * needing a direct way to interact
421   *
422   * @param pivot                        Tagalong Subsystem containing a pivot
423   *                                     microsystem
424   * @param goalPositionRot              The goal position to reach, in rotations
425   * @param holdPositionAfter            If the pivot should hold position when
426   *                                     the command completes
427   * @param maxVelocityRPS               The maximum velocity of the pivot, in
428   *                                     rotations per second, during this
429   *                                     command
430   * @param lowerToleranceRot            The number of rotations below the target
431   *                                     position the pivot can be
432   *                                     while still being considered in tolerance
433   * @param upperToleranceRot            The number of rotations beyond the target
434   *                                     position the pivot can be
435   *                                     while still being considered in tolerance
436   * @param requiredInToleranceDurationS the duration (in seconds) the pivot must
437   *                                     stay in
438   *                                     tolerance
439   */
440  public PivotToAbsoluteCmd(
441      T pivot,
442      double goalPositionRot,
443      boolean holdPositionAfter,
444      double maxVelocityRPS,
445      double lowerToleranceRot,
446      double upperToleranceRot,
447      double requiredInToleranceDurationS
448  ) {
449    _pivot = pivot.getPivot();
450    _goalPositionRot = AlgebraicUtils.cppMod(goalPositionRot, 1.0);
451    _holdPositionAfter = holdPositionAfter;
452    _maxVelocityRPS = maxVelocityRPS;
453    _lowerBoundRot = AlgebraicUtils.cppMod(_goalPositionRot, 1.0) - Math.abs(lowerToleranceRot);
454    _upperBoundRot = AlgebraicUtils.cppMod(_goalPositionRot, 1.0) + Math.abs(upperToleranceRot);
455    _requiredInToleranceDurationS = requiredInToleranceDurationS;
456    _requireInTolerance = _requiredInToleranceDurationS >= 0.0;
457
458    addRequirements(pivot);
459  }
460
461  /**
462   * Full constructor, allows for double for goal position (rotations) in rare cases of advance use
463   * needing a direct way to interact
464   *
465   * @param id                           The pivot integer ID
466   * @param pivot                        Tagalong Subsystem containing a pivot
467   *                                     microsystem
468   * @param goalPositionRot              The goal position to reach, in rotations
469   * @param holdPositionAfter            If the pivot should hold position when
470   *                                     the command completes
471   * @param maxVelocityRPS               The maximum velocity of the pivot, in
472   *                                     rotations per second, during this
473   *                                     command
474   * @param lowerToleranceRot            The number of rotations below the target
475   *                                     position the pivot can be
476   *                                     while still being considered in tolerance
477   * @param upperToleranceRot            The number of rotations beyond the target
478   *                                     position the pivot can be
479   *                                     while still being considered in tolerance
480   * @param requiredInToleranceDurationS the duration (in seconds) the pivot must
481   *                                     stay in
482   *                                     tolerance
483   */
484  public PivotToAbsoluteCmd(
485      int id,
486      T pivot,
487      double goalPositionRot,
488      boolean holdPositionAfter,
489      double maxVelocityRPS,
490      double lowerToleranceRot,
491      double upperToleranceRot,
492      double requiredInToleranceDurationS
493  ) {
494    _pivot = pivot.getPivot(id);
495    _goalPositionRot = AlgebraicUtils.cppMod(goalPositionRot, 1.0);
496    _holdPositionAfter = holdPositionAfter;
497    _maxVelocityRPS = maxVelocityRPS;
498    _lowerBoundRot = AlgebraicUtils.cppMod(_goalPositionRot, 1.0) - Math.abs(lowerToleranceRot);
499    _upperBoundRot = AlgebraicUtils.cppMod(_goalPositionRot, 1.0) + Math.abs(upperToleranceRot);
500    _requiredInToleranceDurationS = requiredInToleranceDurationS;
501    _requireInTolerance = _requiredInToleranceDurationS >= 0.0;
502
503    addRequirements(pivot);
504  }
505}