I made shuttling and stuff

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index 45deb1574177a4455e78eb8d678175cbc0318e52..5098aaacba83d9abc22f257e9e8981ddb884cf8e 100644 (file)
--- 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 2bdf67856b4cd05abcf6a750fca70050a36eeeb6..d4ed8103da2e2c52fedfe5c25d3d5b4301291484 100644 (file)
--- 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 (file)
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 :)
+}