Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51: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

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.