diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 4777d33..497847e 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 3.62702380952381, + "y": 6.0 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 4.627023809523808, + "y": 6.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9612559241706156, - "y": 7.0 + "x": 2.180142857142858, + "y": 6.0 }, "prevControl": { - "x": 1.9612559241706156, - "y": 7.0 + "x": 1.180142857142858, + "y": 6.0 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.2, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 178.89829388479367 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.95980050207663 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 56a442f..ec732cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -267,29 +267,28 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.5) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterNotReady(); + m_robotShooter.spinUpIdle(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReady(); + m_robotShooter.spinUpShooting(); m_robotIntake.setMode(IntakeMode.Idle); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterReadyFeeder(); + m_robotShooter.spinUpFeeding(); })); - - + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.setShooterShoot(); + m_robotShooter.allowShooting(); })).onFalse(new InstantCommand(() -> { - m_robotShooter.setShooterNOTShoot(); + m_robotShooter.denyShooting(); })); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 4298429..8d8246e 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 96; - public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1"; - public static final String GIT_DATE = "2026-02-25 18:59:11 MST"; - public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-25 20:19:51 MST"; - public static final long BUILD_UNIX_TIME = 1772075991136L; + public static final int GIT_REVISION = 85; + public static final String GIT_SHA = "3197a3dce03bf34c001ef4009de3e7d98a181536"; + public static final String GIT_DATE = "2026-02-24 22:17:58 MST"; + public static final String GIT_BRANCH = "reveal-night"; + public static final String BUILD_DATE = "2026-02-25 16:32:13 MST"; + public static final long BUILD_UNIX_TIME = 1772062333884L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 96e667e..5c2a083 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -119,7 +119,7 @@ public final class Constants { public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE; - public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; + // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 79fbf75..a580a84 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -7,7 +8,10 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; @@ -50,43 +54,36 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { - // Shooter is actively shooting Shooting, - // Shooter is going to fire soon - Ready, - - ShootingFeeder, - ReadyFeeder, - - // Not ready to shoot - NotReady, + Feeding, + Idle } - private ShooterMode mode = ShooterMode.NotReady; + private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void setShooterReady() { - this.mode = ShooterMode.Ready; + public void spinUpShooting() { + this.mode = ShooterMode.Shooting; } - - public void setShooterReadyFeeder() { - this.mode = ShooterMode.ReadyFeeder; + public void spinUpFeeding() { + this.mode = ShooterMode.Feeding; } - public void setShooterNotReady() { - this.mode = ShooterMode.NotReady; + public void spinUpIdle() { + this.mode = ShooterMode.Idle; } - public void setShooterShoot() { + public void allowShooting() { shooterButtonReady = true; } - public void setShooterNOTShoot() { + public void denyShooting() { shooterButtonReady = false; } + @AutoLogOutput public ShooterMode getMode() { return mode; @@ -101,111 +98,133 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); - ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + // Get robot positon and speeds + ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - // - double distanceToHub = (robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm()); + + + // Calculate aim lead + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); + + // Get the robot's aim distance to hub + double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + //Center of hub to cameras in inches Logger.recordOutput("Hub Dist", distanceToHub); - - if(this.mode != ShooterMode.NotReady) { - // TODO: get if the robot is within the angle of the hub - - boolean driverError = - // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | - // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | - distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | - distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); - - double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; - double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; - - boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); - // boolean intakeBad = m_Intake.getMode() == IntxakeMode.Extended; + boolean driverError = + // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | + // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); - int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (this.mode == ShooterMode.ReadyFeeder ? 4 : 0); - switch (bitmask) { - case 0b000: // No Errors - m_robotLED.setMode(Constants.LEDConstants.OPREADY); - break; - case 0b001: // No op err, yes driver err - m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); - break; - - case 0b010: - case 0b110: // Bad flywheel, no driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); - break; - - case 0b011: - case 0b111: // Bad flywheel, yes driver err - m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); - break; - - case 0b100: - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); - break; - case 0b101: - m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED_BADPHYS); - break; - } - - // // We set the shooter mode to ready if there are no errors - mode = ( - bitmask == 0 ? - ShooterMode.Shooting : - ShooterMode.Ready - ); - - } else { - m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); - - } - - + double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + double shooterSpeedTarget = Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond) + state.motor2TargetVelocity.in(RotationsPerSecond)) / 2; + boolean badShooterVelocity = Math.abs(shooterSpeed - shooterSpeedTarget) > ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); switch (mode) { case Shooting: io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); - if(shooterButtonReady) { - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - } else { - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + int bitmask = ( + (shooterButtonReady ? 1 : 0) + + (badShooterVelocity ? 2 : 0) + + (driverError ? 4 : 0) + ); + + switch (bitmask) { + case 0b000: // No errors but button is not pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + + case 0b001: // No errors and shoot button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + + case 0b010: // Bad shooter velocity, button is not pressed + case 0b011: // Bad shooter velocty, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + + case 0b100: // Driver error, button is not pressed + case 0b101: // Driver error, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); + break; + + case 0b110: // Driver error, bad shooter vel, button is not pressed + case 0b111: // Driver error, bad shooter vel, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + break; } break; - - case Ready: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - break; - - case ShootingFeeder: + case Feeding: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_FEED_VELOCITY.get())); - - if(shooterButtonReady) { - io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - } else { - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + + int bitmask2 = ( + (shooterButtonReady ? 1 : 0) + + (badShooterVelocity ? 2 : 0) + ); + + switch (bitmask2) { + case 0b000: // No errors but button is not pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); + break; + + case 0b001: // No errors and shoot button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); + break; + + case 0b010: // Bad shooter velocity, button is not pressed + case 0b011: // Bad shooter velocty, button is pressed + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + + // case 0b100: // Driver error, button is not pressed + // case 0b101: // Driver error, button is pressed + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + // break; + + // case 0b110: // Driver error, bad shooter vel, button is not pressed + // case 0b111: // Driver error, bad shooter vel, button is pressed + // m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + // io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + // break; } - break; - case ReadyFeeder: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); break; + case Idle: - case NotReady: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + io.setShooterCurrentLimitSpeed( + state, + ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get() + // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), + // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) + ); io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - } + } } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 3f4c63d..c504b1d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,14 +18,18 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - // public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -40); public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); - public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); + // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.3); + // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); + // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); + public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); // Shoot mode tolerances diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 463cb4b..c7d3369 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -38,6 +38,12 @@ public interface ShooterIO { // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity + ) {} // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerOutput(ShooterState state, double percentOutput) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 04fb459..9a71787 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,9 +1,13 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; public class ShooterReal implements ShooterIO { @@ -54,6 +58,31 @@ public class ShooterReal implements ShooterIO { m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } + @Override + public void setShooterCurrentLimitSpeed( + ShooterState state, + double percentOutput + // Current currentLimit, + // AngularVelocity targetVelocity + ) { + // state.motor1TargetVelocity = targetVelocity; + // state.motor2TargetVelocity = targetVelocity; + + // double current = Math.abs(state.motor1Current.in(Amps)) + Math.abs(state.motor2Current.in(Amps)); + // double velocity = (Math.abs(state.motor1Velocity.in(RotationsPerSecond)) + Math.abs(state.motor2Velocity.in(RotationsPerSecond))) / 2; + + // if( + // Math.abs(currentLimit.in(Amps)) > current && + // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity + // ) { + m_shooter1Motor.set(percentOutput); + m_shooter2Motor.set(percentOutput); + // } else { + // m_shooter1Motor.set(0); + // m_shooter2Motor.set(0); + // } + } + @Override public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 4d2c808..a6c772e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Rotations; + import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -245,6 +247,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + if(!TimesNegativeOne.isRed) { + leftStick.rotateBy(new Rotation2d(Math.PI/2.)); + } + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index d0b438c..f3a1f17 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -6,7 +6,7 @@ public class FieldPositions { // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534 + 0.3); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534 + 0.3); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534);// + 0.3); // We set the default position to one just in case it doesn't update