Further tests (doesn't spin anymore but maybe right angle??)

chopshop-166 · Feb 20, 2025 · be9c378 · be9c378
1 parent eb1490f
commit be9c378
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 13 deletions.
diff --git a/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java b/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java
@@ -12,6 +12,9 @@
 
 import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
 
 public class VisionMap {
 
@@ -48,6 +51,7 @@ public VisionMap(final List<CameraSource> visionSources) {
      * @param estimator The WPIlib estimator object.
      */
     public <T> void updateData(Data data) {
+        data.targets.clear();
         for (var source : this.visionSources) {
             var results = source.camera.getAllUnreadResults();
             if (!results.isEmpty()) {
@@ -59,13 +63,14 @@ public <T> void updateData(Data data) {
                             est.timestampSeconds);
                 });
                 // Now copy the targets that are found
-                if (latestResult.targets.size() != 0) {
-                    data.targets.clear();
-                }
                 for (var target : latestResult.targets) {
                     int targetID = target.getFiducialId();
                     Logger.recordOutput("target" + targetID, target.bestCameraToTarget);
-                    target.bestCameraToTarget = target.bestCameraToTarget.plus(source.robotToCam);
+                    Transform3d robotToCam = new Transform3d(source.robotToCam.getX(), source.robotToCam.getY(),
+                            source.robotToCam.getZ(),
+                            new Rotation3d(source.robotToCam.getRotation().getX(),
+                                    source.robotToCam.getRotation().getY(), -source.robotToCam.getRotation().getZ()));
+                    target.bestCameraToTarget = target.bestCameraToTarget.plus(robotToCam);
                     Logger.recordOutput("targetOffset" + targetID, target.bestCameraToTarget);
                     if (data.targets.containsKey(target.getFiducialId())) {
                         data.targets.get(targetID).add(target);

diff --git a/src/main/java/frc/robot/maps/Shrimp.java b/src/main/java/frc/robot/maps/Shrimp.java
@@ -126,8 +126,8 @@ public VisionMap getVisionMap() {
                         Units.inchesToMeters(9.75)),
                 new Rotation3d(0, Units.degreesToRadians(-64.752), Units.degreesToRadians(-45)));
 
-        return new VisionMap(new CameraSource("ShrimpCamFL", robotToCamFL),
-                new CameraSource("ShrimpCamFR", robotToCamFR));
+        return new VisionMap(new CameraSource("FLCamera", robotToCamFL),
+                new CameraSource("FRCamera", robotToCamFR));
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -60,6 +60,7 @@ public class Drive extends LoggedSubsystem<SwerveDriveData, SwerveDriveMap> {
     Optional<Translation2d> aimTarget = Optional.empty();
     boolean isAimingAtReef = false;
     Branch targetBranch = Branch.NONE;
+    Pose2d targetPose = new Pose2d();
 
     SwerveDrivePoseEstimator estimator;
 
@@ -213,19 +214,23 @@ private void periodicMove(final double xSpeed, final double ySpeed, final double
         if (aimTarget.isPresent()) {
             rotationSpeed = calculateRotateSpeedToTarget(aimTarget::get);
         }
-        if (targetBranch != Branch.NONE) {
+        if (visionData.targets.size() > 0) {
             vision.filterReefTags(isBlueAlliance, visionData.targets);
             Transform2d reefLocation = vision.pickBestReefLocation(visionData.targets);
             Transform2d robotToBranch = vision
-                    .adjustTranslationForBranch(reefLocation, targetBranch);
+                    .adjustTranslationForBranch(reefLocation, Branch.LEFT_BRANCH);
             Logger.recordOutput("Reef Location Transform2d", estimator.getEstimatedPosition().plus(reefLocation));
-            Logger.recordOutput("Robot To Branch Transform2d", estimator.getEstimatedPosition().plus(robotToBranch));
+            Logger.recordOutput("Robot To Branch Transform2d",
+                    estimator.getEstimatedPosition().plus(robotToBranch));
+            targetPose = estimator.getEstimatedPosition().plus(robotToBranch);
+        }
+        if (targetBranch != Branch.NONE) {
             Pose2d robotPose = estimator.getEstimatedPosition();
-            translateXSpeed = translationPID_X.calculate(robotPose.getX(), robotPose.getX() + robotToBranch.getX());
-            translateYSpeed = translationPID_Y.calculate(robotPose.getY(), robotPose.getY() + robotToBranch.getY());
+            translateXSpeed = translationPID_X.calculate(robotPose.getX(), targetPose.getX());
+            translateYSpeed = translationPID_Y.calculate(robotPose.getY(), targetPose.getY());
             rotationSpeed = rotationPID.calculate(robotPose.getRotation().getDegrees(),
-                    robotPose.getRotation().getDegrees() - robotToBranch.getRotation().getDegrees());
-            Logger.recordOutput("Robot To Branch Angle", robotToBranch.getRotation().getDegrees());
+                    targetPose.getRotation().getDegrees());
+            Logger.recordOutput("Rotation PID Error", rotationPID.getPositionError());
         }
 
         move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);