Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add TrapezoidProfileSubsystem (#2077)

Browse Source 
Adds a subsystem that continually runs a TrapezoidProfile, for composition with offboard PID controllers.
This commit is contained in:
Oblarg
2019-11-19 15:38:42 -05:00
committed by Peter Johnson
parent 45201d15fc
commit 8c4d9f5415
2 changed files with 139 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java Normal file
View File

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