Log the errors in our scoring position so that they can be calibrated…

… on the field
FRC3476 · Apr 18, 2023 · 05c2804 · 05c2804
1 parent 67a0382
commit 05c2804
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -14,12 +14,9 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj.IterativeRobotBase;
-import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.Watchdog;
 import frc.robot.ScoringPositionManager.PositionType;
 import frc.robot.ScoringPositionManager.SelectedPosition;
 import frc.subsytem.AbstractSubsystem;
@@ -354,6 +351,20 @@ public void robotInit() {
      */
     @Override
     public void robotPeriodic() {
+        if (DriverStation.isDisabled()) {
+            // Update the scoring position here when we're disabled so that we can change it when the robot is disabled
+            buttonPanel.update();
+            ScoringPositionManager.getInstance().updateSelectedPosition(buttonPanel);
+        }
+
+        if (xbox.getRisingEdge(XBOX_RESET_HEADING)) {
+            if (isRed()) {
+                robotTracker.resetPose(new Pose2d(robotTracker.getLatestPose().getTranslation(), Rotation2d.fromDegrees(0)));
+            } else {
+                robotTracker.resetPose(new Pose2d(robotTracker.getLatestPose().getTranslation(), Rotation2d.fromDegrees(180)));
+            }
+        }
+
         runAsyncScheduledTasks();
         AbstractSubsystem.tick();
 
@@ -642,14 +653,6 @@ && dist2(autoDriveAlignError) <= SCORE_POSITION_ERROR_SQUARED
             }
         }
 
-        if (xbox.getRisingEdge(XBOX_RESET_HEADING)) {
-            if (isRed()) {
-                robotTracker.resetPose(new Pose2d(robotTracker.getLatestPose().getTranslation(), Rotation2d.fromDegrees(0)));
-            } else {
-                robotTracker.resetPose(new Pose2d(robotTracker.getLatestPose().getTranslation(), Rotation2d.fromDegrees(180)));
-            }
-        }
-
         if (grabber.isGrabbed() && wantedMechanismState.shouldAutoGrab) {
             setStowed();
         }

diff --git a/src/main/java/frc/robot/ScoringPositionManager.java b/src/main/java/frc/robot/ScoringPositionManager.java
@@ -5,9 +5,11 @@
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.littletonrobotics.junction.networktables.LoggedDashboardString;
 
 import java.util.HashMap;
+import java.util.List;
 import java.util.Map;
 
 import static frc.robot.Constants.IS_PRACTICE;
@@ -105,6 +107,12 @@ public ScoringDirection getScoringDirection() {
             new LoggedDashboardBoolean("Does Wanted Position Type Match Selected Position Type", true);
 
 
+    private final List<LoggedDashboardNumber> yPositionErrors = List.of(
+            new LoggedDashboardNumber("Y Position Error Grid 0", 0),
+            new LoggedDashboardNumber("Y Position Error Grid 1", 0),
+            new LoggedDashboardNumber("Y Position Error Grid 2", 0)
+    );
+
     public PositionType getWantedPositionType() {
         return wantedPositionType;
     }
@@ -145,6 +153,15 @@ public boolean updateSelectedPosition(Controller buttonPanel) {
             selectedPositions[selectedPosition.ordinal()].set(true);
             doesWantedPositionTypeMatchSelectedPositionType.set(doesWantedPositionTypeMatchSelectedPositionType());
         }
+
+        double robotY = Robot.getRobotTracker().getLatestPose3d().getY();
+
+        double[] possibleFieldYs = getPossibleFieldYs(selectedPosition, Robot.isRed());
+        for (int i = 0; i < 3; i++) {
+            yPositionErrors.get(i).set(robotY - possibleFieldYs[i]);
+        }
+
+
         return oldSelectedPosition != selectedPosition;
     }
 
@@ -334,7 +351,12 @@ enum ScoringDirection {
 
 
         for (int i = 0; i < 3; i++) {
-            possibleYs[i] = y + CUBE_SCORING_Y_CENTER[i];
+            possibleYs[i] = CUBE_SCORING_Y_CENTER[i] +
+                    y +
+                    yScoringOffsets
+                            .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
+                            .get(i)
+                            .get(selectedPosition.getScoringDirection());
         }
         return possibleYs;
     }
@@ -379,12 +401,12 @@ public static BestFieldY getBestFieldY(@NotNull SelectedPosition selectedPositio
             }
         }
 
-        bestY = bestY
-                + getGridRelativeY(selectedPosition, isRedAlliance)
-                + yScoringOffsets
-                .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
-                .get(chosenGridIndex)
-                .get(selectedPosition.getScoringDirection());
+        bestY = bestY +
+                getGridRelativeY(selectedPosition, isRedAlliance) +
+                yScoringOffsets
+                        .get(isRedAlliance ? AllianceSide.RED : AllianceSide.BLUE)
+                        .get(chosenGridIndex)
+                        .get(selectedPosition.getScoringDirection());
 
 
         return new BestFieldY(bestY, chosenGridIndex);