From c4fb9b0f8886eb64faa932bb9322e84b3cdb40cc Mon Sep 17 00:00:00 2001 From: Apoorva Saurav Date: Thu, 10 Oct 2024 07:12:48 -0400 Subject: [PATCH] current zeroing maybe --- src/main/java/frc/robot/RobotContainer.java | 353 +++++++++--------- src/main/java/frc/robot/Visualizer.java | 3 + .../frc/robot/subsystems/pivot2/Pivot.java | 30 +- .../frc/robot/subsystems/pivot2/PivotIO.java | 3 +- .../robot/subsystems/pivot2/PivotIOSim.java | 6 +- .../subsystems/pivot2/PivotIOSparkMax.java | 8 +- 6 files changed, 224 insertions(+), 179 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ad601e9..9437114 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,10 +20,12 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotMap; +import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.climber.Climb; import frc.robot.subsystems.climber.ClimbIOReplay; import frc.robot.subsystems.climber.ClimbIOSim; @@ -65,202 +67,207 @@ */ public class RobotContainer { - // Subsystems - // private final SwerveSubsystem m_drive = new SwerveSubsystem(new - // File(Filesystem.getDeployDirectory(), - // "swerve/swerve"));; - private final Drive m_drive; - private final Climb m_climber; - private final Intake m_intake; - private final Feeder m_feeder; - private final Pivot m_pivot; - private final Shooter m_shooter; - private final Visualizer m_visualizer; + // Subsystems + // private final SwerveSubsystem m_drive = new SwerveSubsystem(new + // File(Filesystem.getDeployDirectory(), + // "swerve/swerve"));; + private final Drive m_drive; + private final Climb m_climber; + private final Intake m_intake; + private final Feeder m_feeder; + private final Pivot m_pivot; + private final Shooter m_shooter; + private final Visualizer m_visualizer; - // Controller - private final CommandXboxController m_driver = new CommandXboxController(0); - private final CommandXboxController m_operator = new CommandXboxController(1); + // Controller + private final CommandXboxController m_driver = new CommandXboxController(0); + private final CommandXboxController m_operator = new CommandXboxController(1); - // final CommandXboxController driverXbox = new CommandXboxController(0); - // private final SwerveSubsystem drivebase + // final CommandXboxController driverXbox = new CommandXboxController(0); + // private final SwerveSubsystem drivebase - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - switch (Constants.currentMode) { - case REAL: - // Real robot, instantiate hardware IO implementations - m_drive = new Drive( - new GyroIOPigeon2Phoenix6(), - new ModuleIOReal(0), - new ModuleIOReal(1), - new ModuleIOReal(2), - new ModuleIOReal(3)); - m_climber = new Climb(new ClimbIOSparkMax()); - m_intake = new Intake(new IntakeIOSparkMax()); - m_feeder = new Feeder(new FeederIOSparkMax(), new BeambreakIOReal(RobotMap.Shooter.feederBeambreak), - new BeambreakIOReal(RobotMap.Shooter.shooterBeambreak)); - m_pivot = new Pivot(new PivotIOSparkMax()); - m_shooter = new Shooter(new ShooterIOSparkMax()); - break; + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + switch (Constants.currentMode) { + case REAL: + // Real robot, instantiate hardware IO implementations + m_drive = new Drive( + new GyroIOPigeon2Phoenix6(), + new ModuleIOReal(0), + new ModuleIOReal(1), + new ModuleIOReal(2), + new ModuleIOReal(3)); + m_climber = new Climb(new ClimbIOSparkMax()); + m_intake = new Intake(new IntakeIOSparkMax()); + m_feeder = new Feeder(new FeederIOSparkMax(), + new BeambreakIOReal(RobotMap.Shooter.feederBeambreak), + new BeambreakIOReal(RobotMap.Shooter.shooterBeambreak)); + m_pivot = new Pivot(new PivotIOSparkMax()); + m_shooter = new Shooter(new ShooterIOSparkMax()); + break; - case SIM: - // Sim robot, instantiate physics sim IO implementations - m_drive = new Drive( - new GyroIOReplay() { - }, - new ModuleIOSim("FrontLeft"), - new ModuleIOSim("FrontRight"), - new ModuleIOSim("BackLeft"), - new ModuleIOSim("BackRight")); - m_climber = new Climb(new ClimbIOSim()); - m_intake = new Intake(new IntakeIOSim()); - m_feeder = new Feeder(new FeederIOSim(), new BeambreakIOSim(RobotMap.Shooter.feederBeambreak), - new BeambreakIOSim(RobotMap.Shooter.shooterBeambreak)); - m_pivot = new Pivot(new PivotIOSim()); - m_shooter = new Shooter(new ShooterIOSim()); - break; + case SIM: + // Sim robot, instantiate physics sim IO implementations + m_drive = new Drive( + new GyroIOReplay() { + }, + new ModuleIOSim("FrontLeft"), + new ModuleIOSim("FrontRight"), + new ModuleIOSim("BackLeft"), + new ModuleIOSim("BackRight")); + m_climber = new Climb(new ClimbIOSim()); + m_intake = new Intake(new IntakeIOSim()); + m_feeder = new Feeder(new FeederIOSim(), + new BeambreakIOSim(RobotMap.Shooter.feederBeambreak), + new BeambreakIOSim(RobotMap.Shooter.shooterBeambreak)); + m_pivot = new Pivot(new PivotIOSim()); + m_shooter = new Shooter(new ShooterIOSim()); + break; - default: - // Replayed robot, disable IO implementations - m_drive = new Drive( - new GyroIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }, - new ModuleIOReplay() { - }); - m_climber = new Climb(new ClimbIOReplay()); - m_intake = new Intake(new IntakeIOReplay()); - m_feeder = new Feeder(new FeederIOReplay(), new BeambreakIOReplay(), new BeambreakIOReplay()); - m_pivot = new Pivot(new PivotIOReplay()); - m_shooter = new Shooter(new ShooterIOReplay()); - break; + default: + // Replayed robot, disable IO implementations + m_drive = new Drive( + new GyroIOReplay() { + }, + new ModuleIOReplay() { + }, + new ModuleIOReplay() { + }, + new ModuleIOReplay() { + }, + new ModuleIOReplay() { + }); + m_climber = new Climb(new ClimbIOReplay()); + m_intake = new Intake(new IntakeIOReplay()); + m_feeder = new Feeder(new FeederIOReplay(), new BeambreakIOReplay(), + new BeambreakIOReplay()); + m_pivot = new Pivot(new PivotIOReplay()); + m_shooter = new Shooter(new ShooterIOReplay()); + break; + } + m_visualizer = new Visualizer(m_climber, m_intake, m_pivot); + // Configure the button bindings + configureButtonBindings(); } - m_visualizer = new Visualizer(m_climber, m_intake, m_pivot); - // Configure the button bindings - configureButtonBindings(); - } - /** - * Use this method to define your button->command mappings. Buttons can be - * created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing - * it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { + /** + * Use this method to define your button->command mappings. Buttons can be + * created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing + * it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { - // Driver Controller - m_drive.setDefaultCommand( - m_drive.runVoltageTeleopFieldRelative( - () -> new ChassisSpeeds( - -teleopAxisAdjustment(m_driver.getLeftY()) * - DriveConstants.maxLinearVelocity, - -teleopAxisAdjustment(m_driver.getLeftX()) * - DriveConstants.maxLinearVelocity, - -teleopAxisAdjustment(m_driver.getRightX()) - * DriveConstants.maxLinearVelocity))); + // Driver Controller + m_drive.setDefaultCommand( + m_drive.runVoltageTeleopFieldRelative( + () -> new ChassisSpeeds( + -teleopAxisAdjustment(m_driver.getLeftY()) * + DriveConstants.maxLinearVelocity, + -teleopAxisAdjustment(m_driver.getLeftX()) * + DriveConstants.maxLinearVelocity, + -teleopAxisAdjustment(m_driver.getRightX()) + * DriveConstants.maxLinearVelocity))); + m_intake.setDefaultCommand( + m_intake.setIntakeUp()); - m_intake.setDefaultCommand( - m_intake.setIntakeUp() - ); - - // left trigger -> climb up - m_driver.leftTrigger(0.1).onTrue( - m_climber.setDutyCycle(-1)).onFalse( - m_climber.setDutyCycle(0)); + // left trigger -> climb up + m_driver.leftTrigger(0.1).onTrue( + m_climber.setDutyCycle(-1)).onFalse( + m_climber.setDutyCycle(0)); - // right trigger -> climb up - m_driver.rightTrigger(0.1).onTrue( - m_climber.setDutyCycle(1)).onFalse( - m_climber.setDutyCycle(0)); + // right trigger -> climb up + m_driver.rightTrigger(0.1).onTrue( + m_climber.setDutyCycle(1)).onFalse( + m_climber.setDutyCycle(0)); - // Operator Controller + // Operator Controller - // D-Pad Up for intake down, rollers forward, until note in feeder beambreak - m_operator.povUp().whileTrue( - Commands.parallel( - m_intake.setIntakeDown(false), - m_feeder.setRPM(() -> 2500)).until(() -> m_feeder.feederBeambreakObstructed())) - .onFalse(m_intake.setIntakeUp()); + // D-Pad Up for intake down, rollers forward, until note in feeder beambreak + m_operator.povUp().whileTrue( + Commands.parallel( + m_intake.setIntakeDown(false), + m_feeder.setRPM(() -> 2500)) + .until(() -> m_feeder.feederBeambreakObstructed())) + .onFalse(m_intake.setIntakeUp()); - // D-Pad Down for intake down, rollers backward - m_operator.povDown().whileTrue( - Commands.parallel( - m_intake.setIntakeDown(true), - m_feeder.setRPM(() -> -3000))) - .onFalse(m_intake.setIntakeUp()); + // D-Pad Down for intake down, rollers backward + m_operator.povDown().whileTrue( + Commands.parallel( + m_intake.setIntakeDown(true), + m_feeder.setRPM(() -> -3000))) + .onFalse(m_intake.setIntakeUp()); - // Y for shooter at subwoofer - m_operator.y().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)), - m_shooter.setRPM(() -> 5800, 0.3)).until(() -> m_shooter.atSetpoint()) - .andThen(m_feeder.setRPM(() -> 3000) - .until(() -> (!m_feeder.feederBeambreakObstructed() - && !m_feeder.shooterBeambreakObstructed())))); + // Y for shooter at subwoofer + m_operator.y().whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(45)), + m_shooter.setRPM(() -> 5800, 0.3)).until(() -> m_shooter.atSetpoint()) + .andThen(m_feeder.setRPM(() -> 3000) + .until(() -> (!m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())))); - // X for shooter at amp - m_operator.leftBumper().whileTrue( - Commands.parallel( - m_feeder.setVoltage(() -> 0), - m_shooter.stopShooter(), - m_pivot.setPivotVoltage(() -> 0) - ) - ); + // X for shooter at amp + m_operator.leftBumper().whileTrue( + Commands.parallel( + m_feeder.setVoltage(() -> 0), + m_shooter.stopShooter(), + Commands.sequence(m_pivot.setPivotTarget(() -> ShooterConstants.down) + .until(() -> m_pivot.atSetpoint() + || m_pivot.isStalled()) + .andThen( + Commands.either(m_pivot.resetEncoder(), + m_pivot.runZero(), + () -> m_pivot.isStalled()))))); - // m_operator.rightBumper().whileTrue( - // m_pivot.runCurrentZeroing() - // ); + // m_operator.rightBumper().whileTrue( + // m_pivot.runCurrentZeroing() + // ); + // B for shooter at podium or feeding + m_operator.b().whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(20)), + m_shooter.setRPM(() -> 5800, 1.0)).until(() -> m_shooter.atSetpoint()) + .andThen(m_feeder.setRPM(() -> 2000) + .until(() -> (!m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())))); + // A for shooter at source + m_operator.a().whileTrue( + Commands.parallel( + m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)), + m_shooter.setRPM(() -> -1500, 1.0), + m_feeder.setRPM(() -> -1500), + m_intake.setRollerRPM(() -> -1000)) + .until(() -> (m_feeder.feederBeambreakObstructed() + && !m_feeder.shooterBeambreakObstructed())) + .andThen(Commands.parallel(m_shooter.stopShooter(), + m_pivot.setPivotTarget( + () -> Units.degreesToRadians(0.0))))); - // B for shooter at podium or feeding - m_operator.b().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(20)), - m_shooter.setRPM(() -> 5800, 1.0)).until(() -> m_shooter.atSetpoint()) - .andThen(m_feeder.setRPM(() -> 2000) - .until(() -> (!m_feeder.feederBeambreakObstructed() - && !m_feeder.shooterBeambreakObstructed()))) - .andThen(new WaitCommand(0.5)).andThen(Commands.parallel(m_shooter.stopShooter(), - m_pivot.setPivotTarget(() -> Units.degreesToRadians(0.0))))); + m_operator.leftTrigger(0.1).onTrue( + m_shooter.setRPM(() -> 5800, 0.3)) + .onFalse(m_shooter.stopShooter() + .andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)))); - // A for shooter at source - m_operator.a().whileTrue( - Commands.parallel( - m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)), - m_shooter.setRPM(() -> -1500, 1.0), - m_feeder.setRPM(() -> -1500), - m_intake.setRollerRPM(() -> -1000)) - .until(() -> (m_feeder.feederBeambreakObstructed() && !m_feeder.shooterBeambreakObstructed())) - .andThen(Commands.parallel(m_shooter.stopShooter(), - m_pivot.setPivotTarget(() -> Units.degreesToRadians(0.0))))); - - m_operator.leftTrigger(0.1).onTrue( - m_shooter.setRPM(() -> 5800, 0.3)) - .onFalse(m_shooter.stopShooter().andThen(m_pivot.setPivotTarget(() -> Units.degreesToRadians(0)))); - - } + } - public void robotPeriodic() { - m_visualizer.periodic(); - } + public void robotPeriodic() { + m_visualizer.periodic(); + } - private static double teleopAxisAdjustment(double x) { - return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); - } + private static double teleopAxisAdjustment(double x) { + return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02); + } - public Command getAutonomousCommand() { - return null; - } + public Command getAutonomousCommand() { + return null; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Visualizer.java b/src/main/java/frc/robot/Visualizer.java index ec76dd6..a7a68f1 100644 --- 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 index 33aa639..6b7033e 100644 --- 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()); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java index 3bda793..38f50f0 100644 --- a/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivot2/PivotIO.java @@ -14,7 +14,7 @@ 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; @@ -22,6 +22,7 @@ public static class PivotIOInputs { 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 index 03f9c7a..43b5806 100644 --- 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 index 4c12020..7d563d1 100644 --- 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