diff --git a/src/main/java/frc/robot/commands/zeroing/BaseZeroing.java b/src/main/java/frc/robot/commands/zeroing/BaseZeroing.java new file mode 100644 index 0000000..4ba77fa --- /dev/null +++ b/src/main/java/frc/robot/commands/zeroing/BaseZeroing.java @@ -0,0 +1,142 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.zeroing; + +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class BaseZeroing extends Command { + boolean zeroingSuccess = false; + Timer zeroingTimer = new Timer(); + AngularVelocity lastRotorVelocity; + AngularVelocity currentRotorVelocity; + TalonFX motor; + BooleanSupplier attemptingZeroing; + Consumer setAttemptingZeroing; + Consumer setHasZeroed; + String name; + double zeroedPosition; + + /** + * + * Constructs a BaseZeroing instance which encapsulates the logic and state + * needed + * to zero a single TalonFX motor. + * + *

+ * This constructor stores the supplied motor and state callbacks and registers + * a dependency on the shared motion subsystem (RobotContainer.motionInstance). + * The provided {@code attemptingZeroing} supplier and + * {@code setAttemptingZeroing} + * / {@code setHasZeroed} consumers are retained as callbacks for reading and + * updating zeroing state during the command's lifecycle. + * + * @param mainMotor the TalonFX *MASTER* motor instance that this + * command will attempt to zero + * @param attemptingZeroing a BooleanSupplier that returns true when a + * zeroing attempt is in progress (used to query + * current attempt state) + * @param setAttemptingZeroing a Consumer invoked to mark whether a + * zeroing attempt is being started or stopped + * @param setHasZeroed a Consumer invoked to record whether the + * motor + * has successfully been zeroed + * @param name a name for this zeroing mechanism (e.g. + * "Elevator" or "Pivot") + * (used for logging or telemetry) + * + * @param zeroedPosition the encoder position that corresponds to the + * zeroed state of the motor (used to set the + * motor's position to the zeroed value) + * + * @implNote the constructor retains references to the supplied objects (does + * not + * make defensive copies), so callers should ensure the supplied + * callbacks and + * motor remain valid for the command's lifetime. The constructor also + * calls + * addRequirements(RobotContainer.motionInstance) to declare a + * dependency on the + * motion subsystem. + * + */ + public BaseZeroing(TalonFX mainMotor, + BooleanSupplier attemptingZeroing, + Consumer setAttemptingZeroing, + Consumer setHasZeroed, + String name, + double zeroedPosition) { + this.motor = mainMotor; + this.attemptingZeroing = attemptingZeroing; + this.setAttemptingZeroing = setAttemptingZeroing; + this.setHasZeroed = setHasZeroed; + this.name = name; + this.zeroedPosition = zeroedPosition; + addRequirements(RobotContainer.motionInstance); + } + + @Override + public void initialize() { + zeroingSuccess = false; + zeroingTimer.reset(); + } + + @Override + public void execute() { + // Check if we have raised the elevator above a certain speed + currentRotorVelocity = motor.getVelocity().getValue(); + if (currentRotorVelocity.gte(Units.RotationsPerSecond.of(0.5)) + || attemptingZeroing.getAsBoolean()) { + // Enter zeroing mode! + if (!attemptingZeroing.getAsBoolean()) { + setAttemptingZeroing.accept(true); + zeroingTimer.start(); + System.out.println(name + " Zeroing Started!"); + } + + // Check if time elapsed is too high (zeroing timeout) + if (zeroingTimer.hasElapsed(3.0)) { + setAttemptingZeroing.accept(false); + System.out.println(name + " Zeroing Failed :("); + } else { + boolean deltaRotorVelocity = motor.getVelocity().getValue().minus(lastRotorVelocity) + .lte(Units.RotationsPerSecond.of(-0.5)); + + if (deltaRotorVelocity && lastRotorVelocity.lte(Units.RotationsPerSecond.of(0))) { + zeroingSuccess = true; + } else { + lastRotorVelocity = motor.getVelocity().getValue(); + } + } + } + } + + @Override + public void end(boolean interrupted) { + if (!interrupted) { + setHasZeroed.accept(true); + motor.setPosition(zeroedPosition); + System.out.println(name + " Zeroing Successful!!!! Yippee and hooray!!! :3"); + } else { + System.out.println(name + " was never zeroed :((( blame eli"); + } + } + + @Override + public boolean isFinished() { + boolean rotorVelocityIsZero = motor.getVelocity().getValue().isNear(Units.RotationsPerSecond.zero(), 0.01); + SmartDashboard.putBoolean("Zeroing/Pivot/Is Rotor Velocity Zero", rotorVelocityIsZero); + return zeroingSuccess && rotorVelocityIsZero; + } +} \ No newline at end of file