Merge #79

Patribots4738 · Mar 17, 2023 · 4068ce4 · 4068ce4
1 parent 0b6c2d8
commit 4068ce4
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 0 deletions.
diff --git a/src/main/java/calc/Constants.java b/src/main/java/calc/Constants.java
@@ -403,6 +403,10 @@ public static final class AlignmentConstants {
         public static final double ALLOWABLE_ERROR_METERS = Units.inchesToMeters(2);
         public static final double FIELD_WIDTH_METERS = 16.53;
 
+        public static final double GRID_WIDTH_METERS = 1.364;
+        public static final double SUBSTATION_WIDTH_METERS = 0.344;
+        public static final double FIELD_HEIGHT_METERS = 8.029;
+
         public static final double CHARGE_PAD_CORRECTION_P = 0.05;
 
         public static final Pose3d TAG_0_POSE = null;

diff --git a/src/main/java/hardware/Swerve.java b/src/main/java/hardware/Swerve.java
@@ -20,13 +20,16 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import io.github.oblarg.oblog.Loggable;
 import io.github.oblarg.oblog.annotations.Log;
 import calc.SwerveUtils;
+import calc.Constants.AlignmentConstants;
 import calc.Constants.DriveConstants;
+import calc.Constants.PlacementConstants;
 
 public class Swerve implements Loggable{
 
@@ -122,6 +125,7 @@ public void periodic() {
         // Update the odometry in the periodic block
         this.field.setRobotPose(getPose());
         poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getGyroAngle(), getModulePositions());
+        clampOdometry();
         getPitch();
         getRoll();
         // if (Math.abs(getPitch().getDegrees()) > 7 || Math.abs(getRoll().getDegrees()) > 7) {
@@ -130,6 +134,30 @@ public void periodic() {
         // }
     }
 
+    private void clampOdometry() {
+      // Clamp the position Odometry on the low end when on BLUE alliance
+      // Clamp the position Odometry on the high end when on RED alliance
+      Pose2d clampedPose =
+          (DriverStation.getAlliance() == DriverStation.Alliance.Blue) ?
+              new Pose2d(
+                      MathUtil.clamp(getPose().getX(), AlignmentConstants.GRID_WIDTH_METERS - (PlacementConstants.ROBOT_LENGTH_METERS/2),
+                          AlignmentConstants.FIELD_WIDTH_METERS - AlignmentConstants.SUBSTATION_WIDTH_METERS - (PlacementConstants.ROBOT_LENGTH_METERS/2)),
+                      MathUtil.clamp(getPose().getY(),
+                          (0),
+                          AlignmentConstants.FIELD_HEIGHT_METERS),
+                      getPose().getRotation()) :
+              new Pose2d(
+                      MathUtil.clamp(getPose().getX(), AlignmentConstants.SUBSTATION_WIDTH_METERS - (PlacementConstants.ROBOT_LENGTH_METERS/2),
+                          AlignmentConstants.FIELD_WIDTH_METERS - AlignmentConstants.GRID_WIDTH_METERS - (PlacementConstants.ROBOT_LENGTH_METERS/2)),
+                      MathUtil.clamp(getPose().getY(),
+                          (0),
+                          AlignmentConstants.FIELD_HEIGHT_METERS),
+                      getPose().getRotation());
+
+      if (getPose().minus(clampedPose).getTranslation().getNorm() != 0) {
+        resetOdometry(clampedPose);
+      }
+  }
     /**
      * Returns the currently-estimated pose of the robot.
      *