more fixes

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index afcd616074daa01f72feeadefa352601369750c3..cf16f9a01b6fedb8344b8cb31b70b73a9167771a 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -230,9 +230,9 @@ public class Superstructure extends Command {
                 hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
             }
             
-            
             double x = drivepose.getX(); // compared as meters
             double y = drivepose.getY();
+
             // if (FieldConstants.underTrench(x, y)) {
             //     System.out.println("Hood forced down");
             // } else {

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 451307a585170ed39b46cf78a393ac1b05dbcd81..e04673acbd276a8241670a16bfe6c26f44c49fdf 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -93,6 +93,10 @@ public class FieldConstants {
   public static final double ladderRedRight = FIELD_WIDTH - 35.75;
   public static final double ladderBlueRight = FIELD_WIDTH + 35.75;
 
+  public static final double TRENCH_CENTER_CHANNEL_WIDTH_INCHES = 50.0;
+  public static final double TRENCH_X_MIN_INCHES = 152.5;
+  public static final double TRENCH_X_MAX_INCHES = 187.5;
+
   public static final Zone neutralStrip = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
   public static final Zone neutralLeft = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
   public static final Zone neutralRight = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
@@ -193,12 +197,12 @@ public class FieldConstants {
 
   public static boolean underTrench(double x, double y) {
     // ensures we aren't in center channel
-    if (y > Units.inchesToMeters(50.0) && y < FIELD_WIDTH - Units.inchesToMeters(50)) {
+    if (y > Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES) && y < FIELD_WIDTH - Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)) {
       return false;
     }
     // if our location is to far away from right underneath trench in terms of x
     // in between blue alliance trench
-    if (!(x > Units.inchesToMeters(152.5) && x < Units.inchesToMeters(187.5)) && !(x < FIELD_LENGTH - Units.inchesToMeters(152.5) && x > FIELD_LENGTH - Units.inchesToMeters(187.5))) {
+    if (!(x > Units.inchesToMeters(TRENCH_X_MIN_INCHES) && x < Units.inchesToMeters(TRENCH_X_MAX_INCHES)) && !(x < FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MIN_INCHES) && x > FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MAX_INCHES))) {
       return false;
     }
     return true;

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 205e9b124b179879b54ed5f5dae0f8fb032945e2..346629273e836f0a15f28c86c927b3ca2e541042 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -40,7 +40,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Hood hood;
     private Intake intake;
     private Spindexer spindexer;
-    private LinearClimb climb;
 
     public PS5ControllerDriverConfig(
             Drivetrain drive,
@@ -56,7 +55,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         this.hood = hood;
         this.intake = intake;
         this.spindexer = spindexer;
-        this.climb = climb;
     }
 
     public void configureControls() {
@@ -121,13 +119,15 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 .alongWith(new InstantCommand(()-> intakeBoolean = true)));
 
             // Calibration: you can now calibrate easily using this button
-            controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
-                intake.calibrate();
-                hood.calibrate();
-            })).onFalse(new InstantCommand(() -> {
-                intake.stopCalibrating();
-                hood.stopCalibrating();
-            }, intake, hood));
+            if (hood != null && intake != null) {
+                controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+                    intake.calibrate();
+                    hood.calibrate();
+                }, intake, hood)).onFalse(new InstantCommand(() -> {
+                    intake.stopCalibrating();
+                    hood.stopCalibrating();
+                }, intake, hood));
+            }
 
             // Stop intake roller
             controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{
@@ -151,7 +151,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         }
 
         // Auto shoot
-        if (turret != null && hood != null && shooter != null) {
+        if (turret != null && hood != null && shooter != null && spindexer != null) {
             autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer);
             controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot);
         }

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 9fb8779488caf9a75e539dc00a704d47953d0f01..5620b76d1b1d9a120e9bccb5a3d401feb9657ed7 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -109,10 +109,9 @@ public class Hood extends SubsystemBase implements HoodIO {
                // goalAngle = Rotation2d.fromDegrees(SmartDashboard.getNumber("Hood Setpoint", goalAngle.getDegrees()));
                // SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees());
 
-               if (forceHoodDown){
+               if (forceHoodDown) {
                        goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE);
                        goalVelocityRadPerSec = 0.0;
-               } else {
                }
 
                double setpointRad = goalAngle.getRadians();