a

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 1e68d07a335a169eb6b42fb5d04f16906566bdf4..7b957154d03b476f6f0c7d0a3b6c310fe7b8d4c7 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -22,8 +22,8 @@ public class FieldConstants {
 
   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;
+  public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.10;
+  public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.90;
 
   /**The coordinate of the climb position */
   public static final Pose2d BLUE_CLIMB_LOCATION = new Pose2d(1.5, FIELD_WIDTH/2 - 2.0, new Rotation2d()); // TODO: find this
@@ -151,19 +151,30 @@ public class FieldConstants {
     }
   }
 
+  /**
+   * 
+   * @return Whether Y coordinate is in the upper half (left side on blue alliance)
+   */
+  public static boolean isOnLeftSideOfField(Translation2d drivepose){
+    if (drivepose.getY() > FIELD_WIDTH/2){
+      return true;
+    }
+    return false;
+  }
+
   public static Translation3d getOppositionTranslation(boolean sideLeft) {
     if (sideLeft) {
       if (Robot.getAlliance() == Alliance.Blue) {
         return ALLIANCE_LEFT_RED;
       } else {
         // Reversed it so we shoot same side, but probably need to change this
-        return ALLIANCE_RIGHT_BLUE;
+        return ALLIANCE_LEFT_BLUE;
       }
     } else {
       if (Robot.getAlliance() == Alliance.Blue) {
         return ALLIANCE_RIGHT_RED;
       } else {
-        return ALLIANCE_LEFT_BLUE;
+        return ALLIANCE_RIGHT_BLUE;
       }
     }
   }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 7c7e9bfc477af87a92d19bab5030e4ee8efdaae8..ecc2bd1185f023a2537b8fbb8187031b08bba728 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -183,9 +183,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             // }));
 
             // Set the hood down -- for safety measures under trench
-            controller.get(DPad.LEFT).whileTrue(new InstantCommand(()->{
-                hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0);
-            }, hood));
+            controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{
+                hood.forceHoodDown(true);
+            }, hood)).onFalse(new InstantCommand(()->{
+                hood.forceHoodDown(false);
+            }));
         }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index b71304226c38d4a7548dc27754c587f5c1e54302..ade7e350dc728ed11ba5533aebe1b2a6a7f13c1d 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -36,6 +36,8 @@ public class Hood extends SubsystemBase implements HoodIO{
        private boolean calibrating = false;
        private Debouncer calibrateDebouncer = new Debouncer(0.5, DebounceType.kRising);
 
+       private boolean forceHoodDown = false;
+
     private HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged();
 
     public Hood(){
@@ -88,11 +90,20 @@ public class Hood extends SubsystemBase implements HoodIO{
                return Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / HoodConstants.HOOD_GEAR_RATIO;
        }
 
+       public void forceHoodDown(boolean taranNathan){
+               forceHoodDown = taranNathan;
+       }
+
     @Override
     public void periodic() {
                updateInputs();
                Logger.processInputs("Hood", inputs);
 
+               if (forceHoodDown){
+                       goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE);
+                       goalVelocityRadPerSec = 0.0;
+               }
+
                double setpointRad = goalAngle.getRadians();
 
         // calculate shortest angular delta

diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java
index 45375347d2945ec97daa992a841d1ee9541bfe7f..9eb3931db16ac7ba692f543067ee01684255fe0d 100644 (file)
--- a/src/main/java/frc/robot/util/PhaseManager.java
+++ b/src/main/java/frc/robot/util/PhaseManager.java
@@ -84,9 +84,10 @@ public class PhaseManager {
         return !(currentState == CurrentState.TURNING_AROUND) && !(currentState == CurrentState.UNDER_TRENCH); //&& !(currentState == CurrentState.STARTING_UP);
     }
 
-    public Translation2d getTarget() {
-        return wantedState == WantedState.SHOOTING ? 
-            FieldConstants.getHubTranslation().toTranslation2d() 
-            : FieldConstants.getAllianceSideTranslation(true).toTranslation2d();
+    public Translation2d getTarget(Pose2d drivePose) {
+        return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d()
+                : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation())
+                        ? FieldConstants.getAllianceSideTranslation(true).toTranslation2d()
+                        : FieldConstants.getAllianceSideTranslation(false).toTranslation2d());
     }
 }
\ No newline at end of file