diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 94a7615..dea90db 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Forte-2-5"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 53; - public static final String GIT_SHA = "b44357c75fc5d008c8de9056a477ce15fae23851"; - public static final String GIT_DATE = "2024-10-08 19:18:00 EDT"; + public static final int GIT_REVISION = 57; + public static final String GIT_SHA = "3bce2b6cdf6012df5e1c4645e0fb0e2b16b02ffd"; + public static final String GIT_DATE = "2024-10-10 17:55:55 EDT"; public static final String GIT_BRANCH = "pivot2"; - public static final String BUILD_DATE = "2024-10-09 19:20:41 EDT"; - public static final long BUILD_UNIX_TIME = 1728516041419L; + public static final String BUILD_DATE = "2024-10-10 22:49:28 EDT"; + public static final long BUILD_UNIX_TIME = 1728614968375L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61a0f56..90220dc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ public final class Constants { public static final double loopPeriodSecs = 0.02; - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.REAL; public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9437114..ded3f4b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,13 +218,15 @@ private void configureButtonBindings() { Commands.parallel( m_feeder.setVoltage(() -> 0), m_shooter.stopShooter(), - Commands.sequence(m_pivot.setPivotTarget(() -> ShooterConstants.down) + Commands.sequence(m_pivot.setPivotVoltage(() -> -1))) .until(() -> m_pivot.atSetpoint() || m_pivot.isStalled()) .andThen( Commands.either(m_pivot.resetEncoder(), m_pivot.runZero(), - () -> m_pivot.isStalled()))))); + () -> m_pivot.isStalled()).andThen(m_pivot.setPivotTarget(() -> m_pivot.getAngleRadians()).andThen( + m_pivot.setPivotVoltage(() -> 0)) + ))); // m_operator.rightBumper().whileTrue( // m_pivot.runCurrentZeroing() @@ -253,7 +255,9 @@ private void configureButtonBindings() { () -> Units.degreesToRadians(0.0))))); m_operator.leftTrigger(0.1).onTrue( - m_shooter.setRPM(() -> 5800, 0.3)) + Commands.parallel(m_shooter.setRPM(() -> 5800, 0.3), + m_feeder.setRPM(() -> 2000), + m_intake.setRollerRPM(() -> 2000))) .onFalse(m_shooter.stopShooter() .andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)))); diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java index 9f20d42..b2fd87c 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java @@ -47,19 +47,23 @@ public PivotIOSparkMax() { pivotEnc.setPosition(pivotAbs.abs.getPosition()); pivot.burnFlash(); + + if ((Math.abs(pivotAbs.getPosition()) % (2 * Math.PI)) > 0.1) { + pivotAbs.setOffset(pivotAbs.getOffset() + pivotAbs.getPosition()); + } } @Override public void processInputs(PivotIOInputsAutoLogged inputs) { inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition()); - inputs.pivotAbsolutePosition = Rotation2d.fromRadians(pivotAbs.abs.getPosition()); + inputs.pivotAbsolutePosition = Rotation2d.fromRadians(pivotAbs.getPosition()); inputs.pivotRelativeEncoder = Rotation2d.fromRadians(pivotEnc.getPosition()); inputs.pivotVelocityRadPerSec = pivotEnc.getVelocity(); inputs.pivotAppliedVolts = pivot.getAppliedOutput() * pivot.getBusVoltage(); inputs.pivotCurrentAmps = pivot.getOutputCurrent(); inputs.pivotTempCelsius = pivot.getMotorTemperature(); inputs.pivotOffset = pivotAbs.getOffset(); - inputs.pivotStalled = stallDebouncer.calculate((pivot.getOutputCurrent() > 20) && (pivotEnc.getVelocity() > )); + inputs.pivotStalled = stallDebouncer.calculate((pivot.getOutputCurrent() > 15) && (Math.abs(pivotEnc.getVelocity()) < 0.02)); } @Override diff --git a/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java b/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java index 5a3ef3c..a234e57 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java +++ b/src/main/java/frc/robot/subsystems/pivot2/ThroughboreEncoder.java @@ -12,7 +12,11 @@ public ThroughboreEncoder(SparkAbsoluteEncoder absoluteEncoder, double absOffset } public double getPosition() { - return (abs.getPosition() + offset) % (2 * Math.PI); + return (abs.getPosition() - offset) % (2 * Math.PI); + } + + public double getAbsPosition() { + return abs.getPosition(); } public void setOffset(double absOffset) {