diff --git a/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java b/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java index f72ef18..c1790f3 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java @@ -3,20 +3,20 @@ import static com.team2813.Constants.INTAKE_PIVOT; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.team2813.lib2813.control.Encoder; import com.team2813.lib2813.control.InvertType; -import com.team2813.lib2813.control.Motor; import com.team2813.lib2813.control.PIDMotor; import com.team2813.lib2813.control.encoders.CancoderWrapper; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.subsystems.MotorSubsystem; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.function.Supplier; + public class IntakePivot extends MotorSubsystem { - - Motor intakePivotMotor; - Encoder intakePivotEncoder; public IntakePivot() { @@ -27,8 +27,6 @@ public IntakePivot() { .PID(0.313, 0, 0) .acceptableError(0.5) .startingPosition(Rotations.INTAKE_UP)); - - intakePivotMotor = new TalonFXWrapper(INTAKE_PIVOT, InvertType.COUNTER_CLOCKWISE); } public void resetPosition() { @@ -56,7 +54,7 @@ public void periodic() { SmartDashboard.putNumber("Intake Pivot CANCoder Position", encoder.position()); } - public static enum Rotations implements MotorSubsystem.Position { + public enum Rotations implements Supplier> { INTAKE_DOWN(0.825439), INTAKE_UP(0); @@ -65,12 +63,17 @@ public static enum Rotations implements MotorSubsystem.Position { } private final double pos; - @Override + + @Deprecated public double getPos() { return pos; } - - } + + @Override + public Measure get() { + return Units.Rotations.of(pos); + } + } } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java index b84442f..12d8b9f 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java @@ -14,8 +14,13 @@ import com.team2813.lib2813.subsystems.MotorSubsystem; import com.team2813.lib2813.util.ConfigUtils; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.function.Supplier; + public class ShooterPivot extends MotorSubsystem { public ShooterPivot() { @@ -62,7 +67,7 @@ public void periodic() { SmartDashboard.putNumber("Shoooter Pivot CANCoder Position", encoder.position()); } - public static enum Position implements MotorSubsystem.Position { + public static enum Position implements Supplier> { TOP_HARD_STOP(0), SUBWOOFER_FRONT(0.023926), SUBWOOFER_SIDE(0.023926), @@ -80,9 +85,14 @@ public static enum Position implements MotorSubsystem.Position { this.pos = pos; } - @Override + @Deprecated public double getPos() { return pos; } + + @Override + public Measure get() { + return Units.Rotations.of(pos); + } } }