Merge pull request #50 from WHS-FRC-3467/main

Update Sensor-Fallback-Impl with main changes
WHS-FRC-3467 · Feb 15, 2025 · 7928bcc · 7928bcc
2 parents 7d5f6cb + 331c327
commit 7928bcc
Show file tree

Hide file tree

Showing 31 changed files with 957 additions and 887 deletions.
diff --git a/ascope_assets/Robot_Alpha/config.json b/ascope_assets/Robot_Alpha/config.json
@@ -26,9 +26,9 @@
         1304
       ],
       "position": [
-        0.287,
-        0.260,
-        0.211
+        0.289591,
+        0.261649,
+        0.205829
       ],
       "rotations": [
         {
@@ -45,9 +45,9 @@
         1304
       ],
       "position": [
-        0.287,
-        -0.260,
-        0.211
+        0.289591,
+        -0.261649,
+        0.205829
       ],
       "rotations": [
         {
@@ -114,4 +114,4 @@
       ]
     }
   ]
-}
+}
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
     id "com.peterabeles.gversion" version "1.10"
     id "com.diffplug.spotless" version "6.12.0"
     id "io.freefair.lombok" version "8.6"

diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java
@@ -21,6 +21,8 @@
 public class FieldConstants {
     public static final double fieldLength = Units.inchesToMeters(690.876);
     public static final double fieldWidth = Units.inchesToMeters(317);
+    public static final Translation2d fieldCenter =
+        new Translation2d(fieldLength / 2, fieldWidth / 2);
     public static final double startingLineX =
         Units.inchesToMeters(299.438); // Measured from the inside of starting
     // line
@@ -57,14 +59,14 @@ public static class CoralStation {
     }
 
     public static class Reef {
-        public static final Translation2d center =
+        public static final Translation2d centerOfReef =
             new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501));
         public static final double faceToZoneLine =
             Units.inchesToMeters(12); // Side of the reef to the inside of the
         // reef zone line
 
         public static final Pose2d[] centerFaces =
-            new Pose2d[6]; // Starting facing the driver station in clockwise
+            new Pose2d[12]; // Starting facing the driver station in clockwise
         // order
         public static final List<Map<ReefHeight, Pose3d>> branchPositions =
             new ArrayList<>(); // Starting at the right
@@ -105,15 +107,31 @@ public static class Reef {
                     Units.inchesToMeters(130.144),
                     Rotation2d.fromDegrees(-120));
 
+            centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg);
+            centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg);
+
             // Initialize branch positions
-            for (int face = 0; face < 6; face++) {
+            for (int face = 0; face < centerFaces.length; face++) {
                 Map<ReefHeight, Pose3d> fillRight = new HashMap<>();
                 Map<ReefHeight, Pose3d> fillLeft = new HashMap<>();
                 for (var level : ReefHeight.values()) {
-                    Pose2d poseDirection =
-                        new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face)));
-                    double adjustX = Units.inchesToMeters(30.738);
-                    double adjustY = Units.inchesToMeters(6.469);
+                    Pose2d poseDirection = new Pose2d();
+                    if (face < 6) {
+                        poseDirection =
+                            new Pose2d(centerOfReef, centerFaces[face].getRotation());
+                    } else {
+                        poseDirection =
+                            new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg),
+                                centerFaces[face].getRotation());
+                    }
+
+                    double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face
+                    double adjustY = Units.inchesToMeters(6.469); // Offset from reef face
+                                                                  // centerline to branch
 
                     fillRight.put(
                         level,
@@ -153,6 +171,8 @@ public static class Reef {
                 branchPositions.add(fillRight);
                 branchPositions.add(fillLeft);
             }
+
+
         }
     }
 
@@ -202,15 +222,20 @@ public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side)
 
     public static Pose2d getNearestCoralStation(Pose2d currentPose)
     {
-        double distanceToLeftStation = currentPose.getTranslation()
-            .getDistance(FieldConstants.CoralStation.leftCenterFace.getTranslation());
-        double distanceToRightStation = currentPose.getTranslation()
-            .getDistance(FieldConstants.CoralStation.rightCenterFace.getTranslation());
-
-        if (distanceToLeftStation > distanceToRightStation) {
-            return FieldConstants.CoralStation.rightCenterFace;
+        if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) {
+            if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) {
+                return FieldConstants.CoralStation.rightCenterFace
+                    .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg);
+            } else {
+                return FieldConstants.CoralStation.leftCenterFace
+                    .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg);
+            }
         } else {
-            return FieldConstants.CoralStation.leftCenterFace;
+            if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) {
+                return FieldConstants.CoralStation.leftCenterFace;
+            } else {
+                return FieldConstants.CoralStation.rightCenterFace;
+            }
         }
     }
 }
diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java
@@ -13,6 +13,7 @@ public class Ports {
     public static final CanDeviceId CLAW_LASERCAN = new CanDeviceId(16, "rio");
     public static final CanDeviceId CLAW_ROLLER = new CanDeviceId(17, "rio");
 
+
     public static final CanDeviceId ARM_CANCODER = new CanDeviceId(18, "rio");
     public static final CanDeviceId ARM_MAIN = new CanDeviceId(19, "rio");
 
@@ -23,6 +24,8 @@ public class Ports {
     public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(22, "rio"); // Top Kraken
     public static final CanDeviceId ELEVATOR_FOLLOWER = new CanDeviceId(23, "rio"); // Bottom Kraken
 
-    public static final CanDeviceId CLIMBER = new CanDeviceId(24, "rio");
+    public static final CanDeviceId CLIMBER = new CanDeviceId(24, "Drivetrain");
+
+    public static final CanDeviceId PDH = new CanDeviceId(25, "rio");
 
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -148,47 +148,11 @@ public void disabledInit()
     public void disabledPeriodic()
     {
         // Test
+        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
         var alliance = DriverStation.getAlliance();
-        Pose2d test;
         // Test
         // Get currently selected command
-        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-        // Check if is the same as the last one
-        if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) {
-            // Check if its contained in the list of our autos
-            if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) {
-                // Clear the current path
-                m_pathsToShow.clear();
-                // Grabs all paths from the auto
-                try {
-                    for (PathPlannerPath path : PathPlannerAuto
-                        .getPathGroupFromAutoFile(m_autonomousCommand.getName())) {
-                        // Adds all poses to master list
-                        m_pathsToShow.addAll(path.getPathPoses());
-                    }
-                } catch (IOException e) {
-                    // TODO Auto-generated catch block
-                    e.printStackTrace();
-                } catch (ParseException e) {
-                    // TODO Auto-generated catch block
-                    e.printStackTrace();
-                }
-                // Displays all poses on Field2d widget depending on Alliance Side
-                if (alliance.isPresent() && alliance.get() == Alliance.Red) {
-                    m_autoTraj.getObject("traj").setPoses(m_pathsToShow);
-                } else {
-                    for (int i = 0; i < m_pathsToShow.size(); i++) {
-                        test = m_pathsToShow.get(0).transformBy(
-                            new Transform2d());
-                        Logger.recordOutput("m_pathsToShow" + i, m_pathsToShow.get(i));
-                        Logger.recordOutput("NEW_m_pathsToShow" + i, test);
-                    }
-                }
-                //
-                m_autoTraj.setRobotPose(m_pathsToShow.get(0));
-            }
-        }
-        m_lastAutonomousCommand = m_autonomousCommand;
+
 
     }