diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java index 0fde03b3..08c99e76 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java +++ b/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, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 1860952b..a10e8478 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/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, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java index a1bc64ef..df95313a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java +++ b/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; + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java index 446083f7..dd6a1a65 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java +++ b/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; + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java index d9fd6458..523a9234 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java +++ b/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; + } } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java index 45d12d9a..038b41d4 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java +++ b/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(); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index df92cb1a..981f0943 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp +++ b/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); diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp index 2b39b623..3851ef16 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathfindingCommand.cpp +++ b/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()); diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h index 7b388293..a68c62bc 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPHolonomicDriveController.h @@ -71,6 +71,16 @@ class PPHolonomicDriveController: public PathFollowingController { const frc::Pose2d ¤tPose, 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>>; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPLTVController.h b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPLTVController.h index 2449baca..5748baa2 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPLTVController.h +++ b/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; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPRamseteController.h b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPRamseteController.h index 6be121a9..3e91fc77 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PPRamseteController.h +++ b/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; }; diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h index 387ea5b4..dba487b8 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h +++ b/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; }; }