Skip to content

Fix build, add red hub #112

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JAMthepersonj merged 3 commits into from
Mar 12, 2026
+25 −9
Merged

Fix build, add red hub #112

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9d536e6
Fix build
armadilloz Mar 12, 2026
54ceb17
Add red hub
armadilloz Mar 12, 2026
a64896a
Add red hub shooting speed
armadilloz Mar 12, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down
32 changes: 24 additions & 8 deletions src/main/java/frc/robot/subsystems/ControllerSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand All @@ -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";
Expand All @@ -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);

Expand Down Expand Up @@ -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));
}
}
Expand Down Expand Up @@ -180,9 +196,9 @@ private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) {
}

private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) {
if (profile == 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;
Expand All @@ -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) || (profile == RED_HUB_PROFILE)) {
double distance = UnitConversion.METER_TO_FOOT * computedDistanceMeters;
return Math.floor(
8.46 * distance * distance
Expand Down