From 9d536e6ac5d094410ef39e9d6beecc1d48b61120 Mon Sep 17 00:00:00 2001 From: ohad Date: Wed, 11 Mar 2026 20:59:45 -0400 Subject: [PATCH 1/3] Fix build --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5131262..8463dcf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -545,7 +545,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData("Drive Command", driveDirectionTime); SmartDashboard.putData("Fake vision near trench", new FakeVision(drivebase, 4, 1)); SmartDashboard.putData("Fake vision away from trench", new FakeVision(drivebase, 1, 1)); - SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); + SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0, 0, 0))); SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem)); SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem)); From 54ceb1779e9d69afd86441cd347d12eaad3eda86 Mon Sep 17 00:00:00 2001 From: ohad Date: Wed, 11 Mar 2026 22:16:54 -0400 Subject: [PATCH 2/3] Add red hub --- .../robot/subsystems/ControllerSubsystem.java | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 75ec60c..b156482 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Robot; import frc.robot.utils.math.TurretCalculations; import org.dyn4j.UnitConversion; import org.littletonrobotics.junction.Logger; @@ -13,7 +14,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; -import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -22,7 +22,8 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; // Placeholder target poses until real field target values are finalized - private static final Pose2d HUB_TARGET_POSE = new Pose2d(4.0, 4.0, Rotation2d.kZero); + private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, Constants.BLUE_HUB_Y_POSITION, Rotation2d.kZero); + private static final Pose2d RED_HUB_TARGET_POSE = new Pose2d(Constants.RED_HUB_X_POSITION, Constants.RED_HUB_Y_POSITION, Rotation2d.kZero); private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; @@ -46,8 +47,10 @@ public class ControllerSubsystem extends SubsystemBase { new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); // Placeholder pose-driven profiles. - private static final PoseControlProfile HUB_PROFILE = - new PoseControlProfile(HUB_TARGET_POSE, 32.0, 230.0, 14.0); + private static final PoseControlProfile BLUE_HUB_PROFILE = + new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, 14.0); + private static final PoseControlProfile RED_HUB_PROFILE = + new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, 14.0); private static final PoseControlProfile SHUTTLE_PROFILE = new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); @@ -136,7 +139,20 @@ private void updateTargets(ShootState state, Pose2d robotPose) { case STOPPED -> updateStoppedTargets(); case FIXED -> useShotTargets(FIXED_TARGETS); case FIXED_2 -> useShotTargets(FIXED_2_TARGETS); - case SHOOTING_HUB -> useShotTargets(calculateTargetsFromPose(HUB_PROFILE, robotPose)); + case SHOOTING_HUB -> { + if (Robot.allianceColor().isEmpty()) { + // No color, do nothing... + useShotTargets(FIXED_TARGETS); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Blue)) { + useShotTargets(calculateTargetsFromPose(BLUE_HUB_PROFILE, robotPose)); + } else if (Robot.allianceColor().get().equals(DriverStation.Alliance.Red)) { + useShotTargets(calculateTargetsFromPose(RED_HUB_PROFILE, robotPose)); + } else { + // Unknown color, do nothing... + useShotTargets(FIXED_TARGETS); + } + } + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); } } @@ -180,7 +196,7 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { - if (profile == HUB_PROFILE) { + if (profile == BLUE_HUB_PROFILE) { double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; return 0.169 * distance * distance @@ -191,7 +207,7 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo } private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { - if (profile == HUB_PROFILE) { + if (profile == BLUE_HUB_PROFILE) { double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; return Math.floor( 8.46 * distance * distance From a64896a912e6fac9f183d26af5fe2ca36242fd6c Mon Sep 17 00:00:00 2001 From: ohad Date: Wed, 11 Mar 2026 22:35:39 -0400 Subject: [PATCH 3/3] Add red hub shooting speed --- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index b156482..8574fe5 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -196,9 +196,9 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { } private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { - if (profile == BLUE_HUB_PROFILE) { + if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; - return + return 0.169 * distance * distance - 1.73 * distance + 20.4; @@ -207,7 +207,7 @@ private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseCo } private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { - if (profile == BLUE_HUB_PROFILE) { + if ((profile == BLUE_HUB_PROFILE) || (profile == RED_HUB_PROFILE)) { double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters; return Math.floor( 8.46 * distance * distance