Fix commands for differential drivetrains

pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java

@@ -82,6 +82,9 @@ public void initialize() {
   public void execute() {
     double currentTime = timer.get();
     PathPlannerTrajectory.State targetState = generatedTrajectory.sample(currentTime);
+    if (!controller.isHolonomic() && path.isReversed()) {
+      targetState = targetState.reverse();
+    }
 
     Pose2d currentPose = poseSupplier.get();
     ChassisSpeeds currentSpeeds = speedsSupplier.get();
@@ -105,9 +108,16 @@ public void execute() {
         Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
 
     PPLibTelemetry.setCurrentPose(currentPose);
-    PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose());
     PathPlannerLogging.logCurrentPose(currentPose);
-    PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose());
+
+    if (controller.isHolonomic()) {
+      PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose());
+      PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose());
+    } else {
+      PPLibTelemetry.setTargetPose(targetState.getDifferentialPose());
+      PathPlannerLogging.logTargetPose(targetState.getDifferentialPose());
+    }
+
     PPLibTelemetry.setVelocities(
         currentVel,
         targetState.velocityMps,

pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java

@@ -200,9 +200,16 @@ public void execute() {
           Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
 
       PPLibTelemetry.setCurrentPose(currentPose);
-      PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose());
       PathPlannerLogging.logCurrentPose(currentPose);
-      PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose());
+
+      if (controller.isHolonomic()) {
+        PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose());
+        PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose());
+      } else {
+        PPLibTelemetry.setTargetPose(targetState.getDifferentialPose());
+        PathPlannerLogging.logTargetPose(targetState.getDifferentialPose());
+      }
+
       PPLibTelemetry.setVelocities(
           currentVel,
           targetState.velocityMps,

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java

@@ -154,4 +154,15 @@ public ChassisSpeeds calculateRobotRelativeSpeeds(
   public double getPositionalError() {
     return translationError.getNorm();
   }
+
+  /**
+   * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+   * in the path following command.
+   *
+   * @return True if this controller is for a holonomic drive train
+   */
+  @Override
+  public boolean isHolonomic() {
+    return true;
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java

@@ -109,4 +109,15 @@ public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
   public double getPositionalError() {
     return lastError;
   }
+
+  /**
+   * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+   * in the path following command.
+   *
+   * @return True if this controller is for a holonomic drive train
+   */
+  @Override
+  public boolean isHolonomic() {
+    return false;
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java

@@ -68,4 +68,15 @@ public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
   public double getPositionalError() {
     return lastError;
   }
+
+  /**
+   * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+   * in the path following command.
+   *
+   * @return True if this controller is for a holonomic drive train
+   */
+  @Override
+  public boolean isHolonomic() {
+    return false;
+  }
 }

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java

@@ -30,4 +30,12 @@ ChassisSpeeds calculateRobotRelativeSpeeds(
    * @return Positional error, in meters
    */
   double getPositionalError();
+
+  /**
+   * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+   * in the path following command.
+   *
+   * @return True if this controller is for a holonomic drive train
+   */
+  boolean isHolonomic();
 }

pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp

@@ -40,6 +40,9 @@ void FollowPathCommand::Execute() {
 	units::second_t currentTime = m_timer.Get();
 	PathPlannerTrajectory::State targetState = m_generatedTrajectory.sample(
 			currentTime);
+	if (m_controller->isHolonomic()) {
+		targetState = targetState.reverse();
+	}
 
 	frc::Pose2d currentPose = m_poseSupplier();
 	frc::ChassisSpeeds currentSpeeds = m_speedsSupplier();
@@ -68,11 +71,18 @@ void FollowPathCommand::Execute() {
 					targetState);
 
 	PPLibTelemetry::setCurrentPose(currentPose);
-	PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose());
+	PathPlannerLogging::logCurrentPose(currentPose);
+
+	if (m_controller->isHolonomic()) {
+		PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose());
+		PathPlannerLogging::logTargetPose(targetState.getTargetHolonomicPose());
+	} else {
+		PPLibTelemetry::setTargetPose(targetState.getDifferentialPose());
+		PathPlannerLogging::logTargetPose(targetState.getDifferentialPose());
+	}
+
 	PPLibTelemetry::setVelocities(currentVel, targetState.velocity,
 			currentSpeeds.omega, targetSpeeds.omega);
-	PathPlannerLogging::logCurrentPose(currentPose);
-	PathPlannerLogging::logTargetPose(targetState.getTargetHolonomicPose());
 	PPLibTelemetry::setPathInaccuracy(m_controller->getPositionalError());
 
 	m_output(targetSpeeds);

pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp

@@ -130,9 +130,18 @@ void PathfindingCommand::Execute() {
 				currentSpeeds.vx, currentSpeeds.vy);
 
 		PPLibTelemetry::setCurrentPose(currentPose);
-		PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose());
 		PathPlannerLogging::logCurrentPose(currentPose);
-		PathPlannerLogging::logTargetPose(targetState.getTargetHolonomicPose());
+
+		if (m_controller->isHolonomic()) {
+			PPLibTelemetry::setTargetPose(targetState.getTargetHolonomicPose());
+			PathPlannerLogging::logTargetPose(
+					targetState.getTargetHolonomicPose());
+		} else {
+			PPLibTelemetry::setTargetPose(targetState.getDifferentialPose());
+			PathPlannerLogging::logTargetPose(
+					targetState.getDifferentialPose());
+		}
+
 		PPLibTelemetry::setVelocities(currentVel, targetState.velocity,
 				currentSpeeds.omega, targetSpeeds.omega);
 		PPLibTelemetry::setPathInaccuracy(m_controller->getPositionalError());

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h

@@ -71,6 +71,16 @@ class PPHolonomicDriveController: public PathFollowingController {
 			const frc::Pose2d &currentPose,
 			const PathPlannerTrajectory::State &referenceState) override;
 
+	/**
+	 * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+	 * in the path following command.
+	 *
+	 * @return True if this controller is for a holonomic drive train
+	 */
+	inline bool isHolonomic() override {
+		return true;
+	}
+
 private:
 	using rpsPerMps_t = units::unit_t<units::compound_unit<units::radians_per_second, units::inverse<units::meters_per_second>>>;
 

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPLTVController.h

@@ -59,6 +59,16 @@ class PPLTVController: public frc::LTVUnicycleController,
 		return m_lastError;
 	}
 
+	/**
+	 * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+	 * in the path following command.
+	 *
+	 * @return True if this controller is for a holonomic drive train
+	 */
+	inline bool isHolonomic() override {
+		return false;
+	}
+
 private:
 	units::meter_t m_lastError;
 };

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPRamseteController.h

@@ -44,6 +44,16 @@ class PPRamseteController: public frc::RamseteController,
 		return m_lastError;
 	}
 
+	/**
+	 * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+	 * in the path following command.
+	 *
+	 * @return True if this controller is for a holonomic drive train
+	 */
+	inline bool isHolonomic() override {
+		return false;
+	}
+
 private:
 	units::meter_t m_lastError;
 };

pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h

@@ -37,5 +37,13 @@ class PathFollowingController {
 	 * @return Positional error, in meters
 	 */
 	virtual units::meter_t getPositionalError() = 0;
+
+	/**
+	 * Is this controller for holonomic drivetrains? Used to handle some differences in functionality
+	 * in the path following command.
+	 *
+	 * @return True if this controller is for a holonomic drive train
+	 */
+	virtual bool isHolonomic() = 0;
 };
 }