diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 093c0d5d..fff70594 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -76,6 +76,13 @@ public enum FeedTarget { RIGHT } + public enum FixedShotTarget { + LEFT, + MID, + RIGHT, + NONE + } + @AutoLogOutput(key = "Superstructure/State") private static SuperState state = SuperState.IDLE; @@ -119,7 +126,7 @@ public enum FeedTarget { @AutoLogOutput(key = "Superstructure/Score Request") private Trigger scoreReq = new Trigger(() -> shotTarget == ShotTarget.SCORE) - .and(() -> canScore()) + // .and(() -> canScore()) .or(Autos.autoScoreReq); @AutoLogOutput(key = "Superstructure/Feed Request") @@ -140,9 +147,12 @@ public enum FeedTarget { @AutoLogOutput(key = "Superstructure/Ready?") private Trigger readyTrigger; - @AutoLogOutput(key = "Superstructure/Operator Override?") + @AutoLogOutput(key = "Superstructure/Operator Pose Override?") private boolean override; + @AutoLogOutput(key = "Superstructure/Fixed Shot") + private static FixedShotTarget fixedShotTarget = FixedShotTarget.NONE; + /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, @@ -190,11 +200,18 @@ private void addTriggers() { operator.leftTrigger().onTrue(Commands.runOnce(() -> override = true)); operator.rightTrigger().onTrue(Commands.runOnce(() -> override = 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)); + shootReq = driver .rightTrigger() .and(DriverStation::isTeleop) - .or(Autos.autoScoreReq); // Maybe should include if its our turn? //TODO fix auto + .and(() -> canShoot()) + .or(Autos.autoScoreReq) + .and(() -> canShoot()); // Maybe should include if its our turn? //TODO fix auto // bindings intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); @@ -311,7 +328,10 @@ private void addTransitions() { (preClimbReq.and(climbReq.negate()).and(() -> DriverStation.isTeleop())) .onTrue(changeStateTo(SuperState.PRE_CLIMB)); - bindTransition(SuperState.PRE_CLIMB, SuperState.CLIMB, climbReq); + bindTransition( + SuperState.PRE_CLIMB, + SuperState.CLIMB, + climbReq); // TODO maybe add transition out of climb in case we fall bindTransition( SuperState.PRE_CLIMB, SuperState.IDLE, preClimbReq.negate().and(climbReq.negate())); @@ -746,6 +766,10 @@ public boolean canScore() { && !swerve.isNearTrench(); } + public boolean canShoot() { + return !swerve.isNearTrenchForHood(); + } + public static ShotTarget getShotTarget() { return shotTarget; } @@ -753,4 +777,8 @@ public static ShotTarget getShotTarget() { public static FeedTarget getFeedTarget() { return feedTarget; } + + public static FixedShotTarget getFixedShotTarget() { + return fixedShotTarget; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index c8fd9577..632c5f84 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -196,7 +196,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelFollowerTorqueCurrent = flywheelFollowerTorqueCurrent.getValueAsDouble(); } - @AutoLogOutput(key = "Shooter/Setpoint") + @AutoLogOutput(key = "Shooter/Flywheel/Setpoint") public double getSetpointRotPerSec() { return velocitySetpointRotPerSec; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ae05b484..1d8e7ff6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -113,4 +113,8 @@ public default void turretInit() {} public default Pose2d getTurretPose(Pose2d robotPose) { return Pose2d.kZero; } + + public default Command currentZeroTurret() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 079da6b6..6fb1df19 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -29,6 +29,7 @@ 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; @@ -50,9 +51,14 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(73); public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(23.16); public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0; + public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find public static final double FLYWHEEL_DIAMETER_INCHES = 4; + public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; + public static final Rotation2d TURRET_MID_FIXED_SHOT_ANGLE = Rotation2d.kZero; + // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing @AutoLogOutput(key = "Shooter/Turret/Rear Hardstop") @@ -72,7 +78,8 @@ 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 currentFilterValue = 0.0; + double hoodCurrentFilterValue = 0.0; + double turretCurrentFilterValue = 0.0; private CANcoderIO cancoder24t; private CANcoderIO cancoder26t; @@ -182,7 +189,7 @@ public void periodic() { Logger.processInputs("Shooter/Turret Cancoder24t", cancoder24tInputs); cancoder26t.updateInputs(cancoder26tInputs); Logger.processInputs("Shooter/Turret Cancoder26t", cancoder26tInputs); - currentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); + hoodCurrentFilterValue = currentFilter.calculate(hoodInputs.hoodStatorCurrentAmps); cancoder24tDisconnectedAlert.set(!cancoder24tInputs.connected); cancoder26tDisconnectedAlert.set(!cancoder26tInputs.connected); @@ -398,7 +405,7 @@ public Command zeroHood() { public Command runHoodCurrentZeroing() { return this.run(() -> hoodIO.setHoodVoltage(-3.0)) .until( - new Trigger(() -> Math.abs(currentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) + new Trigger(() -> Math.abs(hoodCurrentFilterValue) > HOOD_CURRENT_ZERO_THRESHOLD) .debounce(0.25)) .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); } @@ -458,15 +465,35 @@ public Command score( Supplier chassisSpeedsSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + 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())); + } }); } @@ -639,14 +666,35 @@ public Command spinUp( Supplier chassisSpeedsSupplier) { return this.run( () -> { - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); + 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())); + } }); } @@ -654,4 +702,18 @@ public Command spinUp( public Pose2d getTurretPose(Pose2d robotPose) { return robotPose.transformBy(new Transform2d(ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); } + + @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 Command zeroTurret() { + return this.runOnce(() -> turretIO.resetTurretEncoder(TURRET_FORWARD_HARDSTOP_ANGLE)); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 04be3342..5e7a1fcf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -818,6 +818,20 @@ public boolean isNearTrench() { return inXTol && inYTol; } + @AutoLogOutput(key = "Swerve/Near Trench for hood") + public boolean isNearTrenchForHood() { + double x = getPose().getX(); + double y = getPose().getY(); + + boolean inXTol = + MathUtil.isNear(TrenchPoses.BLUE_RIGHT.getPose().getX(), x, 1) + || MathUtil.isNear(TrenchPoses.RED_RIGHT.getPose().getX(), x, 1); + boolean inYTol = + MathUtil.isNear(TrenchPoses.BLUE_RIGHT.getPose().getY(), y, 0.515) + || MathUtil.isNear(TrenchPoses.RED_RIGHT.getPose().getY(), y, 0.515); + return inXTol && inYTol; + } + public boolean isNearBump() { double x = getPose().getX(); double y = getPose().getY(); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 8ec53e41..7fd28fae 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -24,6 +24,11 @@ public class AutoAim { public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); + public static final Rotation2d LEFT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-73.916016); + public static final Rotation2d MID_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-82); + public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = + Rotation2d.fromDegrees(-109.775391); + static { // For hub shot tree ALPHA_HUB_SHOT_TREE.put( Units.inchesToMeters(24 + 17), new ShotData(Rotation2d.fromDegrees(8), 27.5, 1.46)); @@ -256,4 +261,16 @@ public static ShotData getCompensatedSOTMShotData( public static boolean targetInTurretDeadzone() { return outOfRange; } + + public static ShotData getLeftFixedShotData() { + return new ShotData(Rotation2d.fromDegrees(36), 36, 0); + } + + public static ShotData getRightFixedShotData() { + return new ShotData(Rotation2d.fromDegrees(23.16), 35.7, 0); + } + + public static ShotData getMidFixedShotData() { + return new ShotData(Rotation2d.fromDegrees(32.84), 35, 0); + } }