mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
8ac45f20bb
Add reference to which VendorDep the class is included in. Add missing OldCommands C++ Documentation (copied from Java).
97 lines
2.9 KiB
C++
97 lines
2.9 KiB
C++
// 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 <frc/trajectory/TrapezoidProfile.h>
|
|
#include <units/time.h>
|
|
|
|
#include "frc2/command/SubsystemBase.h"
|
|
|
|
namespace frc2 {
|
|
/**
|
|
* A subsystem that generates and runs trapezoidal motion profiles
|
|
* automatically. The user specifies how to use the current state of the motion
|
|
* profile by overriding the `UseState` method.
|
|
*
|
|
* This class is provided by the NewCommands VendorDep
|
|
*/
|
|
template <class Distance>
|
|
class TrapezoidProfileSubsystem : public SubsystemBase {
|
|
using Distance_t = units::unit_t<Distance>;
|
|
using Velocity =
|
|
units::compound_unit<Distance, units::inverse<units::seconds>>;
|
|
using Velocity_t = units::unit_t<Velocity>;
|
|
using State = typename frc::TrapezoidProfile<Distance>::State;
|
|
using Constraints = typename frc::TrapezoidProfile<Distance>::Constraints;
|
|
|
|
public:
|
|
/**
|
|
* Creates a new TrapezoidProfileSubsystem.
|
|
*
|
|
* @param constraints The constraints (maximum velocity and acceleration)
|
|
* for the profiles.
|
|
* @param initialPosition The initial position of the controller mechanism
|
|
* when the subsystem is constructed.
|
|
* @param period The period of the main robot loop, in seconds.
|
|
*/
|
|
explicit TrapezoidProfileSubsystem(Constraints constraints,
|
|
Distance_t initialPosition = Distance_t{0},
|
|
units::second_t period = 20_ms)
|
|
: m_constraints(constraints),
|
|
m_state{initialPosition, Velocity_t(0)},
|
|
m_goal{initialPosition, Velocity_t{0}},
|
|
m_period(period) {}
|
|
|
|
void Periodic() override {
|
|
auto profile =
|
|
frc::TrapezoidProfile<Distance>(m_constraints, m_goal, m_state);
|
|
m_state = profile.Calculate(m_period);
|
|
if (m_enabled) {
|
|
UseState(m_state);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Sets the goal state for the subsystem.
|
|
*
|
|
* @param goal The goal state for the subsystem's motion profile.
|
|
*/
|
|
void SetGoal(State goal) { m_goal = goal; }
|
|
|
|
/**
|
|
* Sets the goal state for the subsystem. Goal velocity assumed to be zero.
|
|
*
|
|
* @param goal The goal position for the subsystem's motion profile.
|
|
*/
|
|
void SetGoal(Distance_t goal) { m_goal = State{goal, Velocity_t(0)}; }
|
|
|
|
protected:
|
|
/**
|
|
* Users should override this to consume the current state of the motion
|
|
* profile.
|
|
*
|
|
* @param state The current state of the motion profile.
|
|
*/
|
|
virtual void UseState(State state) = 0;
|
|
|
|
/**
|
|
* Enable the TrapezoidProfileSubsystem's output.
|
|
*/
|
|
void Enable() { m_enabled = true; }
|
|
|
|
/**
|
|
* Disable the TrapezoidProfileSubsystem's output.
|
|
*/
|
|
void Disable() { m_enabled = false; }
|
|
|
|
private:
|
|
Constraints m_constraints;
|
|
State m_state;
|
|
State m_goal;
|
|
units::second_t m_period;
|
|
bool m_enabled{false};
|
|
};
|
|
} // namespace frc2
|