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 edu.wpi.first.wpilibj.Timer;
010import tagalong.commands.TagalongCommand;
011import tagalong.subsystems.TagalongSubsystemBase;
012import tagalong.subsystems.micro.Elevator;
013import tagalong.subsystems.micro.augments.ElevatorAugment;
014
015/**
016 * Command that finds the elevator zero position and sets the encoder position
017 * to zero.
018 */
019public class ElevatorZeroCmd<T extends TagalongSubsystemBase & ElevatorAugment>
020    extends TagalongCommand {
021  /**
022   * Elevator subsystem.
023   */
024  private final Elevator _elevator;
025  /**
026   * The height of the elevator when initialized, in meters.
027   */
028  private double _prevHeightM;
029  /**
030   * Timer to track the stall duration to ensure a true bottom
031   */
032  private Timer stallTimer = new Timer();
033
034  /**
035   * Construct the command according to the below parameters.
036   *
037   * @param elevator the elevator subsystem
038   */
039  public ElevatorZeroCmd(T elevator) {
040    _elevator = elevator.getElevator();
041    addRequirements(elevator);
042  }
043
044  /**
045   * Construct the command according to the below parameters.
046   *
047   * @param id       Integer ID of the elevator microsystem inside the Tagalong
048   *                 Subsystem
049   * @param elevator the elevator subsystem
050   */
051  public ElevatorZeroCmd(int id, T elevator) {
052    _elevator = elevator.getElevator(id);
053    addRequirements(elevator);
054  }
055
056  @Override
057  public void initialize() {
058    _elevator.setPrimaryPower(_elevator._elevatorZeroingPower);
059    _prevHeightM = _elevator.getElevatorHeightM();
060    stallTimer.reset();
061  }
062
063  @Override
064  public void execute() {
065    double currentHeightM = _elevator.getElevatorHeightM();
066    if (Math.abs(_prevHeightM - currentHeightM) > _elevator._elevatorZeroingStallToleranceM) {
067      stallTimer.reset();
068    } else {
069      stallTimer.start();
070    }
071    _prevHeightM = currentHeightM;
072  }
073
074  @Override
075  public void end(boolean interrupted) {
076    _elevator.setPrimaryPower(0.0);
077    if (!interrupted)
078      _elevator.setElevatorHeight(0.0);
079  }
080
081  @Override
082  public boolean isFinished() {
083    return stallTimer.hasElapsed(_elevator._elevatorZeroingDurationS);
084  }
085}