diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f82ea33c..a0964d1d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -213,10 +213,6 @@ public enum RobotEdition { @AutoLogOutput(key = "Superstructure/Autoaim Request") private Trigger autoAimReq; - // TODO - // @AutoLogOutput(key = "Superstructure/Autoaim Request") - // private Trigger climbAutoAlignInAutoReq; - // Auto stuff private final Autos autos; private Optional lastAlliance = Optional.empty(); @@ -329,10 +325,10 @@ public Robot() { new SpindexerSubsystem( canivore, (ROBOT_MODE == RobotMode.REAL) - ? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore) + ? new RollerIO(9, SpindexerSubsystem.getIndexerConfig(), canivore) : new RollerIOSim( 9, - SpindexerSubsystem.getIndexerConfigs(), + SpindexerSubsystem.getIndexerConfig(), new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX44Foc(1), @@ -342,10 +338,10 @@ public Robot() { MotorType.KrakenX44, canivore), (ROBOT_MODE == RobotMode.REAL) - ? new RollerIO(10, SpindexerSubsystem.getKickerConfigs(), canivore) + ? new RollerIO(10, SpindexerSubsystem.getKickerConfig(), canivore) : new RollerIOSim( 10, - SpindexerSubsystem.getKickerConfigs(), + SpindexerSubsystem.getKickerConfig(), new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX44Foc(1), @@ -410,7 +406,9 @@ public Robot() { 11, TurretSubsystem.HOOD_MIN_ANGLE, TurretSubsystem.HOOD_MAX_ANGLE), - ROBOT_MODE == RobotMode.REAL ? new TurretIO(canivore) : new TurretIOSim(canivore), + ROBOT_MODE == RobotMode.REAL + ? new TurretIO(canivore, TurretSubsystem.getTurretConfig()) + : new TurretIOSim(canivore), ROBOT_MODE == RobotMode.REAL ? new CANcoderIO(5, TurretSubsystem.getCancoder24tConfigs(), canivore) : new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore), @@ -422,26 +420,11 @@ public Robot() { climber = ROBOT_EDITION == RobotEdition.ALPHA ? new EmptyClimberSubsystem(canivore) - : new ClimberSubsystem( - new ClimberIO(canivore)); // TODO: SWITCH BACK TO REAL CLIMBER WHEN FIXED - // : new ClimberSubsystem(new ClimberIO(canivore)); + : new ClimberSubsystem(new ClimberIO(canivore)); // now that we've assigned the correct subsystems based on robot edition, we can pass them into // the superstructure superstructure = new Superstructure(swerve, indexer, intake, shooter, climber, driver, operator); - addCompSysids(climber, indexer, intake, shooter); - - autoAimReq = - driver - .leftBumper() - .or( - new Trigger( - () -> - Superstructure.getState() == SuperState.SPIN_UP_SCORE - || Superstructure.getState() == SuperState.SCORE) - .and(() -> isTeleopEnabled())); - - // climbAutoAlignInAutoReq = Autos.autoAlignClimbReq; DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); @@ -509,12 +492,7 @@ public Robot() { SmartDashboard.putData( "Zero Intake Off Cancoder", intake.zeroPivotOffCancoder().ignoringDisable(true)); SmartDashboard.putData("Zero Hood", shooter.zeroHood().ignoringDisable(true)); - SmartDashboard.putData( - "Test shot", - Commands.parallel( - shooter.torqueCurrentTest(swerve::getPose, swerve::getVelocityFieldRelative), - Commands.waitUntil(new Trigger(shooter::atFlywheelVelocitySetpoint).debounce(0.1)) - .andThen(indexer.testShoot()))); + SmartDashboard.putData( "Set Turret to 0", shooter.resetTurretToPosition(Rotation2d.kZero).ignoringDisable(true)); SmartDashboard.putData( @@ -529,7 +507,8 @@ public Robot() { // Set default commands driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); - shooter.setDefaultCommand(shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative)); + shooter.setDefaultCommand( + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::canScore)); swerve.setDefaultCommand( swerve .driveOpenLoopFieldRelative( @@ -543,18 +522,9 @@ public Robot() { * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) .times(-1)) .withName("default")); - // swerve.setDefaultCommand(swerve.stop()); indexer.setDefaultCommand(indexer.rest()); intake.setDefaultCommand(intake.restExtended()); climber.setDefaultCommand(climber.retract()); - // swerve.faceHubSOTM( - // () -> - // modifyJoystick(driver.getLeftX()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - // () -> - // modifyJoystick(driver.getLeftY()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed() - // * -1)); addControllerBindings(indexer, shooter, intake); @@ -593,8 +563,6 @@ public Robot() { .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); - // SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); - // SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers canInitialErrorTimer.restart(); @@ -628,8 +596,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta () -> swerve.setYaw( DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) - // ? Rotation2d.kCW_90deg - // : Rotation2d.kCCW_90deg))); ? Rotation2d.kZero : Rotation2d.k180deg))); @@ -637,13 +603,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta autoAimReq .and(() -> ROBOT_EDITION == RobotEdition.ALPHA) .whileTrue( - // swerve.faceHubSOTM( - // () -> - // modifyJoystick(driver.getLeftY()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - // () -> - // modifyJoystick(driver.getLeftX()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); swerve.faceHub( () -> -1 @@ -657,63 +616,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE)); - // climbAutoAlignInAutoReq.whileTrue( - // swerve.alignToClimb( - // () -> - // ClimbTargets.CLIMB_TARGETS_LIST.stream() - // .filter(target -> target.getLeftHanded() == leftClimbTarget) - // .filter( - // target -> - // target.isBlueAlliance() - // == (DriverStation.getAlliance().orElse(Alliance.Blue) - // == Alliance.Blue)) - // .findFirst() - // .get())); - - // climbAutoAlignInAutoReq.whileTrue( - // swerve.alignToClimb( - // () -> - // ClimbTargets.CLIMB_TARGETS_LIST.stream() - // .filter(target -> target.getLeftHanded() == leftClimbTarget) - // .filter( - // target -> - // target.isBlueAlliance() - // == (DriverStation.getAlliance().orElse(Alliance.Blue) - // == Alliance.Blue)) - // .findFirst() - // .get())); - - // new Trigger(swerve::isCloseToBump) - // .whileTrue( - // swerve.bumpAlign( - // () -> - // DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) - // ? -1 - // : 1 - // * modifyJoystick(driver.getLeftY()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - // () -> - // DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue) - // ? -1 - // : 1 - // * modifyJoystick(driver.getLeftX()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); - new Trigger(swerve::isNearTrench) .and(() -> Superstructure.getState() != SuperState.INTAKE) .and(() -> isTeleopEnabled()) + .and(() -> !Superstructure.getPoseOverride()) .whileTrue( swerve - // .driveClosedLoopFieldRelative( - // () -> - // new ChassisSpeeds( - // modifyJoystick(driver.getLeftY()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - // modifyJoystick(driver.getLeftX()) - // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - // AutoAlign.calculateRotationVelocity( - // swerve.getRotation(), Rotation2d.fromDegrees(30))) - // .times(-1)) .trenchAlign( () -> modifyJoystick(driver.getLeftY()) @@ -722,7 +630,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()) .alongWith(Commands.print("afkljsdflkjs"))); - // 0)); // current zero shooter hood driver @@ -736,6 +643,15 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta new Trigger(() -> AutoAim.targetInTurretDeadzone()) .onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25)); + // ---zeroing stuff--- + driver.povUp().whileTrue(shooter.currentZeroTurretAgainstForwardHardstop()); + + driver + .leftBumper() + .onTrue( + Commands.parallel( + shooter.resetTurretToPosition(shooter.getCalculatedTurretRotations()), + intake.zeroPivotOffCancoder())); operator .leftBumper() @@ -745,23 +661,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .rightBumper() .or(Autos.autoLeftClimbReq.negate()) .onTrue(Commands.runOnce(() -> leftClimbTarget = false)); + // I HATE THIS! + operator.leftStick().whileTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret())); - // TODO: ACTUAL BINDING LOL - // test shot driver .rightBumper() .whileTrue( - // Commands.parallel( - // // shooter.torqueCurrentTest(), - // shooter.testShoot(swerve::getPose, swerve::getVelocityFieldRelative), - // Commands.waitUntil( - // new Trigger(shooter::atFlywheelVelocitySetpoint).debounce(0.05) - // // .and(shooter::atTurretSetpoint) - // // .debounce(0.25) - // ) - // .andThen(indexer.kick()))); - // SmartDashboard.putData( - // "climb align", swerve.alignToClimb( () -> ClimbTargets.CLIMB_TARGETS_LIST.stream() @@ -774,10 +679,12 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .findFirst() .get())); // turn swerve if target is in turret deadzone - driver - .leftBumper() - .and(AutoAim::targetInTurretDeadzone) + // driver + // .leftBumper() + // .and( + new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAScoreState()) + .and(() -> !Superstructure.getPoseOverride()) .whileTrue( swerve.faceHubComp( () -> @@ -789,7 +696,6 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta * modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), shooter::getTurretPosition)); - // ---zeroing stuff--- // create triggers for joystick disconnect alerts new Trigger(() -> DriverStation.isJoystickConnected(0)) @@ -821,36 +727,16 @@ private void addAutos() { System.out.println("Done generating autos"); } - // Sysid Autos - private void addCompSysids( - ClimberSubsystem climber, Indexer indexer, Intake intake, Shooter shooter) { - autoChooser.addOption("Climber Sysid", climber.runClimberSysid()); - autoChooser.addOption("Indexer Roller Sysid", indexer.runRollerSysId()); - autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); - autoChooser.addOption("Intake Pivot Sysid", intake.runPivotSysid()); - autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); - - autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); - autoChooser.addOption("Turret Sysid", shooter.runTurretSysid()); - autoChooser.addOption("Kicker Sysid", indexer.runKickerSysId()); - autoChooser.addOption("Turn Sysid", swerve.runTurnSysid()); - } - @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); superstructure.periodic(); - // TODO: YAW VALUE FROM HARDWARE Pose3d turretPose = new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), - // new Rotation3d(0, 0, Units.degreesToRadians(turretAngle.getAsDouble()))); new Rotation3d(0, 0, shooter.getTurretPosition().getRadians())); - // 0)); - // )); - // TODO: USE MEASURED EXTENSIONS AND ANGLES Logger.recordOutput( "Robot/Mechanism Poses", new Pose3d[] { @@ -877,12 +763,11 @@ public void robotPeriodic() { new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) }); - // TODO: ACTUAL SETPOINT Pose3d turretSetpoint = new Pose3d( new Translation3d(-0.177413, -0.111702, 0.350341), new Rotation3d(0, 0, shooter.getTurretSetpoint().getRadians())); - // TODO: ACTUAL SETPOINTS + Logger.recordOutput( "Robot/Mechanism Setpoints", new Pose3d[] { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index fff70594..7c60780f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -148,7 +148,7 @@ public enum FixedShotTarget { private Trigger readyTrigger; @AutoLogOutput(key = "Superstructure/Operator Pose Override?") - private boolean override; + private static boolean poseOverride = false; @AutoLogOutput(key = "Superstructure/Fixed Shot") private static FixedShotTarget fixedShotTarget = FixedShotTarget.NONE; @@ -197,13 +197,40 @@ private void addTriggers() { operator.leftBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.LEFT)); operator.rightBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.RIGHT)); - operator.leftTrigger().onTrue(Commands.runOnce(() -> override = true)); - operator.rightTrigger().onTrue(Commands.runOnce(() -> override = false)); + operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true)); + operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false)); - operator.povLeft().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.LEFT)); - operator.povUp().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.MID)); - operator.povRight().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.RIGHT)); - operator.povDown().onTrue(Commands.runOnce(() -> fixedShotTarget = FixedShotTarget.NONE)); + operator + .povLeft() + .onTrue( + Commands.runOnce( + () -> { + fixedShotTarget = FixedShotTarget.LEFT; + poseOverride = true; + })); + operator + .povUp() + .onTrue( + Commands.runOnce( + () -> { + fixedShotTarget = FixedShotTarget.MID; + poseOverride = true; + })); + operator + .povRight() + .onTrue( + Commands.runOnce( + () -> { + fixedShotTarget = FixedShotTarget.RIGHT; + poseOverride = true; + })); + operator + .povDown() + .onTrue( + Commands.runOnce( + () -> { + fixedShotTarget = FixedShotTarget.NONE; + })); shootReq = driver @@ -226,18 +253,7 @@ private void addTriggers() { new Trigger(shooter::atFlywheelVelocitySetpoint) // .debounce(0.05) .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05)) - .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)) - // .and( - // new Trigger( - // () -> { - // if (Robot.ROBOT_EDITION == RobotEdition.ALPHA) { - // return swerve.isFacingTarget(); - // } else { - // return shooter.isFacingTarget(); - // } - // }) - // .debounce(0.07)); - ; + .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)); } private void addTransitions() { @@ -368,14 +384,14 @@ private void addCommands() { intake.restExtended(), // intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), climber.retract()); bindCommands( SuperState.INTAKE, intake.intake(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), climber.retract()); bindCommands( @@ -460,7 +476,7 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.restExtended(), indexer.rest(), - shooter.spinUp( + shooter.score( swerve::getPose, () -> AutoAim.getCompensatedSOTMShotData( @@ -498,8 +514,6 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), - // shooter.testShoot(), - // shooter.torqueCurrentTest(swerve::getPose, swerve::getVelocityFieldRelative), climber.retract()); bindCommands( @@ -517,7 +531,6 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE), swerve::getVelocityFieldRelative), - // shooter.testShoot(swerve::getPose, swerve::getVelocityFieldRelative), climber.retract()); bindCommands( @@ -533,7 +546,6 @@ private void addCommands() { ? AutoAim.ALPHA_HUB_SHOT_TREE : AutoAim.COMP_HUB_SHOT_TREE) .flywheelVelocityRotPerSec()), - // shooter.testShoot(swerve::getPose, swerve::getVelocityFieldRelative), shooter.score( swerve::getPose, () -> @@ -553,19 +565,19 @@ private void addCommands() { SuperState.PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), climber.extend()); bindCommands( SuperState.CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), climber.retract()); bindCommands( SuperState.POST_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative), + shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), climber.extend()); bindCommands( @@ -762,8 +774,8 @@ public boolean inScoringArea() { public boolean canScore() { return (isOurShift() || !DriverStation.isFMSAttached()) - && (inScoringArea() || override) - && !swerve.isNearTrench(); + && (inScoringArea() || poseOverride) + && (!swerve.isNearTrench() || poseOverride || fixedShotTarget != FixedShotTarget.NONE); } public boolean canShoot() { @@ -781,4 +793,8 @@ public static FeedTarget getFeedTarget() { public static FixedShotTarget getFixedShotTarget() { return fixedShotTarget; } + + public static boolean getPoseOverride() { + return poseOverride; + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 940ab039..15182c76 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.climber; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.util.Units; @@ -11,10 +9,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -31,17 +25,6 @@ public class ClimberSubsystem extends SubsystemBase { ClimberIO io; ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); - // turned off climber - - private SysIdRoutine climberSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Climber/SysID State", state.toString())), - new Mechanism((voltage) -> io.setClimberVoltage(voltage.in(Volts)), null, this)); - @AutoLogOutput(key = "Climber/Current Filter Value") private double currentFilterValue = 0.0; @@ -84,25 +67,6 @@ public Command zeroClimber() { return this.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true); } - public Command runClimberSysid() { - return Commands.sequence( - climberSysid - .quasistatic(Direction.kForward) - .until( - () -> - inputs.positionMeters - > (MAX_EXTENSION_METERS - Units.inchesToMeters(1))), // Stop before endstop - climberSysid - .quasistatic(Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(1)), - climberSysid - .dynamic(Direction.kForward) - .until(() -> inputs.positionMeters > (MAX_EXTENSION_METERS - Units.inchesToMeters(1))), - climberSysid - .dynamic(Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(1))); - } - public Command runCurrentZeroing() { return this.run(() -> io.setClimberVoltage(-0.5)) .until(new Trigger(() -> Math.abs(currentFilterValue) > CURRENT_ZERO_THRESHOLD)) diff --git a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java index 6a226f24..6e354442 100644 --- a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java @@ -27,11 +27,6 @@ public Command zeroClimber() { return this.idle(); } - @Override - public Command runClimberSysid() { - return this.idle(); - } - @Override public Command runCurrentZeroing() { return this.idle(); diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 28dcbe39..c28631a7 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -5,7 +5,6 @@ package frc.robot.subsystems.indexer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import java.util.function.DoubleSupplier; @@ -23,14 +22,4 @@ public interface Indexer extends Subsystem { /** Not running (set to 0) */ public Command rest(); - - public Command runRollerSysId(); - - public default Command runKickerSysId() { - return Commands.none(); - } // alpha also has a kicker but ig we didn't use sysid for that and i'm too lazy to add it just - - // for this - - public Command testShoot(); } diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 9363aa16..28fa3b7b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -157,10 +157,4 @@ public Command runRollerSysId() { indexRollerSysid.dynamic(Direction.kForward), indexRollerSysid.dynamic(Direction.kReverse)); } - - @Override - public Command testShoot() { - // whatever bro - return Commands.none(); - } } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 2998a390..ffeeed31 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.indexer; -import static edu.wpi.first.units.Units.Volts; - import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; @@ -11,14 +9,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; import frc.robot.subsystems.shooter.TurretSubsystem; -import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -40,30 +33,9 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { RollerIO kickerIO; RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged(); - private SysIdRoutine indexRollerSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())), - new Mechanism((volts) -> spinnerIO.setRollerVoltage(volts.in(Volts)), null, this)); - - private SysIdRoutine kickerSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Indexer/Kicker/SysID State", state.toString())), - new Mechanism((volts) -> kickerIO.setRollerVoltage(volts.in(Volts)), null, this)); - public static final double MAX_ACCELERATION = 10.0; public static final double MAX_VELOCITY = 10.0; - private LoggedTunableNumber testKickVolts = new LoggedTunableNumber("Indexer/Kicker Voltage", 10); - private LoggedTunableNumber testSpinVolts = new LoggedTunableNumber("Indexer/Spinner Voltage", 8); - private final Alert spinnerDisconnectedAlert = new Alert("Disconnected spinner motor!", AlertType.kError); private final Alert kickerDisconnectedAlert = @@ -131,7 +103,17 @@ public Command rest() { }); } - public static TalonFXConfiguration getIndexerConfigs() { + @Override + public void periodic() { + spinnerIO.updateInputs(spinnerInputs); + Logger.processInputs("Indexer/Spinner", spinnerInputs); + kickerIO.updateInputs(kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); + spinnerDisconnectedAlert.set(!spinnerInputs.connected); + kickerDisconnectedAlert.set(!kickerInputs.connected); + } + + public static TalonFXConfiguration getIndexerConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -156,7 +138,7 @@ public static TalonFXConfiguration getIndexerConfigs() { return config; } - public static TalonFXConfiguration getKickerConfigs() { + public static TalonFXConfiguration getKickerConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -181,40 +163,4 @@ public static TalonFXConfiguration getKickerConfigs() { return config; } - - @Override - public void periodic() { - spinnerIO.updateInputs(spinnerInputs); - Logger.processInputs("Indexer/Spinner", spinnerInputs); - kickerIO.updateInputs(kickerInputs); - Logger.processInputs("Indexer/Kicker", kickerInputs); - spinnerDisconnectedAlert.set(!spinnerInputs.connected); - kickerDisconnectedAlert.set(!kickerInputs.connected); - } - - public Command runRollerSysId() { - return Commands.sequence( - indexRollerSysid.quasistatic(Direction.kForward), - indexRollerSysid.quasistatic(Direction.kReverse), - indexRollerSysid.dynamic(Direction.kForward), - indexRollerSysid.dynamic(Direction.kReverse)); - } - - @Override - public Command runKickerSysId() { - return Commands.sequence( - kickerSysid.quasistatic(Direction.kForward), - kickerSysid.quasistatic(Direction.kReverse), - kickerSysid.dynamic(Direction.kForward), - kickerSysid.dynamic(Direction.kReverse)); - } - - @Override - public Command testShoot() { - return this.run( - () -> { - kickerIO.setRollerVoltage(testKickVolts.get()); - spinnerIO.setRollerVoltage(testSpinVolts.get()); - }); - } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 622a9247..1c626b6c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -29,12 +29,6 @@ public interface Intake extends Subsystem { public Rotation2d getPositionSetpoint(); - public Command runRollerSysid(); - - public default Command runPivotSysid() { - return Commands.none(); - } - public Command zeroPivotOffCancoder(); public Command runCurrentZeroing(); diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 0174bac5..05d72a22 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Volt; - import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -16,10 +14,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.components.cancoder.CANcoderIOSim; @@ -56,32 +50,10 @@ public class SlapdownSubsystem extends SubsystemBase implements Intake { @AutoLogOutput(key = "Intake/Pivot/Current Filter Value") private double currentFilterValue = 0.0; - private final SysIdRoutine rollerSysid; - - private final SysIdRoutine pivotSysid; - public SlapdownSubsystem(PivotIO pivotIO, CANcoderIO cancoderIO, RollerIO rollerIO) { this.pivotIO = pivotIO; this.cancoderIO = cancoderIO; this.rollerIO = rollerIO; - - rollerSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Intake/Roller/Sysid State", state.toString())), - new Mechanism((volts) -> rollerIO.setRollerVoltage(volts.in(Volt)), null, this)); - - pivotSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Intake/Pivot/Sysid State", state.toString())), - new Mechanism((volts) -> pivotIO.setMotorVoltage(volts.in(Volt)), null, this)); } @Override @@ -181,39 +153,6 @@ public Command runCurrentZeroing() { Commands.print("Intake pivot zeroed")); } - @Override - public Command runRollerSysid() { - return Commands.sequence( - rollerSysid.quasistatic(Direction.kForward), - rollerSysid.quasistatic(Direction.kReverse), - rollerSysid.dynamic(Direction.kForward), - rollerSysid.dynamic(Direction.kReverse)); - } - - @Override - public Command runPivotSysid() { - return Commands.sequence( - pivotSysid - .quasistatic(Direction.kForward) - .until( - () -> - pivotIOInputs.position.getDegrees() - > (PIVOT_MAX_POSITION.getDegrees() - 5) // Stop 5 degrees before hardstop - ), - pivotSysid - .quasistatic(Direction.kReverse) - .until( - () -> pivotIOInputs.position.getDegrees() < (PIVOT_MIN_POSITION.getDegrees() + 5)), - pivotSysid - .dynamic(Direction.kForward) - .until( - () -> pivotIOInputs.position.getDegrees() > (PIVOT_MAX_POSITION.getDegrees() - 5)), - pivotSysid - .dynamic(Direction.kReverse) - .until( - () -> pivotIOInputs.position.getDegrees() < (PIVOT_MIN_POSITION.getDegrees() + 5))); - } - @Override public Command zeroPivotOffCancoder() { return this.runOnce(() -> pivotIO.resetEncoder(cancoderIOInputs.cancoderPositionRotations)); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1d8e7ff6..d57152f7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; /** Add your docs here. */ @@ -37,7 +38,9 @@ public Command feed( /** Not running (set to 0) */ public Command rest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier); + Supplier robotPoseSupplier, + Supplier chassisSpeedsSupplier, + BooleanSupplier canScore); /** Run balls out from the shooter. This is for antijamming the robot */ public Command spit(); @@ -51,15 +54,6 @@ public Command rest( /** Reset hood encoder to its minimum position */ public Command zeroHood(); - /** Shoots based on dashboard numbers. For testing only */ - public Command testShoot( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier); - - public default Command torqueCurrentTest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return Commands.none(); - } - /** * Runs the hood backwards until it hits its hard stop and the current spikes, then resets encoder * position. @@ -72,14 +66,6 @@ public default Command torqueCurrentTest( public boolean atTurretSetpoint(); - public Command runFlywheelSysid(); - - public Command runHoodSysid(); - - public default Command runTurretSysid() { - return Commands.none(); - } - public default Command resetTurretToPosition(Rotation2d rot) { return Commands.none(); } @@ -96,25 +82,17 @@ public default Rotation2d getTurretSetpoint() { return Rotation2d.kZero; } - public default Command spinUpTest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return Commands.none(); - } - - public default Command spinUp( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier) { - return Commands.none(); - } - public default void turretInit() {} public default Pose2d getTurretPose(Pose2d robotPose) { return Pose2d.kZero; } - public default Command currentZeroTurret() { + public default Command currentZeroTurretAgainstForwardHardstop() { + return Commands.none(); + } + + public default Command stopTurret() { return Commands.none(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index f403d053..5f208e48 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -21,11 +21,11 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -83,16 +83,6 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; } - @Override - public Command testShoot( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - }); - } - public Command score( Supplier robotPoseSupplier, Supplier shotDataSupplier, @@ -127,7 +117,9 @@ public Command feed( @Override public Command rest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { + Supplier robotPoseSupplier, + Supplier chassisSpeedsSupplier, + BooleanSupplier canScore) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); @@ -201,44 +193,6 @@ public boolean atTurretSetpoint() { return false; } - @Override - public Command runHoodSysid() { - return Commands.sequence( - hoodSysid - .quasistatic(Direction.kForward) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ROTATION.getDegrees() - 5)), // Stop before endstop - hoodSysid - .quasistatic(Direction.kReverse) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - < (HOOD_MIN_ROTATION.getDegrees() + 5)), - hoodSysid - .dynamic(Direction.kForward) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ROTATION.getDegrees() - 5)), - hoodSysid - .dynamic(Direction.kReverse) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - < (HOOD_MIN_ROTATION.getDegrees() + 5))); - } - - @Override - public Command runFlywheelSysid() { - return Commands.sequence( - flywheelSysid.quasistatic(Direction.kForward), - flywheelSysid.quasistatic(Direction.kReverse), - flywheelSysid.dynamic(Direction.kForward), - flywheelSysid.dynamic(Direction.kReverse)); - } - public static TalonFXConfiguration getFlywheelConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java index 8359ea56..f3d46c09 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIO.java @@ -6,8 +6,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -18,10 +16,6 @@ import org.littletonrobotics.junction.AutoLogOutput; public class TurretIO { - public static double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); - - public static double CANCODER_24T_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); - public static double CANCODER_26T_TO_TURRET_GEAR_RATIO = (26.0 / 32.0) * (10.0 / 85.0); protected final TalonFX motor; @@ -48,25 +42,8 @@ public static class TurretIOInputs { // todo private Rotation2d turretSetpoint = Rotation2d.kZero; - public TurretIO(CANBus canivore) { + public TurretIO(CANBus canivore, TalonFXConfiguration config) { motor = new TalonFX(15, canivore); - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = TURRET_GEAR_RATIO; - config.CurrentLimits.StatorCurrentLimit = 40.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 40.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - - config.Slot0.kS = 0.45; - config.Slot0.kV = 5.7; - config.Slot0.kP = 240.0; - - config.MotionMagic.MotionMagicAcceleration = 20; // 2.064; - config.MotionMagic.MotionMagicCruiseVelocity = 50; // 8.0; - motor.getConfigurator().apply(config); angularVelocityRotationsPerSec = motor.getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java index 1d94a78e..e57098ab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretIOSim.java @@ -22,11 +22,11 @@ public class TurretIOSim extends TurretIO { private double lastSimTime = 0.0; public TurretIOSim(CANBus canivore) { - super(canivore); + super(canivore, TurretSubsystem.getTurretConfig()); physicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, TurretIO.TURRET_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1), 0.01, TurretSubsystem.TURRET_GEAR_RATIO), DCMotor.getKrakenX44Foc(1)); motorSim = motor.getSimState(); @@ -48,10 +48,10 @@ public TurretIOSim(CANBus canivore) { motorSim.setRawRotorPosition( Units.radiansToRotations(physicsSim.getAngularPositionRad()) - * (TurretIO.TURRET_GEAR_RATIO)); + * (TurretSubsystem.TURRET_GEAR_RATIO)); motorSim.setRotorVelocity( Units.radiansToRotations(physicsSim.getAngularVelocityRadPerSec()) - * TurretIO.TURRET_GEAR_RATIO); + * TurretSubsystem.TURRET_GEAR_RATIO); }); simNotifier.startPeriodic(simLoopPeriod); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 6fb1df19..e70fd515 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Volts; - import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -25,17 +23,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; -import frc.robot.utils.LoggedTunableNumber; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -59,10 +54,15 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; public static final Rotation2d TURRET_MID_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final double TURRET_GEAR_RATIO = (42.0 / 12.0) * (32.0 / 16.0) * (85.0 / 10.0); + + public static final double CANCODER_24T_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); + public static final double CANCODER_26T_TO_TURRET_GEAR_RATIO = (26.0 / 32.0) * (10.0 / 85.0); + // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing - @AutoLogOutput(key = "Shooter/Turret/Rear Hardstop") - public static final Rotation2d TURRET_REAR_HARDSTOP_ANGLE = + @AutoLogOutput(key = "Shooter/Turret/Left Hardstop") + public static final Rotation2d TURRET_LEFT_HARDSTOP_ANGLE = // Changed to avoid cooking cable chain/wires // Plus 0 because then the rotation2d automatically wraps the value between -0.5 and 0.5 // (worked in sim) @@ -78,8 +78,9 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final Translation2d ROBOT_TO_TURRET_TRANSLATION = new Translation2d(-0.177413, -0.111702); // , 0.350341); public static final double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; - double hoodCurrentFilterValue = 0.0; - double turretCurrentFilterValue = 0.0; + + private double hoodCurrentFilterValue = 0.0; + private double turretCurrentFilterValue = 0.0; private CANcoderIO cancoder24t; private CANcoderIO cancoder26t; @@ -88,6 +89,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private HoodIO hoodIO; private HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); + private FlywheelIO flywheelIO; private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); @@ -96,42 +98,10 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { private LinearFilter currentFilter = LinearFilter.movingAverage(10); - private SysIdRoutine hoodSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), - new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); - - private SysIdRoutine flywheelSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), - new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); - - private SysIdRoutine turretSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Shooter/Turret/SysID State", state.toString())), - new Mechanism((voltage) -> turretIO.setVoltage(voltage.in(Volts)), null, this)); - - private LoggedTunableNumber testDegrees = - new LoggedTunableNumber("Shooter/Test Hood Degrees", 30.0); - private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); - private static final Alert cancoder24tDisconnectedAlert = new Alert("24T Cancoder disconnected!", AlertType.kError); private static final Alert cancoder26tDisconnectedAlert = new Alert("26T Cancoder disconnected!", AlertType.kError); - private final Alert flywheelLeaderDisconnectedAlert = new Alert("Disconnected flywheel leader!", AlertType.kError); private final Alert flywheelFollowerDisconnectedAlert = @@ -197,71 +167,19 @@ public void periodic() { flywheelFollowerDisconnectedAlert.set(!flywheelInputs.flywheelFollowerConnected); hoodDisconnectedAlert.set(!hoodInputs.connected); turretMotorDisconnectedAlert.set(!turretInputs.connected); - boolean pastHardstop = - ((getTurretPosition().getDegrees() > 0.002 * 360) - // TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees()) - && (getTurretPosition().getDegrees() - < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees()) - || (getCalculatedTurretRotations().getDegrees() > 0.002 * 360) - // TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees()) - && (getCalculatedTurretRotations().getDegrees() - < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); - if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert - } - - public static CANcoderConfiguration getCancoder24tConfigs() { - CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - // this is to offset the position where both cancoders are equal to be inside the deadzone - // Offset measured at rear hardstop (approx -259 degrees) - config.MagnetSensor.MagnetOffset = -0.765137 - TurretIO.CANCODER_24T_TO_TURRET_GEAR_RATIO / 0.1; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; - - return config; - } - - public static CANcoderConfiguration getCancoder26tConfigs() { - CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - // this is to offset the position where both cancoders are equal to be inside the deadzone - // Offset measured at rear hardstop (approx -259 degrees) - config.MagnetSensor.MagnetOffset = -0.4897 - TurretIO.CANCODER_26T_TO_TURRET_GEAR_RATIO / 0.1; - config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; - - return config; + // boolean pastHardstop = + // ((getTurretPosition().getDegrees() > 0.002 * 360) + // // TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees()) + // && (getTurretPosition().getDegrees() + // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees()) + // || (getCalculatedTurretRotations().getDegrees() > 0.002 * 360) + // // TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees()) + // && (getCalculatedTurretRotations().getDegrees() + // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); + // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert } @Override - // public Command feed( - // Supplier robotPoseSupplier, - // Supplier feedTarget, - // Supplier chassisSpeedsSupplier) { - // return this.run( - // () -> { - // Logger.recordOutput("Robot/Feed Target", feedTarget.get()); - // ShotData shotData = - // AutoAim.FEED_SHOT_TREE.get( - // robotPoseSupplier - // .get() - // .getTranslation() - // .getDistance(feedTarget.get().getTranslation())); - // hoodIO.setHoodPosition(shotData.hoodAngle()); - // // - // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - // flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); - - // // hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - // // flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - // turretIO.setTurretPosition( - // AutoAim.getTurretFeedTargetRotation( - // feedTarget.get().getTranslation(), - // robotPoseSupplier.get(), - // chassisSpeedsSupplier.get())); - // }); - // } - public Command feed( Supplier robotPoseSupplier, Supplier shotDataSupplier, @@ -283,18 +201,27 @@ public Command feed( } @Override - // TODO make this work with feeding also public Command rest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { + Supplier robotPoseSupplier, + Supplier chassisSpeedsSupplier, + BooleanSupplier canScore) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ANGLE); flywheelIO.setFlywheelVoltage(0.0); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + if (canScore.getAsBoolean()) { + turretIO.setTurretPosition( + AutoAim.getTurretFeedTargetRotation( + FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } else { + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } }); } @@ -303,12 +230,97 @@ public Command spit() { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ANGLE); - flywheelIO.setMotionProfiledFlywheelVelocity(20); + flywheelIO.setMotionProfiledFlywheelVelocity(30); // i think we want it to eject as far out from the robot as possible turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } + @Override + public Command zeroHood() { + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ANGLE)); + } + + public Command runHoodCurrentZeroing() { + return this.run(() -> hoodIO.setHoodVoltage(-3.0)) + .until( + new Trigger(() -> Math.abs(hoodCurrentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) + .debounce(0.25)) + .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); + } + + public Command score( + Supplier robotPoseSupplier, + Supplier shotDataSupplier, + Supplier chassisSpeedsSupplier) { + return this.run( + () -> { + switch (Superstructure.getFixedShotTarget()) { + // in front of left trench with intake facing trench + case LEFT: + hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); + // in front of tower with intake facing left (to avoid deadzone) + case MID: + hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); + // in front of right trench with intake facing alliance wall + case RIGHT: + hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); + case NONE: + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotDataSupplier.get().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } + }); + } + + @Override + public Command resetTurretToPosition(Rotation2d rot) { + return this.runOnce(() -> turretIO.resetTurretEncoder(getCalculatedTurretRotations())); + } + + /** sets the motor encoder to the position calculated from the encoders */ + public Command resetTurretToCalculatedPosition() { + return resetTurretToPosition(getCalculatedTurretRotations()); + } + + @Override + public Command currentZeroTurretAgainstForwardHardstop() { + return this.run(() -> turretIO.setVoltage(1.0)) + .until( + new Trigger(() -> Math.abs(turretCurrentFilterValue) > TURRET_CURRENT_ZERO_THRESHOLD) + .debounce(0.25)) + .andThen(Commands.parallel(Commands.print("Turret Zeroed"), zeroTurretForwardHardstop())); + } + + public Command zeroTurretForwardHardstop() { + return this.runOnce(() -> turretIO.resetTurretEncoder(TURRET_FORWARD_HARDSTOP_ANGLE)); + } + + // for defense and stuff + @Override + public Command stopTurret() { + return this.run( + () -> { + turretIO.setVoltage(0); + flywheelIO.setFlywheelVoltage(0); + hoodIO.setHoodVoltage(0); + }); + } + @Override @AutoLogOutput(key = "Shooter/Turret/Turret Calculated Rotation") public Rotation2d getCalculatedTurretRotations() { @@ -337,7 +349,8 @@ public Rotation2d getCalculatedTurretRotations() { // turret maxes out at less then 1 rotation which is like 11 can1 rotations anyways and it // should work up to there // multiply abs can1 rots by the gear ratio - double turretRotations = absoluteRotationsCan1 * TurretIO.CANCODER_24T_TO_TURRET_GEAR_RATIO; + double turretRotations = + absoluteRotationsCan1 * TurretSubsystem.CANCODER_24T_TO_TURRET_GEAR_RATIO; turretRotations = MathUtil.inputModulus(turretRotations + 0.1, 0, 1); // offset such that the shooter facing forward is 0 @@ -397,12 +410,7 @@ public Rotation2d getTurretCancoder26tPosition() { return cancoder26tInputs.cancoderPositionRotations; } - @Override - public Command zeroHood() { - return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ANGLE)); - } - - public Command runHoodCurrentZeroing() { + public Command runCurrentZeroing() { return this.run(() -> hoodIO.setHoodVoltage(-3.0)) .until( new Trigger(() -> Math.abs(hoodCurrentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) @@ -411,197 +419,13 @@ public Command runHoodCurrentZeroing() { } @Override - public Command testShoot( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); - // turretIO.setTurretPosition( - // AutoAim.getTurretHubTargetRotation( - // FieldUtils.getCurrentHubTranslation(), - // robotPoseSupplier.get(), - // chassisSpeedsSupplier.get())); - }); - } - - @Override - public Command torqueCurrentTest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - flywheelIO.setTorqueCurrentVel(testVelocity.get()); - // turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - }); - } - - @Override - public Command spinUpTest( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - // flywheelIO.spinUpVoltage(6, testVelocity.get()); - flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - // turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - }); - } - - public Command score( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - switch (Superstructure.getFixedShotTarget()) { - // in front of left trench with intake facing trench - case LEFT: - hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); - // in front of tower with intake facing left (to avoid deadzone) - case MID: - hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); - // in front of right trench with intake facing alliance wall - case RIGHT: - hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); - case NONE: - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - } - }); - } - - @Override - public Command runHoodSysid() { - return Commands.sequence( - hoodSysid - .quasistatic(Direction.kForward) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ANGLE.getDegrees() - 5)), // Stop before endstop - hoodSysid - .quasistatic(Direction.kReverse) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - < (HOOD_MIN_ANGLE.getDegrees() + 5)), - hoodSysid - .dynamic(Direction.kForward) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - > (HOOD_MAX_ANGLE.getDegrees() - 5)), - hoodSysid - .dynamic(Direction.kReverse) - .until( - () -> - hoodInputs.hoodPositionRotations.getDegrees() - < (HOOD_MIN_ANGLE.getDegrees() + 5))); - } - - @Override - public Command runFlywheelSysid() { - return Commands.sequence( - flywheelSysid.quasistatic(Direction.kForward), - flywheelSysid.quasistatic(Direction.kReverse), - flywheelSysid.dynamic(Direction.kForward), - flywheelSysid.dynamic(Direction.kReverse)); - } - - @Override - public Command runTurretSysid() { - return Commands.sequence( - turretSysid - .quasistatic(Direction.kForward) - .until( - () -> - turretInputs.positionRotations.getDegrees() - > (TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 5)), // Stop before endstop - turretSysid - .quasistatic(Direction.kReverse) - .until( - () -> - turretInputs.positionRotations.getDegrees() - < (TURRET_REAR_HARDSTOP_ANGLE.getDegrees() + 5)), - turretSysid - .dynamic(Direction.kForward) - .until( - () -> - turretInputs.positionRotations.getDegrees() - > (TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 5)), - turretSysid - .dynamic(Direction.kReverse) - .until( - () -> - turretInputs.positionRotations.getDegrees() - < (TURRET_REAR_HARDSTOP_ANGLE.getDegrees() + 5))); - } - - // public boolean isFacingTarget() { - // switch (Superstructure.getShotTarget()) { // ugh maybe this should be in robot.java - // case SCORE: - // return isFacingHub(); - // case FEED: - // return isFacingFeedTarget(); - // default: - // return false; - // } - // } - - // public boolean isFacingHub() { - // Rotation2d target = AutoAim.getVirtualHubYaw(getVelocityFieldRelative(), getPose()); - // return MathUtil.isNear( - // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - // } - - // public boolean isFacingFeedTarget() { - // Translation2d feedTarget = - // FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose().getTranslation(); - // Rotation2d target = AutoAim.getTargetRotation(feedTarget, getPose()); - // return MathUtil.isNear( - // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - // } - - @Override - public Command resetTurretToPosition(Rotation2d rot) { - return this.runOnce(() -> turretIO.resetTurretEncoder(getCalculatedTurretRotations())); - } - - /** sets the motor encoder to the position calculated from the encoders */ - public Command resetTurretToCalculatedPosition() { - return resetTurretToPosition(getCalculatedTurretRotations()); + public Rotation2d getHoodSetpoint() { + return hoodIO.getHoodSetpoint(); } @Override - public Rotation2d getHoodSetpoint() { - return hoodIO.getHoodSetpoint(); + public Pose2d getTurretPose(Pose2d robotPose) { + return robotPose.transformBy(new Transform2d(ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); } public static TalonFXConfiguration getFlywheelConfig() { @@ -659,61 +483,50 @@ public static TalonFXConfiguration getHoodConfig() { return config; } - @Override - public Command spinUp( - Supplier robotPoseSupplier, - Supplier shotDataSupplier, - Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - switch (Superstructure.getFixedShotTarget()) { - // in front of left trench with intake facing trench - case LEFT: - hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(TURRET_LEFT_FIXED_SHOT_ANGLE); // TODO - // in front of tower with intake facing left (to avoid deadzone) - case MID: - hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(TURRET_MID_FIXED_SHOT_ANGLE); // TODO - // in front of right trench with intake facing alliance wall - case RIGHT: - hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(TURRET_RIGHT_FIXED_SHOT_ANGLE); // TODO - case NONE: - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - } - }); - } + public static TalonFXConfiguration getTurretConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); - @Override - public Pose2d getTurretPose(Pose2d robotPose) { - return robotPose.transformBy(new Transform2d(ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = TurretSubsystem.TURRET_GEAR_RATIO; + config.CurrentLimits.StatorCurrentLimit = 40.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + config.Slot0.kS = 0.45; + config.Slot0.kV = 5.7; + config.Slot0.kP = 240.0; + + config.MotionMagic.MotionMagicAcceleration = 20; // 2.064; + config.MotionMagic.MotionMagicCruiseVelocity = 50; // 8.0; + + return config; } - @Override - public Command currentZeroTurret() { - // TODO idk which hardstop we should zero against but - return this.run(() -> turretIO.setVoltage(1.0)) - .until( - new Trigger(() -> Math.abs(turretCurrentFilterValue) > TURRET_CURRENT_ZERO_THRESHOLD) - .debounce(0.25)) - .andThen(Commands.parallel(Commands.print("Turret Zeroed"), zeroTurret())); + public static CANcoderConfiguration getCancoder24tConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + // this is to offset the position where both cancoders are equal to be inside the deadzone + // Offset measured at rear hardstop (approx -259 degrees) + config.MagnetSensor.MagnetOffset = + -0.765137 - TurretSubsystem.CANCODER_24T_TO_TURRET_GEAR_RATIO / 0.1; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; + + return config; } - public Command zeroTurret() { - return this.runOnce(() -> turretIO.resetTurretEncoder(TURRET_FORWARD_HARDSTOP_ANGLE)); + public static CANcoderConfiguration getCancoder26tConfigs() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + // this is to offset the position where both cancoders are equal to be inside the deadzone + // Offset measured at rear hardstop (approx -259 degrees) + config.MagnetSensor.MagnetOffset = + -0.4897 - TurretSubsystem.CANCODER_26T_TO_TURRET_GEAR_RATIO / 0.1; + config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; + + return config; } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 7fd28fae..5d61059e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -180,16 +180,16 @@ public static Rotation2d getTurretTargetRotation( // If its in the deadzone, clamp to nearest hardstop outOfRange = turretTargetDegrees > TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - && (turretTargetDegrees < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees()); + && (turretTargetDegrees < TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()); if (outOfRange) { turretTargetDegrees = // If the requested angle is greater than the halfway point in the deadzone, go to the // read hardstop, otherwise go to forward hardstop turretTargetDegrees > (TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - + TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees()) + + TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()) / 2 - ? TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees() + 2 + ? TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees() + 2 : TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 2; }