Skip to content

Commit

Permalink
update for current 2024 wpilib dev builds
Browse files Browse the repository at this point in the history
  • Loading branch information
mjansen4857 committed Oct 6, 2023
1 parent 204a608 commit 876283a
Show file tree
Hide file tree
Showing 9 changed files with 28 additions and 226 deletions.
16 changes: 8 additions & 8 deletions pathplannerlib/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,14 @@ apply from: 'config.gradle'

// Apply Java configuration
dependencies {
implementation 'edu.wpi.first.cscore:cscore-java:2023.+'
implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+'
implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+'
implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+'
implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+'
implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+'
implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+'
implementation 'edu.wpi.first.hal:hal-java:2023.+'
implementation 'edu.wpi.first.cscore:cscore-java:2024.+'
implementation 'edu.wpi.first.cameraserver:cameraserver-java:2024.+'
implementation 'edu.wpi.first.ntcore:ntcore-java:2024.+'
implementation 'edu.wpi.first.wpilibj:wpilibj-java:2024.+'
implementation 'edu.wpi.first.wpiutil:wpiutil-java:2024.+'
implementation 'edu.wpi.first.wpimath:wpimath-java:2024.+'
implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.+'
implementation 'edu.wpi.first.hal:hal-java:2024.+'
implementation "org.ejml:ejml-simple:0.41"
implementation "com.fasterxml.jackson.core:jackson-annotations:2.12.4"
implementation "com.fasterxml.jackson.core:jackson-core:2.12.4"
Expand Down
6 changes: 3 additions & 3 deletions pathplannerlib/config.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,12 @@ nativeUtils.withCrossRoboRIO()
nativeUtils {
wpi {
configureDependencies {
wpiVersion = "2023.+"
niLibVersion = "2023.+"
wpiVersion = "2024.+"
niLibVersion = "2024.+"
opencvVersion = "4.6.0-3"
googleTestVersion = "1.11.0-4"
imguiVersion = "1.88-9"
wpimathVersion = "2023.+"
wpimathVersion = "2024.+"
}
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,22 +7,10 @@ FollowPathCommand::FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
std::unique_ptr<PathFollowingController> controller,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements) : m_path(path), m_poseSupplier(
poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller(
std::move(controller)), m_replanningConfig(replanningConfig) {
AddRequirements(requirements);
}

FollowPathCommand::FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
std::unique_ptr<PathFollowingController> controller,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements) : m_path(path), m_poseSupplier(
poseSupplier), m_speedsSupplier(speedsSupplier), m_output(output), m_controller(
std::move(controller)), m_replanningConfig(replanningConfig) {
ReplanningConfig replanningConfig, frc2::Requirements requirements) : m_path(
path), m_poseSupplier(poseSupplier), m_speedsSupplier(speedsSupplier), m_output(
output), m_controller(std::move(controller)), m_replanningConfig(
replanningConfig) {
AddRequirements(requirements);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,12 @@

#include <frc2/command/Command.h>
#include <frc2/command/CommandHelper.h>
#include <frc2/command/Requirements.h>
#include <memory>
#include <functional>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/Timer.h>
#include <initializer_list>
#include <span>
#include <units/velocity.h>
#include <units/length.h>
#include <units/time.h>
Expand Down Expand Up @@ -40,28 +39,7 @@ class FollowPathCommand: public frc2::CommandHelper<frc2::Command,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
std::unique_ptr<PathFollowingController> controller,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements);

/**
* Construct a base path following command
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this
* command
* @param controller Path following controller that will be used to follow the path
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathCommand(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
std::unique_ptr<PathFollowingController> controller,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements);
ReplanningConfig replanningConfig, frc2::Requirements requirements);

void Initialize() override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,43 +34,8 @@ class FollowPathHolonomic: public FollowPathCommand {
PIDConstants translationConstants, PIDConstants rotationConstants,
units::meters_per_second_t maxModuleSpeed,
units::meter_t driveBaseRadius, ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements,
units::second_t period = 0.02_s) : FollowPathCommand(path,
poseSupplier, speedsSupplier, output,
std::make_unique < PPHolonomicDriveController
> (translationConstants, rotationConstants, maxModuleSpeed, driveBaseRadius, period),
replanningConfig, requirements) {
}

/**
* Construct a path following command that will use a holonomic drive controller for holonomic
* drive trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this
* command
* @param translationConstants PID constants for the translation PID controllers
* @param rotationConstants PID constants for the rotation controller
* @param maxModuleSpeed The max speed of a drive module in meters/sec
* @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the
* distance from the center of the robot to the furthest module. For mecanum, this is the
* drive base width / 2
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
* @param period Period of the control loop in seconds, default is 0.02s
*/
FollowPathHolonomic(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
PIDConstants translationConstants, PIDConstants rotationConstants,
units::meters_per_second_t maxModuleSpeed,
units::meter_t driveBaseRadius, ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements, units::second_t period =
0.02_s) : FollowPathCommand(path, poseSupplier,
speedsSupplier, output,
frc2::Requirements requirements, units::second_t period = 0.02_s) : FollowPathCommand(
path, poseSupplier, speedsSupplier, output,
std::make_unique < PPHolonomicDriveController
> (translationConstants, rotationConstants, maxModuleSpeed, driveBaseRadius, period),
replanningConfig, requirements) {
Expand All @@ -92,32 +57,7 @@ class FollowPathHolonomic: public FollowPathCommand {
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
HolonomicPathFollowerConfig config,
std::initializer_list<frc2::Subsystem*> requirements) : FollowPathHolonomic(
path, poseSupplier, speedsSupplier, output,
config.translationConstants, config.rotationConstants,
config.maxModuleSpeed, config.driveBaseRadius,
config.replanningConfig, requirements, config.period) {
}

/**
* Construct a path following command that will use a holonomic drive controller for holonomic
* drive trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this
* command
* @param config Holonomic path follower configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathHolonomic(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
HolonomicPathFollowerConfig config,
std::span<frc2::Subsystem*> requirements) : FollowPathHolonomic(
HolonomicPathFollowerConfig config, frc2::Requirements requirements) : FollowPathHolonomic(
path, poseSupplier, speedsSupplier, output,
config.translationConstants, config.rotationConstants,
config.maxModuleSpeed, config.driveBaseRadius,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,40 +26,12 @@ class FollowPathLTV: public FollowPathCommand {
std::function<void(frc::ChassisSpeeds)> output,
const wpi::array<double, 3> &Qelms,
const wpi::array<double, 2> &Relms, units::second_t dt,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements) : FollowPathCommand(
ReplanningConfig replanningConfig, frc2::Requirements requirements) : FollowPathCommand(
path, poseSupplier, speedsSupplier, output,
std::make_unique < PPLTVController > (Qelms, Relms, dt),
replanningConfig, requirements) {
}

/**
* Create a path following command that will use an LTV unicycle controller for differential drive
* trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this command
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt The amount of time between each robot control loop, default is 0.02s
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathLTV(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
const wpi::array<double, 3> &Qelms,
const wpi::array<double, 2> &Relms, units::second_t dt,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements) : FollowPathCommand(path,
poseSupplier, speedsSupplier, output,
std::make_unique < PPLTVController > (Qelms, Relms, dt),
replanningConfig, requirements) {
}

/**
* Create a path following command that will use an LTV unicycle controller for differential drive
* trains
Expand All @@ -76,34 +48,10 @@ class FollowPathLTV: public FollowPathCommand {
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output, units::second_t dt,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements) : FollowPathCommand(
ReplanningConfig replanningConfig, frc2::Requirements requirements) : FollowPathCommand(
path, poseSupplier, speedsSupplier, output,
std::make_unique < PPLTVController > (dt), replanningConfig,
requirements) {
}

/**
* Create a path following command that will use an LTV unicycle controller for differential drive
* trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this command
* @param dt The amount of time between each robot control loop, default is 0.02s
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathLTV(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output, units::second_t dt,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements) : FollowPathCommand(path,
poseSupplier, speedsSupplier, output,
std::make_unique < PPLTVController > (dt), replanningConfig,
requirements) {
}
};
}
Original file line number Diff line number Diff line change
Expand Up @@ -27,41 +27,12 @@ class FollowPathRamsete: public FollowPathCommand {
std::function<void(frc::ChassisSpeeds)> output,
units::unit_t<frc::RamseteController::b_unit> b,
units::unit_t<frc::RamseteController::zeta_unit> zeta,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements) : FollowPathCommand(
ReplanningConfig replanningConfig, frc2::Requirements requirements) : FollowPathCommand(
path, poseSupplier, speedsSupplier, output,
std::make_unique < PPRamseteController > (b, zeta),
replanningConfig, requirements) {
}

/**
* Construct a path following command that will use a Ramsete path following controller for
* differential drive trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this command
* @param b Tuning parameter (b &gt; 0 rad^2/m^2) for which larger values make convergence more
* aggressive like a proportional term.
* @param zeta Tuning parameter (0 rad^-1 &lt; zeta &lt; 1 rad^-1) for which larger values provide
* more damping in response.
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathRamsete(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
units::unit_t<frc::RamseteController::b_unit> b,
units::unit_t<frc::RamseteController::zeta_unit> zeta,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements) : FollowPathCommand(path,
poseSupplier, speedsSupplier, output,
std::make_unique < PPRamseteController > (b, zeta),
replanningConfig, requirements) {
}

/**
* Construct a path following command that will use a Ramsete path following controller for
* differential drive trains
Expand All @@ -77,33 +48,10 @@ class FollowPathRamsete: public FollowPathCommand {
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
ReplanningConfig replanningConfig,
std::initializer_list<frc2::Subsystem*> requirements) : FollowPathCommand(
ReplanningConfig replanningConfig, frc2::Requirements requirements) : FollowPathCommand(
path, poseSupplier, speedsSupplier, output,
std::make_unique<PPRamseteController>(), replanningConfig,
requirements) {
}

/**
* Construct a path following command that will use a Ramsete path following controller for
* differential drive trains
*
* @param path The path to follow
* @param poseSupplier Function that supplies the current field-relative pose of the robot
* @param speedsSupplier Function that supplies the current robot-relative chassis speeds
* @param output Function that will apply the robot-relative output speeds of this command
* @param replanningConfig Path replanning configuration
* @param requirements Subsystems required by this command, usually just the drive subsystem
*/
FollowPathRamsete(std::shared_ptr<PathPlannerPath> path,
std::function<frc::Pose2d()> poseSupplier,
std::function<frc::ChassisSpeeds()> speedsSupplier,
std::function<void(frc::ChassisSpeeds)> output,
ReplanningConfig replanningConfig,
std::span<frc2::Subsystem*> requirements) : FollowPathCommand(path,
poseSupplier, speedsSupplier, output,
std::make_unique<PPRamseteController>(), replanningConfig,
requirements) {
}
};
}
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,8 @@ class PPHolonomicDriveController: public PathFollowingController {
private:
using rpsPerMps_t = units::unit_t<units::compound_unit<units::radians_per_second, units::inverse<units::meters_per_second>>>;

frc2::PIDController m_xController;
frc2::PIDController m_yController;
frc::PIDController m_xController;
frc::PIDController m_yController;
frc::ProfiledPIDController<units::radians> m_rotationController;
units::meters_per_second_t m_maxModuleSpeed;
rpsPerMps_t m_mpsToRps;
Expand Down
4 changes: 2 additions & 2 deletions pathplannerlib/vendordeps/WPILibNewCommands.json
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
{
"fileName": "WPILibNewCommands.json",
"name": "WPILib-New-Commands",
"version": "2023.+",
"version": "2024.+",
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
"mavenUrls": [],
"jsonUrl": "",
Expand All @@ -17,7 +17,7 @@
{
"groupId": "edu.wpi.first.wpilibNewCommands",
"artifactId": "wpilibNewCommands-cpp",
"version": "2023.+",
"version": "2024.+",
"libName": "wpilibNewCommands",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
Expand Down

0 comments on commit 876283a

Please sign in to comment.