Minor changes.

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b299c2b3ffeaea326f2b3a78cb7814c3cfd40e5b..a9b8670987070bcda14b57adbe2b98f03a750cff 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -29,10 +29,10 @@ import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.Turret;
-import frc.robot.subsystems.hood.Hood;
 import frc.robot.util.PathGroupLoader;
 import frc.robot.util.Vision.DetectedObject;
 import frc.robot.util.Vision.Vision;

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
index 32197d5e5c1682231c34340d8fbd791be3e7064c..0188a0d42a75033e4a4c05bbaca8d7d21232d862 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
@@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
+import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
 public class TrenchAssist2 {
@@ -19,7 +20,7 @@ public class TrenchAssist2 {
 
         double distanceFromSlideLatitude;
 
-        if (drive.getPose().getY() > (8.07 / 2.0)) {
+        if (drive.getPose().getY() > (FieldConstants.FIELD_WIDTH / 2.0)) {
             distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]);
         } else {
             distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]);
@@ -29,14 +30,14 @@ public class TrenchAssist2 {
                 new Pose2d[] { new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[0], Rotation2d.kZero),
                         new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[1], Rotation2d.kZero) });
 
-        double impulse = pid.calculate(distanceFromSlideLatitude, 0);
+        double correctionVelocity = pid.calculate(distanceFromSlideLatitude, 0);
 
         if (distanceFromSlideLatitude < Units.inchesToMeters(3)){
-            impulse = 0.0;
+            correctionVelocity = 0.0;
         }
 
 
-        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse,
+        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, correctionVelocity,
                 chassisSpeeds.omegaRadiansPerSecond);
 
         var y = new Translation2d(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond).rotateBy(drive.getYaw());
@@ -61,4 +62,4 @@ public class TrenchAssist2 {
         return robotTranslation.rotateBy(yaw.times(-1));
 
     }
-}
\ No newline at end of file
+}

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index 29a32847636e326a32b4da821cb9a54541111f51..f745e2ed1e5cc1581db98396b7b95b7fdc4e06ec 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -3,6 +3,7 @@ package frc.robot.util.TrenchAssist;
 import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.constants.FieldConstants;
 
 public class TrenchAssistConstants {
     public static final Rectangle2d[] OBSTACLES = new Rectangle2d[] {
@@ -21,11 +22,11 @@ public class TrenchAssistConstants {
     };
 
     public static final double[] SLIDE_LATITUDES = new double[]{
-        8.07 - Units.inchesToMeters(30.0),
+        FieldConstants.FIELD_WIDTH - Units.inchesToMeters(30.0),
         Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small
         // 6.550,
         // 0.668,
 
     };
 
-}
\ No newline at end of file
+}