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.aim;
008
009import edu.wpi.first.math.geometry.Translation2d;
010import java.util.function.DoubleSupplier;
011import java.util.function.Supplier;
012import tagalong.commands.base.PivotToDynamicCmd;
013import tagalong.math.AlgebraicUtils;
014import tagalong.subsystems.TagalongSubsystemBase;
015import tagalong.subsystems.micro.augments.PivotAugment;
016
017/**
018 * Command that continuously points at a target in 2d space.
019 * For rotational targeting give robot position as (X, Y) pose
020 * For vertical targets give (distance from target, mechanism height)
021 */
022public class PivotAimAtCmd<T extends TagalongSubsystemBase & PivotAugment>
023    extends PivotToDynamicCmd<T> {
024  @SuppressWarnings("unused")
025  /**
026   * Target to aim at
027   */
028  private final Translation2d _target;
029  @SuppressWarnings("unused")
030  /**
031   * Robot location supplier (x, y)
032   */
033  private final Supplier<Translation2d> _locationSupplier;
034
035  /**
036   * Transform position into an optimal path angle to target, ignores yaw while respecting the
037   * system's positional limits
038   *
039   * @param location Current robot location supplier
040   * @param pivotPosition Pivot position supplier
041   * @param target Target to aim at's location
042   * @param pivotMinRot pivots minimum position in rotations
043   * @param pivotMaxRot pivots maximum position in rotations
044   * @return Optimal path target angle for the pivot
045   */
046  protected static DoubleSupplier positionTransform(
047      Supplier<Translation2d> location,
048      DoubleSupplier pivotPosition,
049      final Translation2d target,
050      final double pivotMinRot,
051      final double pivotMaxRot
052  ) {
053    return () -> {
054      Translation2d curPos = location.get();
055      double goalRot = Math.atan((target.getX() - curPos.getX()) / (target.getY() - curPos.getY()));
056      double scopedAngle = AlgebraicUtils.placeInScopeRot(pivotPosition.getAsDouble(), goalRot);
057      if (scopedAngle > pivotMaxRot) {
058        scopedAngle -= 1.0;
059      }
060      if (scopedAngle < pivotMinRot) {
061        scopedAngle += 1.0;
062      }
063
064      return AlgebraicUtils.clamp(scopedAngle, pivotMinRot, pivotMaxRot);
065    };
066  }
067
068  /**
069   * Minimal constructor with default parameters
070   *
071   * @param id                Integer ID of the pivot microsystem inside the
072   *                          Tagalong Subsystem
073   * @param pivot            Tagalong Subsystem containing a pivot microsystem
074   * @param positionSupplier  Supplies the current robot position position
075   * @param target            Unmoving 2-dimensional target location
076   */
077  public PivotAimAtCmd(
078      int id, T pivot, Supplier<Translation2d> positionSupplier, Translation2d target
079  ) {
080    this(id, pivot, positionSupplier, target, true);
081  }
082
083  /**
084   * Minimal constructor with default parameters
085   *
086   * @param pivot            Tagalong Subsystem containing a pivot microsystem
087   * @param positionSupplier  Supplies the current robot position position
088   * @param target            Unmoving 2-dimensional target location
089   */
090  public PivotAimAtCmd(T pivot, Supplier<Translation2d> positionSupplier, Translation2d target) {
091    this(pivot, positionSupplier, target, true);
092  }
093
094  /**
095   * Constructor that creates the command with the below parameters.
096   *
097   * @param id                Integer ID of the pivot microsystem inside the
098   *                          Tagalong Subsystem
099   * @param pivot            Tagalong Subsystem containing a pivot microsystem
100   * @param positionSupplier  Supplies the current robot position position
101   * @param target            Unmoving 2-dimensional target location
102   * @param holdPositionAfter If the pivot should hold position when the command completes
103   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per second, during
104   *     this command
105   */
106  public PivotAimAtCmd(
107      int id,
108      T pivot,
109      Supplier<Translation2d> positionSupplier,
110      Translation2d target,
111      boolean holdPositionAfter,
112      double maxVelocityRPS
113  ) {
114    super(
115        id,
116        pivot,
117        positionTransform(
118            positionSupplier,
119            pivot.getPivot(id)::getPivotPosition,
120            target,
121            pivot.getPivot(id)._minPositionRot,
122            pivot.getPivot(id)._maxPositionRot
123        ),
124        holdPositionAfter,
125        maxVelocityRPS
126    );
127    _target = target;
128    _locationSupplier = positionSupplier;
129  }
130
131  /**
132   * Constructor that creates the command with the below parameters.
133   *
134   * @param pivot            Tagalong Subsystem containing a pivot microsystem
135   * @param positionSupplier  Supplies the current robot position position
136   * @param target            Unmoving 2-dimensional target location
137   * @param holdPositionAfter If the pivot should hold position when the command completes
138   * @param maxVelocityRPS    The maximum velocity of the pivot, in rotations per second, during
139   *     this command
140   */
141  public PivotAimAtCmd(
142      T pivot,
143      Supplier<Translation2d> positionSupplier,
144      Translation2d target,
145      boolean holdPositionAfter,
146      double maxVelocityRPS
147  ) {
148    super(
149        pivot,
150        positionTransform(
151            positionSupplier,
152            pivot.getPivot()::getPivotPosition,
153            target,
154            pivot.getPivot()._minPositionRot,
155            pivot.getPivot()._maxPositionRot
156        ),
157        holdPositionAfter,
158        maxVelocityRPS
159    );
160    _target = target;
161    _locationSupplier = positionSupplier;
162  }
163
164  /**
165   * Constructor that creates the command with the below parameters.
166   *
167   * @param id                Integer ID of the pivot microsystem inside the
168   *                          Tagalong Subsystem
169   * @param pivot            Tagalong Subsystem containing a pivot microsystem
170   * @param positionSupplier  Supplies the current robot position position
171   * @param target            Unmoving 2-dimensional target location
172   * @param holdPositionAfter If the pivot should hold position when the command completes
173   */
174  public PivotAimAtCmd(
175      int id,
176      T pivot,
177      Supplier<Translation2d> positionSupplier,
178      Translation2d target,
179      boolean holdPositionAfter
180  ) {
181    this(
182        id, pivot, positionSupplier, target, holdPositionAfter, pivot.getPivot(id)._maxVelocityRPS
183    );
184  }
185
186  /**
187   * Constructor that creates the command with the below parameters.
188   *
189   * @param pivot            Tagalong Subsystem containing a pivot microsystem
190   * @param positionSupplier  Supplies the current robot position position
191   * @param target            Unmoving 2-dimensional target location
192   * @param holdPositionAfter If the pivot should hold position when the command completes
193   */
194  public PivotAimAtCmd(
195      T pivot,
196      Supplier<Translation2d> positionSupplier,
197      Translation2d target,
198      boolean holdPositionAfter
199  ) {
200    this(pivot, positionSupplier, target, holdPositionAfter, pivot.getPivot()._maxVelocityRPS);
201  }
202}