diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java index 8e49706958..2fe8fa6d61 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java @@ -8,7 +8,6 @@ import static edu.wpi.first.math.trajectory.TrapezoidProfile.State; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; @@ -17,12 +16,12 @@ import java.util.function.Supplier; * *

This class is provided by the NewCommands VendorDep */ +@Deprecated(since = "2025", forRemoval = true) public class TrapezoidProfileCommand extends Command { private final TrapezoidProfile m_profile; private final Consumer m_output; private final Supplier m_goal; private final Supplier m_currentState; - private final Timer m_timer = new Timer(); /** * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}. @@ -49,23 +48,19 @@ public class TrapezoidProfileCommand extends Command { } @Override - public void initialize() { - m_timer.restart(); - } + public void initialize() {} @Override @SuppressWarnings("removal") public void execute() { - m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get())); + m_output.accept(m_profile.calculate(0.02, m_currentState.get(), m_goal.get())); } @Override - public void end(boolean interrupted) { - m_timer.stop(); - } + public void end(boolean interrupted) {} @Override public boolean isFinished() { - return m_timer.hasElapsed(m_profile.totalTime()); + return m_profile.isFinished(0); } } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index 30ad9b7087..096402a599 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -6,7 +6,6 @@ #include -#include #include #include "frc2/command/Command.h" @@ -54,16 +53,17 @@ class TrapezoidProfileCommand this->AddRequirements(requirements); } - void Initialize() override { m_timer.Restart(); } + void Initialize() override {} void Execute() override { - m_output(m_profile.Calculate(m_timer.Get(), m_currentState(), m_goal())); + m_output( + m_profile.Calculate(units::second_t{0.02}, m_currentState(), m_goal())); } - void End(bool interrupted) override { m_timer.Stop(); } + void End(bool interrupted) override {} bool IsFinished() override { - return m_timer.HasElapsed(m_profile.TotalTime()); + return m_profile.IsFinished(units::second_t{0}); } private: @@ -71,7 +71,6 @@ class TrapezoidProfileCommand std::function m_output; std::function m_goal; std::function m_currentState; - frc::Timer m_timer; }; } // namespace frc2 diff --git a/wpilibcExamples/src/main/cpp/commands/commands.json b/wpilibcExamples/src/main/cpp/commands/commands.json index 7f0c8f9a15..4fd9706f26 100644 --- a/wpilibcExamples/src/main/cpp/commands/commands.json +++ b/wpilibcExamples/src/main/cpp/commands/commands.json @@ -191,22 +191,6 @@ "replacename": "ReplaceMeSubsystem2", "commandversion": 2 }, - { - "name": "TrapezoidProfileCommand", - "description": "A command that executes a trapezoidal motion profile.", - "tags": [ - "trapezoidprofilecommand" - ], - "foldername": "trapezoidprofilecommand", - "headers": [ - "ReplaceMeTrapezoidProfileCommand.h" - ], - "source": [ - "ReplaceMeTrapezoidProfileCommand.cpp" - ], - "replacename": "ReplaceMeTrapezoidProfileCommand", - "commandversion": 2 - }, { "name": "TrapezoidProfileSubsystem", "description": "A subsystem that executes a trapezoidal motion profile.", diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp deleted file mode 100644 index c8131689ff..0000000000 --- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "ReplaceMeTrapezoidProfileCommand.h" - -#include -#include - -// NOTE: Consider using this command inline, rather than writing a subclass. -// For more information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand() - : CommandHelper - // The profile to execute - ( - frc::TrapezoidProfile( - // The maximum velocity and acceleration of the profile - {5_mps, 5_mps_sq}), - [](frc::TrapezoidProfile::State state) { - // Use the computed intermediate trajectory state here - }, - // The goal state of the profile - [] { - return frc::TrapezoidProfile::State{10_m, 0_mps}; - }, - // The initial state of the profile - [] { - return frc::TrapezoidProfile::State{0_m, 0_mps}; - }) {} diff --git a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h b/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h deleted file mode 100644 index a3cd8c86d6..0000000000 --- a/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include -#include - -class ReplaceMeTrapezoidProfileCommand - : public frc2::CommandHelper, - ReplaceMeTrapezoidProfileCommand> { - public: - ReplaceMeTrapezoidProfileCommand(); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp index 37ae1aac11..f828d3a1dc 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -6,8 +6,6 @@ #include -#include "commands/DriveDistanceProfiled.h" - RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here @@ -34,32 +32,12 @@ void RobotContainer::ConfigureButtonBindings() { // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of // 10 seconds m_driverController.A().OnTrue( - DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s)); + m_drive.ProfiledDriveDistance(3_m).WithTimeout(10_s)); - // Do the same thing as above when the 'B' button is pressed, but defined - // inline + // Do the same thing as above when the 'B' button is pressed, but without + // resetting the encoders m_driverController.B().OnTrue( - frc2::TrapezoidProfileCommand( - frc::TrapezoidProfile( - // Limit the max acceleration and velocity - {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}), - // Pipe the profile state to the drive - [this](auto setpointState) { - m_drive.SetDriveStates(setpointState, setpointState); - }, - // End at desired position in meters; implicitly starts at 0 - [] { - return frc::TrapezoidProfile::State{3_m, 0_mps}; - }, - // Current position - [] { return frc::TrapezoidProfile::State{}; }, - // Require the drive - {&m_drive}) - // Convert to CommandPtr - .ToPtr() - .BeforeStarting( - frc2::cmd::RunOnce([this]() { m_drive.ResetEncoders(); }, {})) - .WithTimeout(10_s)); + m_drive.DynamicProfiledDriveDistance(3_m).WithTimeout(10_s)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp deleted file mode 100644 index 8d45c91b67..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "commands/DriveDistanceProfiled.h" - -#include "Constants.h" - -using namespace DriveConstants; - -DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance, - DriveSubsystem* drive) - : CommandHelper{ - frc::TrapezoidProfile{ - // Limit the max acceleration and velocity - {kMaxSpeed, kMaxAcceleration}}, - // Pipe the profile state to the drive - [drive](auto setpointState) { - drive->SetDriveStates(setpointState, setpointState); - }, - // End at desired position in meters; implicitly starts at 0 - [distance] { - return frc::TrapezoidProfile::State{distance, 0_mps}; - }, - [] { return frc::TrapezoidProfile::State{}; }, - // Require the drive - {drive}} { - // Reset drive encoders since we're starting at 0 - drive->ResetEncoders(); -} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp index 84fae4f2f0..22290ba098 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -4,6 +4,8 @@ #include "subsystems/DriveSubsystem.h" +#include + using namespace DriveConstants; DriveSubsystem::DriveSubsystem() @@ -32,14 +34,21 @@ void DriveSubsystem::Periodic() { } void DriveSubsystem::SetDriveStates( - frc::TrapezoidProfile::State left, - frc::TrapezoidProfile::State right) { - m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - left.position.value(), - m_feedforward.Calculate(left.velocity) / 12_V); - m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition, - right.position.value(), - m_feedforward.Calculate(right.velocity) / 12_V); + frc::TrapezoidProfile::State currentLeft, + frc::TrapezoidProfile::State currentRight, + frc::TrapezoidProfile::State nextLeft, + frc::TrapezoidProfile::State nextRight) { + // Feedforward is divided by battery voltage to normalize it to [-1, 1] + m_leftLeader.SetSetpoint( + ExampleSmartMotorController::PIDMode::kPosition, + currentLeft.position.value(), + m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) / + frc::RobotController::GetBatteryVoltage()); + m_rightLeader.SetSetpoint( + ExampleSmartMotorController::PIDMode::kPosition, + currentRight.position.value(), + m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) / + frc::RobotController::GetBatteryVoltage()); } void DriveSubsystem::ArcadeDrive(double fwd, double rot) { @@ -62,3 +71,60 @@ units::meter_t DriveSubsystem::GetRightEncoderDistance() { void DriveSubsystem::SetMaxOutput(double maxOutput) { m_drive.SetMaxOutput(maxOutput); } + +frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance( + units::meter_t distance) { + return StartRun( + [&] { + // Restart timer so profile setpoints start at the beginning + m_timer.Restart(); + ResetEncoders(); + }, + [&] { + // Current state never changes, so we need to use a timer to get + // the setpoints we need to be at + auto currentTime = m_timer.Get(); + auto currentSetpoint = + m_profile.Calculate(currentTime, {}, {distance, 0_mps}); + auto nextSetpoint = m_profile.Calculate(currentTime + kDt, {}, + {distance, 0_mps}); + SetDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, + nextSetpoint); + }) + .Until([&] { return m_profile.IsFinished(0_s); }); +} + +frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance( + units::meter_t distance) { + return StartRun( + [&] { + // Restart timer so profile setpoints start at the beginning + m_timer.Restart(); + // Store distance so we know the target distance for each encoder + m_initialLeftDistance = GetLeftEncoderDistance(); + m_initialRightDistance = GetRightEncoderDistance(); + }, + [&] { + // Current state never changes for the duration of the command, + // so we need to use a timer to get the setpoints we need to be + // at + auto currentTime = m_timer.Get(); + + auto currentLeftSetpoint = m_profile.Calculate( + currentTime, {m_initialLeftDistance, 0_mps}, + {m_initialLeftDistance + distance, 0_mps}); + auto currentRightSetpoint = m_profile.Calculate( + currentTime, {m_initialRightDistance, 0_mps}, + {m_initialRightDistance + distance, 0_mps}); + + auto nextLeftSetpoint = m_profile.Calculate( + currentTime + kDt, {m_initialLeftDistance, 0_mps}, + {m_initialLeftDistance + distance, 0_mps}); + auto nextRightSetpoint = m_profile.Calculate( + currentTime + kDt, {m_initialRightDistance, 0_mps}, + {m_initialRightDistance + distance, 0_mps}); + SetDriveStates(currentLeftSetpoint, currentRightSetpoint, + nextLeftSetpoint, nextRightSetpoint); + }) + .Until([&] { return m_profile.IsFinished(0_s); }); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h index c9ec7e710f..4135a17d1b 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h @@ -22,6 +22,7 @@ */ namespace DriveConstants { +inline constexpr units::second_t kDt{0.02}; inline constexpr int kLeftMotor1Port = 0; inline constexpr int kLeftMotor2Port = 1; inline constexpr int kRightMotor1Port = 2; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h deleted file mode 100644 index 34cf8f374a..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include - -#include "subsystems/DriveSubsystem.h" - -class DriveDistanceProfiled - : public frc2::CommandHelper, - DriveDistanceProfiled> { - public: - DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive); -}; diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h index 3dd5f03f10..09e59dd83b 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h @@ -5,9 +5,11 @@ #pragma once #include +#include #include #include #include +#include #include #include @@ -28,11 +30,15 @@ class DriveSubsystem : public frc2::SubsystemBase { /** * Attempts to follow the given drive states using offboard PID. * - * @param left The left wheel state. - * @param right The right wheel state. + * @param currentLeft The current left wheel state. + * @param currentRight The current right wheel state. + * @param nextLeft The next left wheel state. + * @param nextRight The next right wheel state. */ - void SetDriveStates(frc::TrapezoidProfile::State left, - frc::TrapezoidProfile::State right); + void SetDriveStates(frc::TrapezoidProfile::State currentLeft, + frc::TrapezoidProfile::State currentRight, + frc::TrapezoidProfile::State nextLeft, + frc::TrapezoidProfile::State nextRight); /** * Drives the robot using arcade controls. @@ -69,7 +75,32 @@ class DriveSubsystem : public frc2::SubsystemBase { */ void SetMaxOutput(double maxOutput); + /** + * Creates a command to drive forward a specified distance using a motion + * profile. + * + * @param distance The distance to drive forward. + * @return A command. + */ + [[nodiscard]] + frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance); + + /** + * Creates a command to drive forward a specified distance using a motion + * profile without resetting the encoders. + * + * @param distance The distance to drive forward. + * @return A command. + */ + [[nodiscard]] + frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance); + private: + frc::TrapezoidProfile m_profile{ + {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}}; + frc::Timer m_timer; + units::meter_t m_initialLeftDistance; + units::meter_t m_initialRightDistance; // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json index d8d57f7b9c..5631c5bf3e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json @@ -119,16 +119,6 @@ "replacename": "ReplaceMeSubsystem", "commandversion": 2 }, - { - "name": "TrapezoidProfileCommand", - "description": "A command that executes a trapezoidal motion profile.", - "tags": [ - "trapezoidprofilecommand" - ], - "foldername": "trapezoidprofilecommand", - "replacename": "ReplaceMeTrapezoidProfileCommand", - "commandversion": 2 - }, { "name": "TrapezoidProfileSubsystem", "description": "A subsystem that executes a trapezoidal motion profile.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java deleted file mode 100644 index fa55f451d2..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj.commands.trapezoidprofilecommand; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand { - /** Creates a new ReplaceMeTrapezoidProfileCommand. */ - public ReplaceMeTrapezoidProfileCommand() { - super( - // The motion profile to be executed - new TrapezoidProfile( - // The motion profile constraints - new TrapezoidProfile.Constraints(0, 0)), - state -> { - // Use current trajectory state here - }, - // Goal state - TrapezoidProfile.State::new, - // Current state - TrapezoidProfile.State::new); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java index 0c7aeeb27f..1748045c49 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java @@ -14,6 +14,7 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; */ public final class Constants { public static final class DriveConstants { + public static final double kDt = 0.02; public static final int kLeftMotor1Port = 0; public static final int kLeftMotor2Port = 1; public static final int kRightMotor1Port = 2; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java index ba2ed04710..afdc53174a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -4,15 +4,11 @@ package edu.wpi.first.wpilibj.examples.drivedistanceoffboard; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants; -import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** @@ -61,28 +57,10 @@ public class RobotContainer { m_driverController.rightBumper().onTrue(m_driveHalfSpeed).onFalse(m_driveFullSpeed); // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds - m_driverController.a().onTrue(new DriveDistanceProfiled(3, m_robotDrive).withTimeout(10)); + m_driverController.a().onTrue(m_robotDrive.profiledDriveDistance(3).withTimeout(10)); - // Do the same thing as above when the 'B' button is pressed, but defined inline - m_driverController - .b() - .onTrue( - new TrapezoidProfileCommand( - new TrapezoidProfile( - // Limit the max acceleration and velocity - new TrapezoidProfile.Constraints( - DriveConstants.kMaxSpeedMetersPerSecond, - DriveConstants.kMaxAccelerationMetersPerSecondSquared)), - // Pipe the profile state to the drive - setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState), - // End at desired position in meters; implicitly starts at 0 - () -> new TrapezoidProfile.State(3, 0), - // Current position - TrapezoidProfile.State::new, - // Require the drive - m_robotDrive) - .beforeStarting(m_robotDrive::resetEncoders) - .withTimeout(10)); + // Do the same thing as above when the 'B' button is pressed, but without resetting the encoders + m_driverController.b().onTrue(m_robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java deleted file mode 100644 index 951cde30ed..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; -import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand; - -/** Drives a set distance using a motion profile. */ -public class DriveDistanceProfiled extends TrapezoidProfileCommand { - /** - * Creates a new DriveDistanceProfiled command. - * - * @param meters The distance to drive. - * @param drive The drive subsystem to use. - */ - public DriveDistanceProfiled(double meters, DriveSubsystem drive) { - super( - new TrapezoidProfile( - // Limit the max acceleration and velocity - new TrapezoidProfile.Constraints( - DriveConstants.kMaxSpeedMetersPerSecond, - DriveConstants.kMaxAccelerationMetersPerSecondSquared)), - // Pipe the profile state to the drive - setpointState -> drive.setDriveStates(setpointState, setpointState), - // End at desired position in meters; implicitly starts at 0 - () -> new TrapezoidProfile.State(meters, 0), - // Current position - TrapezoidProfile.State::new, - // Require the drive - drive); - // Reset drive encoders since we're starting at 0 - drive.resetEncoders(); - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index e2a66a64e1..67fea4c8ae 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -9,10 +9,14 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants; import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class DriveSubsystem extends SubsystemBase { @@ -41,6 +45,16 @@ public class DriveSubsystem extends SubsystemBase { private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader::set, m_rightLeader::set); + // The trapezoid profile + private final TrapezoidProfile m_profile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + DriveConstants.kMaxSpeedMetersPerSecond, + DriveConstants.kMaxAccelerationMetersPerSecondSquared)); + + // The timer + private final Timer m_timer = new Timer(); + /** Creates a new DriveSubsystem. */ public DriveSubsystem() { SendableRegistry.addChild(m_drive, m_leftLeader); @@ -71,18 +85,33 @@ public class DriveSubsystem extends SubsystemBase { /** * Attempts to follow the given drive states using offboard PID. * - * @param left The left wheel state. - * @param right The right wheel state. + * @param currentLeft The current left wheel state. + * @param currentRight The current right wheel state. + * @param nextLeft The next left wheel state. + * @param nextRight The next right wheel state. */ - public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) { + public void setDriveStates( + TrapezoidProfile.State currentLeft, + TrapezoidProfile.State currentRight, + TrapezoidProfile.State nextLeft, + TrapezoidProfile.State nextRight) { + // Feedforward is divided by battery voltage to normalize it to [-1, 1] m_leftLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, - left.position, - m_feedforward.calculate(MetersPerSecond.of(left.velocity)).in(Volts)); + currentLeft.position, + m_feedforward + .calculate( + MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) + .in(Volts) + / RobotController.getBatteryVoltage()); m_rightLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, - right.position, - m_feedforward.calculate(MetersPerSecond.of(right.velocity)).in(Volts)); + currentRight.position, + m_feedforward + .calculate( + MetersPerSecond.of(currentLeft.velocity), MetersPerSecond.of(nextLeft.velocity)) + .in(Volts) + / RobotController.getBatteryVoltage()); } /** @@ -117,4 +146,80 @@ public class DriveSubsystem extends SubsystemBase { public void setMaxOutput(double maxOutput) { m_drive.setMaxOutput(maxOutput); } + + /** + * Creates a command to drive forward a specified distance using a motion profile. + * + * @param distance The distance to drive forward. + * @return A command. + */ + public Command profiledDriveDistance(double distance) { + return startRun( + () -> { + // Restart timer so profile setpoints start at the beginning + m_timer.restart(); + resetEncoders(); + }, + () -> { + // Current state never changes, so we need to use a timer to get the setpoints we need + // to be at + var currentTime = m_timer.get(); + var currentSetpoint = + m_profile.calculate(currentTime, new State(), new State(distance, 0)); + var nextSetpoint = + m_profile.calculate( + currentTime + DriveConstants.kDt, new State(), new State(distance, 0)); + setDriveStates(currentSetpoint, currentSetpoint, nextSetpoint, nextSetpoint); + }) + .until(() -> m_profile.isFinished(0)); + } + + private double m_initialLeftDistance; + private double m_initialRightDistance; + + /** + * Creates a command to drive forward a specified distance using a motion profile without + * resetting the encoders. + * + * @param distance The distance to drive forward. + * @return A command. + */ + public Command dynamicProfiledDriveDistance(double distance) { + return startRun( + () -> { + // Restart timer so profile setpoints start at the beginning + m_timer.restart(); + // Store distance so we know the target distance for each encoder + m_initialLeftDistance = getLeftEncoderDistance(); + m_initialRightDistance = getRightEncoderDistance(); + }, + () -> { + // Current state never changes for the duration of the command, so we need to use a + // timer to get the setpoints we need to be at + var currentTime = m_timer.get(); + var currentLeftSetpoint = + m_profile.calculate( + currentTime, + new State(m_initialLeftDistance, 0), + new State(m_initialLeftDistance + distance, 0)); + var currentRightSetpoint = + m_profile.calculate( + currentTime, + new State(m_initialRightDistance, 0), + new State(m_initialRightDistance + distance, 0)); + var nextLeftSetpoint = + m_profile.calculate( + currentTime + DriveConstants.kDt, + new State(m_initialLeftDistance, 0), + new State(m_initialLeftDistance + distance, 0)); + var nextRightSetpoint = + m_profile.calculate( + currentTime + DriveConstants.kDt, + new State(m_initialRightDistance, 0), + new State(m_initialRightDistance + distance, 0)); + setDriveStates( + currentLeftSetpoint, currentRightSetpoint, nextLeftSetpoint, nextRightSetpoint); + }) + .until(() -> m_profile.isFinished(0)); + } }