From 462a4b024d9afc210abd9fc0147495b9a2eccaaf Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Tue, 27 Jan 2026 09:30:24 -0800 Subject: [PATCH] I made shuttling and stuff --- .../robot/commands/gpm/TurretAutoShoot.java | 41 +++++-- .../frc/robot/constants/FieldConstants.java | 100 ++++++++++++++++++ .../java/frc/robot/util/ShootingTarget.java | 8 ++ 3 files changed, 143 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/util/ShootingTarget.java diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java index 45deb15..5098aaa 100644 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import java.lang.reflect.Field; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -11,6 +13,7 @@ import frc.robot.Robot; import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.turret.Turret; +import frc.robot.util.ShootingTarget; import frc.robot.util.Vision.TurretVision; public class TurretAutoShoot extends Command { @@ -26,7 +29,7 @@ public class TurretAutoShoot extends Command { private boolean turretVisionEnabled = false; private boolean SOTM = true; - + private Translation2d drivepose = drivetrain.getPose().getTranslation(); public TurretAutoShoot(Turret turret, Drivetrain drivetrain, TurretVision turretVision){ this.turret = turret; this.drivetrain = drivetrain; @@ -39,9 +42,22 @@ public class TurretAutoShoot extends Command { this(turret, drivetrain, null); } - public void updateTurretSetpoint() { - Translation2d drivepose = drivetrain.getPose().getTranslation(); - Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); + public void updateTurretSetpoint(Translation2d drivepose) { + ShootingTarget shootingTarget = getZone(drivepose); + Translation2d target; + switch (shootingTarget) { + case HUB: + target = FieldConstants.getHubTranslation().toTranslation2d(); + case NEUTRAL: + target = FieldConstants.getNeutralTranslation(leftSide(drivepose)).toTranslation2d(); + case ALLIANCE: + target = FieldConstants.getAllianceTranslation(leftSide(drivepose)).toTranslation2d(); + case OPPOSITION: + target = FieldConstants.getOppositionTranslation(leftSide(drivepose)).toTranslation2d(); + default: + target = FieldConstants.getHubTranslation().toTranslation2d(); + } + double D_y; double D_x; // TODO: Change time to goal on actual comp bot @@ -98,14 +114,14 @@ public class TurretAutoShoot extends Command { @Override public void initialize() { // Initialize setpoint calculation and set the initial goal for the turret - updateTurretSetpoint(); + updateTurretSetpoint(drivepose); turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2)); } @Override public void execute() { // Continuously update setpoints and adjust based on vision if available - updateTurretSetpoint(); + updateTurretSetpoint(drivepose); updateYawToTag(); if(turretVision != null && turretVisionEnabled && turret.atGoal()){ @@ -121,4 +137,17 @@ public class TurretAutoShoot extends Command { // Set the turret to a safe position when the command ends turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); } + + public boolean leftSide(Translation2d drivepose) { + if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) { + return true; + } else { + return false; + } + } + + public ShootingTarget getZone(Translation2d drivepose) { + return FieldConstants.getZone(drivepose); + } } + diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 2bdf678..d4ed810 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -1,11 +1,18 @@ package frc.robot.constants; +import java.lang.reflect.Field; + +import org.opencv.dnn.Net; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Robot; +import frc.robot.util.ShootingTarget; public class FieldConstants { /** Width of the field [meters] */ @@ -16,9 +23,26 @@ public class FieldConstants { /**Apriltag layout for 2026 REBUILT */ public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); + /** Location of hub target */ public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72)); + + // TODO: Update all of this + public static final Translation3d NEUTRAL_LEFT = + new Translation3d(field.getFieldLength()*0.5, field.getFieldWidth()*0.25, 0); + + public static final Translation3d NEUTRAL_RIGHT = + new Translation3d(field.getFieldLength()*0.5, field.getFieldWidth() - NEUTRAL_LEFT.getX(), 0); + + public static final Translation3d ALLIANCE_LEFT_BLUE = + new Translation3d(156.8+20+50, field.getFieldWidth()*0.25, 0); // previous hub + a few feet + + public static final Translation3d ALLIANCE_RIGHT_BLUE = + new Translation3d(158.8+20+50, field.getFieldWidth() - ALLIANCE_LEFT_BLUE.getX(), 0); + + public static final double BlueAllianceLine = FieldConstants.FIELD_LENGTH * 0.75; + public static final double RedAllianceLine = FieldConstants.FIELD_LENGTH * 0.25; public static Translation3d getHubTranslation() { if (Robot.getAlliance() == Alliance.Blue) { @@ -31,4 +55,80 @@ public class FieldConstants { ); } } + + public static Translation3d getNeutralTranslation(boolean sideLeft) { + if (sideLeft) { + return NEUTRAL_LEFT; + } else { + return NEUTRAL_RIGHT; + } + } + + public static Translation3d getAllianceTranslation(boolean sideLeft) { + if (sideLeft) { + if (Robot.getAlliance() == Alliance.Blue) { + return ALLIANCE_LEFT_BLUE; + } else { + return new Translation3d( + field.getFieldLength() - ALLIANCE_LEFT_BLUE.getX(), + ALLIANCE_LEFT_BLUE.getY(), + ALLIANCE_LEFT_BLUE.getZ() + ); + } + } else { + if (Robot.getAlliance() == Alliance.Blue) { + return ALLIANCE_RIGHT_BLUE; + } else { + return new Translation3d( + field.getFieldLength() - ALLIANCE_LEFT_BLUE.getX(), + ALLIANCE_RIGHT_BLUE.getY(), + ALLIANCE_RIGHT_BLUE.getZ() + ); + } + } + } + + public static ShootingTarget getZone(Translation2d drivepose) { + double x = drivepose.getX(); + double y = drivepose.getY(); + if(x < FieldConstants.RedAllianceLine) { // inside red + if (Robot.getAlliance() == Alliance.Red) { + return ShootingTarget.ALLIANCE; + } else { + return ShootingTarget.OPPOSITION; + } + } else if (x > FieldConstants.BlueAllianceLine) { + if (Robot.getAlliance() == Alliance.Blue) { + return ShootingTarget.ALLIANCE; + } else { + return ShootingTarget.OPPOSITION; + } + } else { + return ShootingTarget.NEUTRAL; + } + } + + public static Translation3d getOppositionTranslation(boolean sideLeft) { + if (sideLeft) { + if (Robot.getAlliance() == Alliance.Blue) { + return new Translation3d( + field.getFieldLength() - ALLIANCE_LEFT_BLUE.getX(), + ALLIANCE_LEFT_BLUE.getY(), + ALLIANCE_LEFT_BLUE.getZ() + ); + } else { + return ALLIANCE_LEFT_BLUE; + } + } else { + if (Robot.getAlliance() == Alliance.Blue) { + return new Translation3d( + field.getFieldLength() - ALLIANCE_LEFT_BLUE.getX(), + ALLIANCE_RIGHT_BLUE.getY(), + ALLIANCE_RIGHT_BLUE.getZ() + ); + } else { + return ALLIANCE_RIGHT_BLUE; + } + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/ShootingTarget.java b/src/main/java/frc/robot/util/ShootingTarget.java new file mode 100644 index 0000000..547097e --- /dev/null +++ b/src/main/java/frc/robot/util/ShootingTarget.java @@ -0,0 +1,8 @@ +package frc.robot.util; + +public enum ShootingTarget { + HUB, + NEUTRAL, + ALLIANCE, + OPPOSITION, // not sure why you'd ever do this :) +} -- 2.39.5