Merge pull request #63 from WHS-FRC-3467/new-claw

New claw
WHS-FRC-3467 · Feb 22, 2025 · 725b444 · 725b444
2 parents 808ab8b + 3882684
commit 725b444
Show file tree

Hide file tree

Showing 9 changed files with 39 additions and 36 deletions.
diff --git a/ascope_assets/Robot_Alpha/config.json b/ascope_assets/Robot_Alpha/config.json
@@ -26,14 +26,18 @@
         1304
       ],
       "position": [
-        0.293688,
-        0.346515,
-        0.219056
+        0.27864816,
+        0.23491698,
+        0.20108418
       ],
       "rotations": [
         {
           "axis": "y",
           "degrees": -15
+        },
+        {
+          "axis": "z",
+          "degrees": 30
         }
       ]
     },
@@ -45,14 +49,18 @@
         1304
       ],
       "position": [
-        0.293688,
-        -0.346515,
-        0.219056
+        0.27864816,
+        0.23491698,
+        0.20108418
       ],
       "rotations": [
         {
           "axis": "y",
           "degrees": -15
+        },
+        {
+          "axis": "z",
+          "degrees": 30
         }
       ]
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -332,7 +332,8 @@ private void configureControllerBindings()
         m_driver
             .y().and(isCoralMode)
             .onTrue(
-                m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4));
+                m_superStruct.getTransitionCommand(Arm.State.LEVEL_4, Elevator.State.LEVEL_4, 0.0,
+                    0.8));
 
         // Driver Y Button held and Right Bumper having been pressed to ALGAE mode: Send Arm and
         // Elevator to NET

diff --git a/src/main/java/frc/robot/subsystems/Arm/Arm.java b/src/main/java/frc/robot/subsystems/Arm/Arm.java
@@ -27,10 +27,10 @@ public class Arm extends GenericMotionProfiledSubsystem<Arm.State> {
     @Getter
     public enum State implements TargetState {
         // HOMING(0.0, 0.0, ProfileType.MM_POSITION),
-        STOW(() -> Units.degreesToRotations(124.0), ProfileType.MM_POSITION),
+        STOW(() -> Units.degreesToRotations(127.18), ProfileType.MM_POSITION),
         // CORAL_INTAKE(() -> 0.42, ProfileType.MM_POSITION),
-        CORAL_INTAKE(() -> Units.degreesToRotations(140.8), ProfileType.MM_POSITION),
-        LEVEL_1(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION),
+        CORAL_INTAKE(() -> Units.degreesToRotations(141.6), ProfileType.MM_POSITION),
+        LEVEL_1(() -> Units.degreesToRotations(127.0), ProfileType.MM_POSITION),
         LEVEL_2(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION),
         LEVEL_3(() -> Units.degreesToRotations(120.0), ProfileType.MM_POSITION),
         LEVEL_4(() -> Units.degreesToRotations(100.0), ProfileType.MM_POSITION),

diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRoller.java
@@ -25,13 +25,13 @@ public class ClawRoller
     @RequiredArgsConstructor
     @Getter
     public enum State implements TargetState {
-        OFF(() -> 0.0, ProfileType.OPEN_VOLTAGE), // TODO: tune on real robot
+        OFF(() -> 0.0, ProfileType.OPEN_VOLTAGE),
         INTAKE(() -> 2.0, ProfileType.OPEN_VOLTAGE),
-        INTAKESLOW(() -> 1, ProfileType.OPEN_VOLTAGE),
-        SHUFFLE(() -> -0.5, ProfileType.OPEN_VOLTAGE),
-        EJECT(() -> 6.0, ProfileType.OPEN_VOLTAGE),
+        INTAKESLOW(() -> 1.5, ProfileType.OPEN_VOLTAGE),
+        SHUFFLE(() -> holdCoralSP.get(), ProfileType.OPEN_VOLTAGE),
+        EJECT(() -> 10.0, ProfileType.OPEN_VOLTAGE),
         SCORE(() -> 8.0, ProfileType.OPEN_VOLTAGE),
-        HOLDCORAL(() -> holdCoralSP.getAsDouble(), ProfileType.MM_POSITION),
+        HOLDCORAL(() -> 0.5, ProfileType.MM_POSITION),
         ALGAE_INTAKE(() -> algaeIntakeSP.getAsDouble(), ProfileType.OPEN_CURRENT);
 
         private final DoubleSupplier output;

diff --git a/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java b/src/main/java/frc/robot/subsystems/Claw/ClawRoller/ClawRollerConstants.java
@@ -41,7 +41,7 @@ public final class ClawRollerConstants {
         kSubSysConstants.kMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true;
 
         /* REAL system profile constants */
-        kSubSysConstants.kMotorConfig.Slot0.kP = 50; // TODO: tune on real robot
+        kSubSysConstants.kMotorConfig.Slot0.kP = 100; // TODO: tune on real robot
         kSubSysConstants.kMotorConfig.Slot0.kI = 0;
         kSubSysConstants.kMotorConfig.Slot0.kD = 0;
         kSubSysConstants.kMotorConfig.Slot0.kG = 0;

diff --git a/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java b/src/main/java/frc/robot/subsystems/Claw/IntakeLaserCAN/IntakeLaserCAN.java
@@ -16,7 +16,7 @@ public class IntakeLaserCAN extends GenericLaserCANSubsystem<IntakeLaserCAN.Stat
     @RequiredArgsConstructor
     @Getter
     public enum State implements DistanceState {
-        DEFAULT(Distance.ofBaseUnits(0.15, Meter));
+        DEFAULT(Distance.ofBaseUnits(0.08, Meter));
 
         @Getter
         private final Distance distance;

diff --git a/src/main/java/frc/robot/subsystems/Elevator/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator/Elevator.java
@@ -35,7 +35,7 @@ public enum State implements TargetState {
         // TODO: Test Voltage and position values (rotations)
         STOW(() -> 0, ProfileType.MM_POSITION),
         CORAL_INTAKE(() -> 0, ProfileType.MM_POSITION),
-        LEVEL_1(() -> 0.2, ProfileType.MM_POSITION),
+        LEVEL_1(() -> 0.913, ProfileType.MM_POSITION),
         LEVEL_2(() -> 1.2, ProfileType.MM_POSITION),
         LEVEL_3(() -> 2.7, ProfileType.MM_POSITION),
         LEVEL_4(() -> 5, ProfileType.MM_POSITION),

diff --git a/src/main/java/frc/robot/subsystems/Vision/Vision.java b/src/main/java/frc/robot/subsystems/Vision/Vision.java
@@ -96,7 +96,7 @@ public void periodic()
             // Add tag poses
             for (int tagId : inputs[cameraIndex].tagIds) {
                 var tagPose = aprilTagLayout.getTagPose(tagId);
-                if (tagPose.isPresent()) {
+                if (tagPose.isPresent() && !rejectedTags.contains(tagId)) {
                     tagPoses.add(tagPose.get());
                     seesThisTarget = true;
                 }

diff --git a/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/Vision/VisionConstants.java
@@ -13,29 +13,19 @@
 
 package frc.robot.subsystems.Vision;
 
-import java.io.File;
-import java.io.IOException;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Filesystem;
 
 public class VisionConstants {
     // AprilTag layout
-    public static AprilTagFieldLayout aprilTagLayout;
-
-    static {
-        try {
-            aprilTagLayout = new AprilTagFieldLayout(
-                new File(Filesystem.getDeployDirectory(), "vision/andymark.json").toPath());
-        } catch (IOException exception) {
-            aprilTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
-        }
-    }
+    public static AprilTagFieldLayout aprilTagLayout =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
 
     // Camera names, must match names configured on coprocessor
     public static String camera0Name = "front_left";
@@ -44,11 +34,13 @@ public class VisionConstants {
     // Robot to camera transforms
     // (Not used by Limelight, configure in web UI instead)
     public static Transform3d robotToCamera0 =
-        new Transform3d(0.293688, 0.346515, 0.219053,
-            new Rotation3d(0.0, Units.degreesToRadians(-15), 0.0));
+        new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(10.9704),
+            Units.inchesToMeters(7.9167),
+            new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(-30)));
     public static Transform3d robotToCamera1 =
-        new Transform3d(0.293688, -0.346515, 0.219053,
-            new Rotation3d(0.0, Units.degreesToRadians(-15), 0.0));
+        new Transform3d(Units.inchesToMeters(9.287), Units.inchesToMeters(-10.9704),
+            Units.inchesToMeters(7.9167),
+            new Rotation3d(0.0, Units.degreesToRadians(-15), Units.degreesToRadians(30)));
 
     // Basic filtering thresholds
     public static double maxAmbiguity = 0.3;
@@ -66,4 +58,6 @@ public class VisionConstants {
                 1.0, // Camera 0
                 1.0 // Camera 1
         };
+
+    public static List<Integer> rejectedTags = Arrays.asList(3, 4, 5, 14, 15, 16);
 }