temporary Algae grabber changes/fixes to try next meeting

team20 · Feb 8, 2025 · b3d9959 · b3d9959
1 parent dfb354b
commit b3d9959
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -11,22 +11,22 @@
 
 public class Constants {
 	public static final class AlgaeConstants {
-		// FLYWHEEL IS 550
-		// OTHER IS NORMAL NEO
-
-		public static final int kFlywheelMotorPort = 90;
-		public static final int kGrabberAnglePort = 3;
-		public static final boolean kFlywheelInvert = false;
+		public static final int kFlywheelMotorPort = 55;
+		public static final int kGrabberAnglePort = 56;
+		public static final boolean kFlywheelInvert = true;
 		public static final boolean kGrabberAngleInvert = false;
 
+		public static final int k550SmartCurrentLimit = 15;
+
 		public static final int kSmartCurrentLimit = 50;
 		public static final int kPeakCurrentDurationMillis = 100;
 
-		public static final int k550SmartCurrentLimit = 15;
-
 		public static final double kCurrentToStop = 20;
 		public static final double kTimeOverCurrentToStop = .01;
 
+		public static final float kDeployGrabberRotations = 2;
+		public static final double kFlywheelSpeed = .8;
+
 		public static final double kP = 0.5;
 		public static final double kI = 0.0;
 		public static final double kD = 0;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -73,6 +73,7 @@ public Robot() {
 		bindDriveControls();
 		bindElevatorControls();
 		bindWristControls();
+		bindAlgaeControls();
 	}
 
 	public void bindDriveControls() {
@@ -96,13 +97,18 @@ public void bindElevatorControls() {
 	}
 
 	public void bindAlgaeControls() {
+		// TODO: needs to be tested
 		m_operatorController.R1().onTrue(
 				m_algaeGrabberSubsystem.deployGrabber(GrabberState.DOWN)
-						.andThen(m_algaeGrabberSubsystem.runFlywheel())); // TODO: Come up after?
+						.andThen(m_algaeGrabberSubsystem.runFlywheel()).until(
+								() -> m_algaeGrabberSubsystem.checkCurrentOnFlywheel()));
 		m_operatorController.L1().onTrue(
-				m_algaeGrabberSubsystem.runFlywheelReverse()
-						.until(m_algaeGrabberSubsystem::checkCurrentOnFlywheel)
+				m_algaeGrabberSubsystem.deployGrabber(GrabberState.UP)
 						.andThen(m_algaeGrabberSubsystem.stopFlywheel()));
+		m_operatorController.options().onTrue(
+				m_algaeGrabberSubsystem.runFlywheelReverse());
+		m_operatorController.options().onFalse(
+				m_algaeGrabberSubsystem.stopFlywheel());
 	}
 
 	public void bindWristControls() {

diff --git a/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java
@@ -26,7 +26,7 @@ public class AlgaeGrabberSubsystem extends SubsystemBase {
 	private final SparkClosedLoopController m_grabberClosedLoopController = m_grabberAngleMotor
 			.getClosedLoopController();
 
-	private final Debouncer m_debouncerCurrentLimitStop = new Debouncer(AlgaeConstants.kTimeOverCurrentToStop);
+	Debouncer m_debouncerCurrentLimitStop;
 
 	private double m_setVelocity;
 
@@ -36,6 +36,8 @@ public enum GrabberState {
 	}
 
 	public AlgaeGrabberSubsystem() {
+		SparkMaxConfig config;
+		m_debouncerCurrentLimitStop = new Debouncer(AlgaeConstants.kTimeOverCurrentToStop);
 		m_flywheel = new SparkMax(AlgaeConstants.kFlywheelMotorPort, MotorType.kBrushless);
 
 		// Initialize Motors
@@ -44,7 +46,7 @@ public AlgaeGrabberSubsystem() {
 		// variable
 		// also applies voltage and current stuff to the motors
 
-		var config = new SparkMaxConfig();
+		config = new SparkMaxConfig();
 		config.inverted(AlgaeConstants.kFlywheelInvert).idleMode(IdleMode.kBrake);
 		config.voltageCompensation(12).smartCurrentLimit(AlgaeConstants.k550SmartCurrentLimit);
 		m_flywheel.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
@@ -64,8 +66,14 @@ public void periodic() {
 		// System.out.println(m_flywheel.getOutputCurrent());
 	}
 
+	/**
+	 * gets the % velocity on the flywheel motor
+	 *
+	 * @return (double) (-1 to 1) returns the % velocity using .getAppliedOutput()
+	 *         from {@link SparkMax}
+	 */
 	public double getVelocity() {
-		return m_flywheel.getAbsoluteEncoder().getVelocity();
+		return m_flywheel.getAppliedOutput();
 	}
 
 	/**
@@ -97,7 +105,7 @@ public boolean checkCurrentOnFlywheel() {
 	/**
 	 * command to stop the algae flywheel using the setVelocity method
 	 *
-	 * @return (setVelocity(0)) sets the flywheel velocity to 0
+	 * @return (setVelocity(0)) sets the flywheel velocity to 0%
 	 */
 	public Command stopFlywheel() {
 		return runOnce(() -> {
@@ -108,22 +116,24 @@ public Command stopFlywheel() {
 	/**
 	 * command to start the algae flywheel using the setVelocity method
 	 *
-	 * @return (setVelocity(.75)) sets the flywheel velocity to 75%
+	 * @return (setVelocity(kFlywheelSpeed)) sets the flywheel velocity to the
+	 *         constant kFlywheelSpeed in AlgaeConstants
 	 */
 	public Command runFlywheel() {
 		return runOnce(() -> {
-			setVelocity(.75);
+			setVelocity(AlgaeConstants.kFlywheelSpeed);
 		});
 	}
 
 	/**
 	 * command to reverse the algae flywheel using the setVelocity method
 	 *
-	 * @return (setVelocity(-.75)) sets the flywheel velocity to -75%
+	 * @return (setVelocity(-kFlywheelSpeed)) sets the flywheel velocity to the
+	 *         negative constant kFlywheelSpeed in AlgaeConstants
 	 */
 	public Command runFlywheelReverse() {
 		return runOnce(() -> {
-			setVelocity(-.75);
+			setVelocity(-AlgaeConstants.kFlywheelSpeed);
 		});
 	}
 
@@ -139,10 +149,11 @@ public Command runFlywheelReverse() {
 	public Command deployGrabber(GrabberState state) {
 		return runOnce(() -> {
 			if (GrabberState.DOWN == state) {
-				m_grabberClosedLoopController.setReference(2, ControlType.kPosition);
+				m_grabberClosedLoopController
+						.setReference(AlgaeConstants.kDeployGrabberRotations, ControlType.kPosition);
 			} else if (GrabberState.UP == state) {
 				m_grabberClosedLoopController.setReference(0, ControlType.kPosition);
 			}
 		});
 	}
-}
+}