YAY

carokhan · Oct 11, 2024 · 561a04a · 561a04a
1 parent 3bce2b6
commit 561a04a
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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) {