Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[commands] Fix and deprecate TrapezoidProfileCommand (#6722)

Browse Source
This commit is contained in:
Gold856
2024-08-14 00:01:17 -04:00
committed by GitHub
parent 55c1c5396d
commit a2060feadc
17 changed files with 240 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
View File

@@ -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;
*
* <p>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<State> m_output;
private final Supplier<State> m_goal;
private final Supplier<State> 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);
}
}

11
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
View File

@@ -6,7 +6,6 @@
#include <functional>
#include <frc/Timer.h>
#include <frc/trajectory/TrapezoidProfile.h>
#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<void(State)> m_output;
std::function<State()> m_goal;
std::function<State()> m_currentState;
frc::Timer m_timer;
};
} // namespace frc2

16
wpilibcExamples/src/main/cpp/commands/commands.json
View File

@@ -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.",

30
wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
View File

@@ -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 <units/acceleration.h>
#include <units/velocity.h>
// 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<units::meters>(
// The maximum velocity and acceleration of the profile
{5_mps, 5_mps_sq}),
[](frc::TrapezoidProfile<units::meters>::State state) {
// Use the computed intermediate trajectory state here
},
// The goal state of the profile
[] {
return frc::TrapezoidProfile<units::meters>::State{10_m, 0_mps};
},
// The initial state of the profile
[] {
return frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps};
}) {}

16
wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
View File

@@ -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 <frc2/command/CommandHelper.h>
#include <frc2/command/TrapezoidProfileCommand.h>
#include <units/length.h>
class ReplaceMeTrapezoidProfileCommand
: public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
ReplaceMeTrapezoidProfileCommand> {
public:
ReplaceMeTrapezoidProfileCommand();
};

30
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
View File

@@ -6,8 +6,6 @@
#include <frc2/command/Commands.h>
#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<units::meters>(
frc::TrapezoidProfile<units::meters>(
// 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<units::meters>::State{3_m, 0_mps};
},
// Current position
[] { return frc::TrapezoidProfile<units::meters>::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() {

30
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
View File

@@ -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<units::meters>{
// 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<units::meters>::State{distance, 0_mps};
},
[] { return frc::TrapezoidProfile<units::meters>::State{}; },
// Require the drive
{drive}} {
// Reset drive encoders since we're starting at 0
drive->ResetEncoders();
}

82
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -4,6 +4,8 @@
#include "subsystems/DriveSubsystem.h"
#include <frc/RobotController.h>
using namespace DriveConstants;
DriveSubsystem::DriveSubsystem()
@@ -32,14 +34,21 @@ void DriveSubsystem::Periodic() {
}
void DriveSubsystem::SetDriveStates(
frc::TrapezoidProfile<units::meters>::State left,
frc::TrapezoidProfile<units::meters>::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<units::meters>::State currentLeft,
frc::TrapezoidProfile<units::meters>::State currentRight,
frc::TrapezoidProfile<units::meters>::State nextLeft,
frc::TrapezoidProfile<units::meters>::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); });
}

1
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
View File

@@ -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;

17
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
View File

@@ -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 <frc2/command/CommandHelper.h>
#include <frc2/command/TrapezoidProfileCommand.h>
#include "subsystems/DriveSubsystem.h"
class DriveDistanceProfiled
: public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
DriveDistanceProfiled> {
public:
DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
};

39
wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
View File

@@ -5,9 +5,11 @@
#pragma once
#include <frc/Encoder.h>
#include <frc/Timer.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <frc2/command/CommandPtr.h>
#include <frc2/command/SubsystemBase.h>
#include <units/length.h>
@@ -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<units::meters>::State left,
frc::TrapezoidProfile<units::meters>::State right);
void SetDriveStates(frc::TrapezoidProfile<units::meters>::State currentLeft,
frc::TrapezoidProfile<units::meters>::State currentRight,
frc::TrapezoidProfile<units::meters>::State nextLeft,
frc::TrapezoidProfile<units::meters>::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<units::meters> 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.

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
View File

@@ -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.",

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
View File

@@ -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);
}
}

1
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
View File

@@ -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;

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
View File

@@ -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));
}
/**

38
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
View File

@@ -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();
}
}

119
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -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));
}
}