current zeroing maybe

carokhan · Oct 10, 2024 · c4fb9b0 · c4fb9b0
1 parent 133b660
commit c4fb9b0
Show file tree

Hide file tree

Showing 6 changed files with 224 additions and 179 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
diff --git a/src/main/java/frc/robot/Visualizer.java b/src/main/java/frc/robot/Visualizer.java
@@ -28,6 +28,7 @@ public class Visualizer extends SubsystemBase {
     private MechanismLigament2d m_climberTarget;
     private MechanismLigament2d m_intakeTarget;
     private MechanismLigament2d m_pivotTarget;
+    private MechanismLigament2d m_pivotRelative;
 
     private MechanismRoot2d m_climberRoot;
     private MechanismRoot2d m_intakeRoot;
@@ -57,6 +58,7 @@ public Visualizer(Climb climber, Intake intake, Pivot pivot) {
         m_intakeTarget = m_intakeRoot.append(new MechanismLigament2d("Intake Target", Units.inchesToMeters(14.914264), 83.649627, 2, new Color8Bit(Color.kBlue)));
 
         m_pivotMech = m_pivotRoot.append(new MechanismLigament2d("Shooter", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kWhite)));
+        m_pivotRelative = m_pivotRoot.append(new MechanismLigament2d("Shooter Relative", Units.inchesToMeters(13.1001837), 12, 8, new Color8Bit(Color.kGray)));
         m_pivotTarget = m_pivotRoot.append(new MechanismLigament2d("Shooter Target", Units.inchesToMeters(13.1001837), 12, 2, new Color8Bit(Color.kBeige)));
 
         SmartDashboard.putData("Climb Up", (Sendable) m_climber.setExtensionCmd(() -> ClimbConstants.maxHeight));
@@ -80,6 +82,7 @@ public void periodic() {
         m_intakeTarget.setAngle(Units.radiansToDegrees(m_intake.getTargetRadians()));
 
         m_pivotMech.setAngle(Units.radiansToDegrees(m_pivot.getAngleRadians()));
+        m_pivotRelative.setAngle(Units.radiansToDegrees(m_pivot.getRelativeRadians()));
         m_pivotTarget.setAngle(Units.radiansToDegrees(m_pivot.getTargetRadians()));
 
         Logger.recordOutput("Visualizer/FullRobot", m_main);

diff --git a/src/main/java/frc/robot/subsystems/pivot2/Pivot.java b/src/main/java/frc/robot/subsystems/pivot2/Pivot.java
@@ -11,6 +11,9 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -48,9 +51,9 @@ public void periodic() {
   public Command setPivotTarget(DoubleSupplier radians) {
     return this.run(() -> {
       double volts = pivotPID.calculate(inputs.pivotPosition.getRadians(), radians.getAsDouble())
-      + pivotFF.calculate(pivotPID.getSetpoint().position,
-      pivotPID.getSetpoint().velocity)
-      ;
+          + pivotFF.calculate(pivotPID.getSetpoint().position,
+              pivotPID.getSetpoint().velocity);
+
       io.setPivotVoltage(volts);
       inputs.pivotAppliedVolts = volts;
       inputs.pivotTargetPosition = Rotation2d.fromRadians(radians.getAsDouble());
@@ -62,6 +65,7 @@ public Command setPivotVoltage(DoubleSupplier volts) {
     return this.run(
         () -> {
           io.setPivotVoltage(volts.getAsDouble());
+          inputs.pivotAppliedVolts = volts.getAsDouble();
         });
   }
 
@@ -79,4 +83,24 @@ public Command resetEncoder() {
           io.resetEncoder();
         });
   }
+
+  public double getRelativeRadians() {
+    return inputs.pivotRelativeEncoder.getRadians() + ShooterConstants.simOffset;
+  }
+
+  public boolean atSetpoint() {
+    return pivotPID.atSetpoint();
+  }
+
+  public boolean isStalled() {
+    return inputs.pivotStalled;
+  }
+
+  public Command runZero() {
+    return this.run(
+        () -> {
+          io.setPivotVoltage(-1);
+        }).until(() -> inputs.pivotStalled).finallyDo(() -> io.resetEncoder());
+  }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java
@@ -14,14 +14,15 @@ public interface PivotIO {
     @AutoLog
     public static class PivotIOInputs {
         public Rotation2d pivotPosition = new Rotation2d();
-        public double pivotRelativeEncoder = 0.0;
+        public Rotation2d pivotRelativeEncoder = new Rotation2d();
         public Rotation2d pivotAbsolutePosition = new Rotation2d();
         public Rotation2d pivotTargetPosition = new Rotation2d();
         public double pivotVelocityRadPerSec = 0.0;
         public double pivotAppliedVolts = 0.0;
         public double pivotCurrentAmps = 0.0;
         public double pivotTempCelsius = 0.0;
         public double pivotOffset = 0.0;
+        public boolean pivotStalled = false;
     }
 
     public abstract void processInputs(final PivotIOInputsAutoLogged inputs);

diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSim.java
@@ -3,6 +3,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -13,7 +15,8 @@
 import frc.robot.Constants.ShooterConstants;
 
 public class PivotIOSim implements PivotIO {
-    private SingleJointedArmSim pivotSim = new SingleJointedArmSim(DCMotor.getNEO(1), ShooterConstants.pivotRatio, ShooterConstants.pivotMOI, ShooterConstants.pivotLength, ShooterConstants.down, ShooterConstants.up, true, ShooterConstants.down);    
+    private SingleJointedArmSim pivotSim = new SingleJointedArmSim(DCMotor.getNEO(1), ShooterConstants.pivotRatio, ShooterConstants.pivotMOI, ShooterConstants.pivotLength, ShooterConstants.down, ShooterConstants.up, true, ShooterConstants.down); 
+	private Debouncer stallDebouncer = new Debouncer(0.2, DebounceType.kRising);
 
 	@Override
 	public void processInputs(PivotIOInputsAutoLogged inputs) {
@@ -22,6 +25,7 @@ public void processInputs(PivotIOInputsAutoLogged inputs) {
 		inputs.pivotPosition = Rotation2d.fromRadians(pivotSim.getAngleRads());
         inputs.pivotVelocityRadPerSec = Units.rotationsToRadians(pivotSim.getVelocityRadPerSec());
         inputs.pivotCurrentAmps = pivotSim.getCurrentDrawAmps();
+		inputs.pivotStalled = stallDebouncer.calculate(pivotSim.getCurrentDrawAmps() > 20);
 	}
 
 	@Override

diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIOSparkMax.java
@@ -15,6 +15,8 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.Constants.RobotMap;
 import frc.robot.Constants.ShooterConstants;
@@ -25,13 +27,15 @@ public class PivotIOSparkMax implements PivotIO {
     private final RelativeEncoder pivotEnc = pivot.getEncoder();
 	private final ThroughboreEncoder pivotAbs;
 
+	private Debouncer stallDebouncer = new Debouncer(0.2, DebounceType.kRising);
+
     public PivotIOSparkMax() {
         pivot.restoreFactoryDefaults();
 		pivotAbs = new ThroughboreEncoder(pivot.getAbsoluteEncoder(), pivot.getAbsoluteEncoder().getPosition());
 
         pivot.setSmartCurrentLimit(ShooterConstants.pivotCurrentLimit);
         pivot.setIdleMode(IdleMode.kCoast);
-		
+
         pivotAbs.abs.setInverted(true);
 		pivot.setInverted(true);
         pivotAbs.abs.setPositionConversionFactor(ShooterConstants.pivotAbsConversion);
@@ -49,11 +53,13 @@ public PivotIOSparkMax() {
 	public void processInputs(PivotIOInputsAutoLogged inputs) {
 		inputs.pivotPosition = Rotation2d.fromRadians(pivotAbs.getPosition());
 		inputs.pivotAbsolutePosition = Rotation2d.fromRadians(pivotAbs.abs.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);
 	}
 
 	@Override