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

[wpimath] Refactor TrapezoidProfile API (#5457)

Browse Source
This commit is contained in:
Gold856
2023-07-19 20:25:10 -04:00
committed by GitHub
parent 72a4543493
commit 86e91e6724
24 changed files with 492 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,13 +13,18 @@
ReplaceMeTrapezoidProfileCommand::ReplaceMeTrapezoidProfileCommand()
: CommandHelper
// The profile to execute
(frc::TrapezoidProfile<units::meters>(
// The maximum velocity and acceleration of the profile
{5_mps, 5_mps_sq},
// The goal state of the profile
{10_m, 0_mps},
// The initial state of the profile
{0_m, 0_mps}),
[](frc::TrapezoidProfile<units::meters>::State state) {
// Use the computed intermediate trajectory state here
}) {}
(
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};
}) {}

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

@@ -40,13 +40,17 @@ void RobotContainer::ConfigureButtonBindings() {
frc2::TrapezoidProfileCommand<units::meters>(
frc::TrapezoidProfile<units::meters>(
// Limit the max acceleration and velocity
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{3_m, 0_mps}),
{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

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

@@ -13,13 +13,16 @@ DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
: CommandHelper{
frc::TrapezoidProfile<units::meters>{
// Limit the max acceleration and velocity
{kMaxSpeed, kMaxAcceleration},
// End at desired position in meters; implicitly starts at 0
{distance, 0_mps}},
{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

8
wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
View File

@@ -33,14 +33,12 @@ class Robot : public frc::TimedRobot {
}
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint, the desired goal, and the
// current setpoint.
frc::TrapezoidProfile<units::meters> profile{m_constraints, m_goal,
m_setpoint};
// acceleration constraints for the next setpoint.
frc::TrapezoidProfile<units::meters> profile{m_constraints};
// Retrieve the profiled setpoint for the next timestep. This setpoint moves
// toward the goal while obeying the constraints.
m_setpoint = profile.Calculate(kDt);
m_setpoint = profile.Calculate(kDt, m_goal, m_setpoint);
// Send setpoint to offboard controller PID
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,

8
wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
View File

@@ -86,8 +86,8 @@ class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_motor{kMotorPort};
frc::XboxController m_joystick{kJoystickPort};
frc::TrapezoidProfile<units::radians>::Constraints m_constraints{
45_deg_per_s, 90_deg_per_s / 1_s};
frc::TrapezoidProfile<units::radians> m_profile{
{45_deg_per_s, 90_deg_per_s / 1_s}};
frc::TrapezoidProfile<units::radians>::State m_lastProfiledReference;
@@ -117,9 +117,7 @@ class Robot : public frc::TimedRobot {
goal = {kLoweredPosition, 0_rad_per_s};
}
m_lastProfiledReference =
(frc::TrapezoidProfile<units::radians>(m_constraints, goal,
m_lastProfiledReference))
.Calculate(20_ms);
m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});

7
wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
View File

@@ -86,8 +86,7 @@ class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_motor{kMotorPort};
frc::XboxController m_joystick{kJoystickPort};
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{3_fps,
6_fps_sq};
frc::TrapezoidProfile<units::meters> m_profile{{3_fps, 6_fps_sq}};
frc::TrapezoidProfile<units::meters>::State m_lastProfiledReference;
@@ -118,9 +117,7 @@ class Robot : public frc::TimedRobot {
goal = {kLoweredPosition, 0_fps};
}
m_lastProfiledReference =
(frc::TrapezoidProfile<units::meters>(m_constraints, goal,
m_lastProfiledReference))
.Calculate(20_ms);
m_profile.Calculate(20_ms, goal, m_lastProfiledReference);
m_loop.SetNextR(frc::Vectord<2>{m_lastProfiledReference.position.value(),
m_lastProfiledReference.velocity.value()});