tested pose est with the drive command but no worky

HighlanderRobotics · Nov 11, 2023 · 18a3961 · 18a3961
1 parent 66162a2
commit 18a3961
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 141 deletions.
diff --git a/src/main/deploy/pathplanner/pose test Blue.path b/src/main/deploy/pathplanner/pose test Blue.path
@@ -2,13 +2,13 @@
   "waypoints": [
     {
       "anchorPoint": {
-        "x": 2.3,
-        "y": 1.8
+        "x": 3.3,
+        "y": 1.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.3000000000000003,
-        "y": 1.8
+        "x": 4.300000000000001,
+        "y": 1.0
       },
       "holonomicAngle": 180.0,
       "isReversal": false,
@@ -24,140 +24,15 @@
     },
     {
       "anchorPoint": {
-        "x": 4.737025208647575,
-        "y": 1.8
-      },
-      "prevControl": {
-        "x": 3.737025208647575,
-        "y": 1.8
-      },
-      "nextControl": {
-        "x": 3.737025208647575,
-        "y": 1.8
-      },
-      "holonomicAngle": 0,
-      "isReversal": true,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 2.5,
-        "y": 1.8
+        "x": 3.3,
+        "y": 1.0
       },
       "prevControl": {
-        "x": 2.014223686941068,
-        "y": 1.8
-      },
-      "nextControl": {
-        "x": 2.985776313058932,
-        "y": 1.8
-      },
-      "holonomicAngle": 180.0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 4.5,
-        "y": 1.8046074006839954
-      },
-      "prevControl": {
-        "x": 4.27385240613827,
-        "y": 1.8046074006839954
-      },
-      "nextControl": {
-        "x": 4.72614759386173,
-        "y": 1.8046074006839954
-      },
-      "holonomicAngle": 180.0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 2.5,
-        "y": 1.8
-      },
-      "prevControl": {
-        "x": 2.2428113772359337,
-        "y": 1.8
-      },
-      "nextControl": {
-        "x": 2.7571886227640663,
-        "y": 1.8
-      },
-      "holonomicAngle": 0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
-        "x": 4.5,
-        "y": 1.8
-      },
-      "prevControl": {
-        "x": 4.380109782805397,
-        "y": 1.8
-      },
-      "nextControl": {
-        "x": 4.619890217194603,
-        "y": 1.8
-      },
-      "holonomicAngle": 0.0,
-      "isReversal": false,
-      "velOverride": null,
-      "isLocked": false,
-      "isStopPoint": false,
-      "stopEvent": {
-        "names": [],
-        "executionBehavior": "parallel",
-        "waitBehavior": "none",
-        "waitTime": 0
-      }
-    },
-    {
-      "anchorPoint": {
         "x": 2.3,
-        "y": 1.8
-      },
-      "prevControl": {
-        "x": 1.2999999999999998,
-        "y": 1.8
+        "y": 1.0
       },
       "nextControl": null,
-      "holonomicAngle": 0,
+      "holonomicAngle": 180.0,
       "isReversal": false,
       "velOverride": null,
       "isLocked": false,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -178,7 +178,13 @@ public RobotContainer() {
 
     autoSystemType.addDefaultOption("Choreo", AutoSystem.choreo);
     autoSystemType.addOption("Path Planner", AutoSystem.pathplanner);
+
+    SmartDashboard.putData(
+        "pose test", swerveSubsystem.poseLockDriveCommand(
+            () -> 3.3, () -> 1, () -> 180, true, false));
+
   }
+
 
   /**
    * Use this method to define your trigger->command mappings. Triggers can be created via the

diff --git a/src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/Swerve/SwerveSubsystem.java
@@ -63,7 +63,6 @@
 import java.util.NoSuchElementException;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
-import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 
@@ -255,7 +254,7 @@ public Command headingLockDriveCommand(
         false);
   }
 
-  public Command poseLockDriveCommand(
+  public CommandBase poseLockDriveCommand(
       DoubleSupplier x,
       DoubleSupplier y,
       DoubleSupplier theta,
@@ -274,17 +273,17 @@ public Command poseLockDriveCommand(
                         deadband(
                             Constants.AutoConstants.xController.calculate(
                                 pose.getX(), x.getAsDouble()),
-                            0.05),
+                            0),
                     () ->
                         deadband(
                             Constants.AutoConstants.yController.calculate(
                                 pose.getY(), y.getAsDouble()),
-                            0.05),
+                            0),
                     () ->
                         deadband(
                             headingController.calculate(
                                 pose.getRotation().getRadians() % (2 * Math.PI)),
-                            0.05),
+                            0),
                     fieldRelative,
                     isOpenLoop,
                     false)
@@ -513,9 +512,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
 
   /** Return the pose of the drivebase, as estimated by the pose estimator. */
   public Pose2d getPose() {
-    return wheelOnlyOdo.getPoseMeters();
+    // return wheelOnlyOdo.getPoseMeters();
     // Use this if we want to use vision
-    // return poseEstimator.getEstimatedPosition();
+    return poseEstimator.getEstimatedPosition();
   }
 
   /** Resets the pose estimator to the given pose */
@@ -661,7 +660,7 @@ public void periodic() {
               .estimatedPose;
       Logger.getInstance().recordOutput("Vision Pose", visionMeasurement);
         poseEstimator.addVisionMeasurement(visionMeasurement.toPose2d(), visionIOInputs.timestamp, VisionHelper.findVisionMeasurements(estPose));
-        resetOdometry(visionMeasurement.toPose2d());
+        //resetOdometry(visionMeasurement.toPose2d());
     } catch (NoSuchElementException e) {}
 
     double[] apriltagX = new double[visionIOInputs.targets.size() * 4];