diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7c3f2ca..d05ab05 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -565,5 +565,8 @@ public static final class AutoAlign { public static final double AMP_TARGET_ANGLE = 0; public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)); - } + } + public static class Elevator{ //Placeholder elevator constant (update as needed) + public static final double GEAR_RATIO = 0; + } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a632e20..786c04d 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -1,5 +1,67 @@ package frc.robot.subsystems; -public class ElevatorSubsystem { - +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.Elevator; + +// private PIDController controller; + +public class ElevatorSubsystem extends SubsystemBase { + + private TalonFX motor; + private PIDController controller; + private double targetHeight; + private double motorPower; + private double currentHeight = 0; + private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); + + /** Creates a new ElevatorSubsystem. */ + public ElevatorSubsystem() { + + motor = new TalonFX(15); + + motor.clearStickyFaults(); + + controller = new PIDController(0.4, 0, 0.0125); + + ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); + ElevatorTab.addNumber("Target Height", () -> this.targetHeight); + // ElevatorTab.addNumber("PID output", () -> this.controller); + ElevatorTab.addNumber("Current Height", () -> this.currentHeight); + ElevatorTab.add(controller); + + targetHeight = 0; + currentHeight = 0; + } + + public static double inchesToRotations(double height) { + return height / Elevator.GEAR_RATIO; + } + + public static double rotationsToInches(double rotations) { + return rotations * Elevator.GEAR_RATIO; + } + + public void setTargetHeight(double targetHeight) { + this.targetHeight = targetHeight; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + motorPower = controller.calculate(rotationsToInches(0), targetHeight); //Placeholder for rotations + + } + + public boolean nearTargetHeight() { + if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) { + return true; + } + return false; + } }