[#64] More Pos logging

frc-862 · Jan 26, 2024 · 20e9b3b · 20e9b3b
1 parent 0b831e4
commit 20e9b3b
Show file tree

Hide file tree

Showing 11 changed files with 247 additions and 19 deletions.
diff --git a/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto b/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "test"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1MeterSquareLL"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Test Autos",
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/autos/SM-3.auto b/src/main/deploy/pathplanner/autos/SM-3.auto
@@ -8,7 +8,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "SM-3"
+            "pathName": "Sipnslow"
           }
         },
         {

diff --git a/src/main/deploy/pathplanner/autos/Spin.auto b/src/main/deploy/pathplanner/autos/Spin.auto
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Spin"
+            "pathName": "Sipnslow"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/autos/Through_Pieces.auto b/src/main/deploy/pathplanner/autos/Through_Pieces.auto
@@ -11,11 +11,35 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "disable-Vision"
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "Run_Through_Pieces"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "test"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "StraightOut"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "test"
+          }
         }
       ]
     }

diff --git a/src/main/deploy/pathplanner/paths/1MeterSquareLL.path b/src/main/deploy/pathplanner/paths/1MeterSquareLL.path
@@ -0,0 +1,97 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.32,
+        "y": 5.525487974052871
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.82,
+        "y": 5.525487974052871
+      },
+      "isLocked": false,
+      "linkedName": "SubwooferFront"
+    },
+    {
+      "anchor": {
+        "x": 2.32,
+        "y": 5.53
+      },
+      "prevControl": {
+        "x": 1.8199999999999998,
+        "y": 5.53
+      },
+      "nextControl": {
+        "x": 2.38,
+        "y": 5.53
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.32,
+        "y": 6.53
+      },
+      "prevControl": {
+        "x": 2.32,
+        "y": 6.03
+      },
+      "nextControl": {
+        "x": 2.32,
+        "y": 6.59
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.32,
+        "y": 6.53
+      },
+      "prevControl": {
+        "x": 1.82,
+        "y": 6.53
+      },
+      "nextControl": {
+        "x": 1.26,
+        "y": 6.53
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.32,
+        "y": 5.525487974052871
+      },
+      "prevControl": {
+        "x": 1.32,
+        "y": 6.025487974052871
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "SubwooferFront"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Test paths",
+  "previewStartingState": null,
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path b/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path
@@ -64,16 +64,16 @@
     },
     {
       "anchor": {
-        "x": 0.7319122330552369,
-        "y": 4.459858150485532
+        "x": 1.32,
+        "y": 5.525487974052871
       },
       "prevControl": {
-        "x": 2.616498309919698,
-        "y": 4.09478126648561
+        "x": 3.204586076864461,
+        "y": 5.160411090052949
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "SubwooferFront"
     }
   ],
   "rotationTargets": [
@@ -103,7 +103,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 118.35496178532406,
+    "rotation": 180.0,
     "rotateFast": false
   },
   "reversed": false,

diff --git a/src/main/deploy/pathplanner/paths/Spin.path → ...in/deploy/pathplanner/paths/Sipnslow.path b/src/main/deploy/pathplanner/paths/Spin.path → ...in/deploy/pathplanner/paths/Sipnslow.path
diff --git a/src/main/deploy/pathplanner/paths/StraightOut.path b/src/main/deploy/pathplanner/paths/StraightOut.path
@@ -0,0 +1,65 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.32,
+        "y": 5.525487974052871
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.4380339887498943,
+        "y": 5.525487974052871
+      },
+      "isLocked": false,
+      "linkedName": "SubwooferFront"
+    },
+    {
+      "anchor": {
+        "x": 2.8631718801899155,
+        "y": 5.525487974052871
+      },
+      "prevControl": {
+        "x": 1.4489583178168204,
+        "y": 5.525487974052871
+      },
+      "nextControl": {
+        "x": 4.27738544256301,
+        "y": 5.525487974052871
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.458177839432603,
+        "y": 5.525487974052871
+      },
+      "prevControl": {
+        "x": 3.937487206858049,
+        "y": 5.525487974052871
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -52,7 +52,7 @@ public class RobotContainer extends LightningContainer {
 	// Shooter shooter;
 	// Collision collision;
 	// Indexer indexer;
-	Climber climber;
+	// Climber climber;
 
 	private SendableChooser<Command> autoChooser;
 	// TODO I want field-centric driving in open loop WE NEED TO FIGURE OUT WHAT
@@ -79,7 +79,7 @@ protected void initializeSubsystems() {
 		// pivot = new Pivot();
 		// shooter = new Shooter(pivot, flywheel, indexer);
 		// collision = new Collision(drivetrain);
-		climber = new Climber();
+		// climber = new Climber();
 
 		drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop
 		slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop
@@ -93,6 +93,7 @@ protected void initializeSubsystems() {
 	@Override
 	protected void initializeNamedCommands() {
 		NamedCommands.registerCommand("test", new InstantCommand(() -> System.out.println("Hello World!")));
+		NamedCommands.registerCommand("disable-Vision", new InstantCommand(() -> drivetrain.disableVision()));
 	}
 
 	@Override
@@ -109,9 +110,9 @@ protected void configureButtonBindings() {
 		new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false)));
 		new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false));
 		new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain));
-		new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false));
 
-		new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED));
+		new Trigger(driver::getYButton).onTrue(new InstantCommand(() -> drivetrain.disableVision()).alongWith(new InstantCommand(() -> System.out.println("Vision Disabled"))));
+		// new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED));
 	}
 
 	@Override
@@ -124,7 +125,7 @@ protected void configureDefaultCommands() {
 						.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_MULT) // Drive counterclockwise with negative X (left)
 				));
 		// climber.setDefaultCommand(new ManualClimb(() -> (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), climber));
-		climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED));
+		// climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED));
 
 		// shooter.setDefaultCommand(new Shoot(shooter, indexer, drivetrain, () -> coPilot.getAButton()));
 

diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -33,6 +33,7 @@ public class Swerve extends SwerveDrivetrain implements Subsystem {
     private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds();
     private Limelight[] limelights;
     private boolean slowMode = false;
+    private boolean disableVision = false;
 
     public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency,
             SwerveModuleConstants... modules) {
@@ -66,9 +67,16 @@ public void simulationPeriodic() {
     public void periodic() {
         for (Limelight limelight : Limelight.filterLimelights(limelights)) {
             Pose4d pose = limelight.getAlliancePose();
-            addVisionMeasurement(pose.toPose2d(),
-                    Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency())
-                            - VisionConstants.PROCESS_LATENCY);
+            // LightningShuffleboard.set("Swerve", "Vision pose", pose.toPose2d());
+            LightningShuffleboard.setDouble("Swerve", "Vision pose X", pose.toPose2d().getX());
+            LightningShuffleboard.setDouble("Swerve", "Vision pose Y", pose.toPose2d().getY());
+            LightningShuffleboard.setDouble("Swerve", "Vision time offset", Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency())
+            - VisionConstants.PROCESS_LATENCY);
+            if(!disableVision) {
+                addVisionMeasurement(pose.toPose2d(),
+                        Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency())
+                                - VisionConstants.PROCESS_LATENCY);
+            }
         }
 
         LightningShuffleboard.setDouble("Swerve", "yaw", m_yawGetter.getValueAsDouble());
@@ -98,6 +106,14 @@ AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(),
                 }, this); // Subsystem for requirements
     }
 
+    public void disableVision() {
+        disableVision = true;
+    }
+
+    public void enableVision() {
+        disableVision = false;
+    }
+
     public Supplier<Pose2d> getPose() {
         return () -> getState().Pose;
     }

diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.4",
+    "version": "2024.1.5",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.4"
+            "version": "2024.1.5"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.4",
+            "version": "2024.1.5",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,