fixes

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 1ab9f939d7fdfcbfc9763af5ace303c34ba0be52..e721a3920ec0aced5dd15e7042d5b8f059913777 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -1,5 +1,7 @@
 package frc.robot.commands.gpm;
 
+import static edu.wpi.first.units.Units.Rotation;
+
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.MathUtil;
@@ -26,6 +28,7 @@ import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.turret.TurretConstants;
 import frc.robot.util.PhaseManager;
 import frc.robot.util.ShooterPhysics;
+import frc.robot.util.PhaseManager.CurrentState;
 import frc.robot.util.ShooterPhysics.Constraints;
 import frc.robot.util.ShooterPhysics.TurretState;
 
@@ -208,7 +211,13 @@ public class Superstructure extends Command {
             stowEverything();
         } else {
             turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2));
-            hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
+            
+            if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
+            } else{
+                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity);
+            }
+            
             shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
             if (phaseManager.shouldFeed()) {

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 6fc6512adb89740970c1fe9da6bac2689511c0fb..a9f60e79593910b284e026e375e69fa42f228225 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -20,8 +20,8 @@ public class FieldConstants {
   /** Height of the field [meters] */
   public static final double FIELD_WIDTH = field.getFieldWidth();
 
-  public static final double RED_BORDER = Units.inchesToMeters(180);
-  public static final double BLUE_BORDER = FIELD_LENGTH - Units.inchesToMeters(180);
+  public static final double RED_BORDER = FIELD_LENGTH/2 + Units.inchesToMeters(167.0);
+  public static final double BLUE_BORDER = FIELD_LENGTH/2 - Units.inchesToMeters(167.0);
   public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.25;
   public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.75;
 
@@ -40,10 +40,10 @@ public class FieldConstants {
 
   /** Location of hub target */
   public static final Translation3d HUB_BLUE =
-      new Translation3d(Units.inchesToMeters(156.8 + 20), FIELD_WIDTH/2, Units.inchesToMeters(72));
+      new Translation3d(Units.inchesToMeters(182.11), FIELD_WIDTH/2, Units.inchesToMeters(72));
   
   public static final Translation3d HUB_RED =
-      new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 - 20), FIELD_WIDTH/2, Units.inchesToMeters(72));
+      new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11), FIELD_WIDTH/2, Units.inchesToMeters(72));
     
   public static final Translation3d NEUTRAL_LEFT =
     new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0);
@@ -52,23 +52,23 @@ public class FieldConstants {
     new Translation3d(FIELD_LENGTH/2, RIGHT_SIDE_TARGET, 0);
 
   public static final Translation3d ALLIANCE_LEFT_BLUE =
-    new Translation3d(BLUE_BORDER - 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
+    new Translation3d(BLUE_BORDER - 2, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
 
   public static final Translation3d ALLIANCE_RIGHT_BLUE =
-    new Translation3d(BLUE_BORDER - 5, RIGHT_SIDE_TARGET, 0);
+    new Translation3d(BLUE_BORDER - 2, RIGHT_SIDE_TARGET, 0);
 
 
   public static final Translation3d ALLIANCE_LEFT_RED =
-    new Translation3d(RED_BORDER + 5, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
+    new Translation3d(RED_BORDER + 2, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
 
   public static final Translation3d ALLIANCE_RIGHT_RED =
-    new Translation3d(RED_BORDER + 5, RIGHT_SIDE_TARGET, 0);
+    new Translation3d(RED_BORDER + 2, RIGHT_SIDE_TARGET, 0);
 
   public static final Translation3d ALLIANCE_CENTER_BLUE =
-    new Translation3d(BLUE_BORDER - 5, FIELD_WIDTH/2, 0);
+    new Translation3d(BLUE_BORDER - 2, FIELD_WIDTH/2, 0);
   
   public static final Translation3d ALLIANCE_CENTER_RED =
-    new Translation3d(RED_BORDER + 5, FIELD_WIDTH/2, 0);
+    new Translation3d(RED_BORDER + 2, FIELD_WIDTH/2, 0);
 
   public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump
   public static final double RED_ALLIANCE_LINE = RED_BORDER; // 
@@ -130,6 +130,10 @@ public class FieldConstants {
   public static FieldZone getZone(Translation2d drivepose) {
     double x = drivepose.getX();
     //double y = drivepose.getY();
+    if ((x < FIELD_LENGTH/2 - Units.inchesToMeters(120.0) && x > BLUE_ALLIANCE_LINE)
+        || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE){
+          return FieldZone.TRENCH_BUMP;
+        }
     if(x < FieldConstants.RED_ALLIANCE_LINE) { // inside red
       if (Robot.getAlliance() == Alliance.Red) {
         return FieldZone.ALLIANCE;
@@ -152,13 +156,14 @@ public class FieldConstants {
       if (Robot.getAlliance() == Alliance.Blue) {
         return ALLIANCE_LEFT_RED;
       } else {
-        return ALLIANCE_LEFT_BLUE;
+        // Reversed it so we shoot same side, but probably need to change this
+        return ALLIANCE_RIGHT_BLUE;
       }
     } else {
       if (Robot.getAlliance() == Alliance.Blue) {
         return ALLIANCE_RIGHT_RED;
       } else {
-        return ALLIANCE_RIGHT_BLUE;
+        return ALLIANCE_LEFT_BLUE;
       }
     }
   }

diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java
index a62b290b0bc37c1b79a46c5f9ce4299b402f0091..61b1e759686ad1cf039bb7c58db08e135919e8e7 100644 (file)
--- a/src/main/java/frc/robot/util/PhaseManager.java
+++ b/src/main/java/frc/robot/util/PhaseManager.java
@@ -19,6 +19,7 @@ public class PhaseManager {
         IDLE,
         STARTING_UP,
         TURNING_AROUND,
+        UNDER_TRENCH,
         SHOOTING,
         PASSING
     }
@@ -51,6 +52,8 @@ public class PhaseManager {
         FieldZone zone = FieldConstants.getZone(drivePose.getTranslation());
         if (zone == FieldConstants.FieldZone.ALLIANCE) {
             currentState = CurrentState.SHOOTING;
+        } else if (zone == FieldConstants.FieldZone.TRENCH_BUMP){
+            currentState = CurrentState.UNDER_TRENCH;
         } else {
             currentState = CurrentState.PASSING;
         }
@@ -78,7 +81,7 @@ public class PhaseManager {
     
     public boolean shouldFeed() {
         // TODO: I'm gonna comment out starting up until i find a solution
-        return !(currentState == CurrentState.TURNING_AROUND); //&& !(currentState == CurrentState.STARTING_UP);
+        return !(currentState == CurrentState.TURNING_AROUND) && !(currentState == CurrentState.UNDER_TRENCH); //&& !(currentState == CurrentState.STARTING_UP);
     }
 
     public Translation2d getTarget() {