diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java new file mode 100644 index 0000000000..20dbad01f7 --- /dev/null +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +/** + * 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. + */ +public abstract class TrapezoidProfileSubsystem extends SubsystemBase { + private final double m_period; + private final TrapezoidProfile.Constraints m_constraints; + + private TrapezoidProfile.State m_state; + + /** + * 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. + */ + public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints, + double initialPosition) { + m_constraints = constraints; + m_state = new TrapezoidProfile.State(initialPosition, 0); + m_period = .02; + } + + /** + * 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. + */ + public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints, + double initialPosition, + double period) { + m_constraints = constraints; + m_state = new TrapezoidProfile.State(initialPosition, 0); + m_period = period; + } + + @Override + public void periodic() { + var profile = new TrapezoidProfile(m_constraints, getGoal(), m_state); + m_state = profile.calculate(m_period); + useState(m_state); + } + + /** + * Users should override this to return the goal state for the subsystem's motion profile. + * + * @return The goal state for the subsystem's motion profile. + */ + public abstract TrapezoidProfile.State getGoal(); + + /** + * Users should override this to consume the current state of the motion profile. + * + * @param state The current state of the motion profile. + */ + protected abstract void useState(TrapezoidProfile.State state); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h new file mode 100644 index 0000000000..f9206c5dc1 --- /dev/null +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "frc2/command/SubsystemBase.h" +#include "frc/trajectory/TrapezoidProfile.h" + +#include + +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. + */ +template +class TrapezoidProfileSubsystem : public SubsystemBase { + using State = frc::TrapezoidProfile::State; + using Constraints = frc::TrapezoidProfile::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. + */ + TrapezoidProfileSubsystem(Constraints constraints, + units::unit_t position, + units::second_t period = 20_ms) + : m_constraints(constraints), + m_state{units::meter_t(position.template to()), 0_mps}, + m_period(period) {} + + void Periodic() override { + auto profile = frc::TrapezoidProfile(m_constraints, GetGoal(), m_state); + m_state = profile.Calculate(m_period); + UseState(m_state); + } + + /** + * Users should override this to return the goal state for the subsystem's motion profile. + * + * @return The goal state for the subsystem's motion profile. + */ + virtual State GetGoal() = 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; + + private: + Constraints m_constraints; + State m_state; + units::second_t m_period; +}; +}