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

Minor improvements/fixes to new command framework (#2186)

Browse Source
This commit is contained in:
Oblarg
2019-12-23 02:09:25 -05:00
committed by Peter Johnson
parent 1ce24a7a2f
commit 67b59f2b31
9 changed files with 107 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
View File

@@ -25,10 +25,20 @@ public abstract class PIDSubsystem extends SubsystemBase {
* Creates a new PIDSubsystem.
*
* @param controller the PIDController to use
* @param initialPosition the initial setpoint of the subsystem
*/
public PIDSubsystem(PIDController controller, double initialPosition) {
setSetpoint(initialPosition);
m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
}
/**
* Creates a new PIDSubsystem. Initial setpoint is zero.
*
* @param controller the PIDController to use
*/
public PIDSubsystem(PIDController controller) {
requireNonNullParam(controller, "controller", "PIDSubsystem");
m_controller = controller;
this(controller, 0);
}
@Override

15
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
View File

@@ -27,10 +27,21 @@ public abstract class ProfiledPIDSubsystem extends SubsystemBase {
* Creates a new ProfiledPIDSubsystem.
*
* @param controller the ProfiledPIDController to use
* @param initialPosition the initial goal position of the controller
*/
public ProfiledPIDSubsystem(ProfiledPIDController controller,
double initialPosition) {
m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
setGoal(initialPosition);
}
/**
* Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
*
* @param controller the ProfiledPIDController to use
*/
public ProfiledPIDSubsystem(ProfiledPIDController controller) {
requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
m_controller = controller;
this(controller, 0);
}
@Override

60
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
View File

@@ -9,6 +9,8 @@ package edu.wpi.first.wpilibj2.command;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
/**
* 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.
@@ -20,41 +22,53 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
private TrapezoidProfile.State m_state;
private TrapezoidProfile.State m_goal;
/**
* 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 = 0.02;
}
private boolean m_enabled = true;
/**
* 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
* @param initialPosition The initial position of the controlled 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_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
m_state = new TrapezoidProfile.State(initialPosition, 0);
setGoal(initialPosition);
m_period = period;
}
/**
* Creates a new TrapezoidProfileSubsystem.
*
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
* @param initialPosition The initial position of the controlled mechanism when the subsystem
* is constructed.
*/
public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
double initialPosition) {
this(constraints, initialPosition, 0.02);
}
/**
* Creates a new TrapezoidProfileSubsystem.
*
* @param constraints The constraints (maximum velocity and acceleration) for the profiles.
*/
public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
this(constraints, 0, 0.02);
}
@Override
public void periodic() {
var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
m_state = profile.calculate(m_period);
useState(m_state);
if (m_enabled) {
useState(m_state);
}
}
/**
@@ -75,6 +89,20 @@ public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
setGoal(new TrapezoidProfile.State(goal, 0));
}
/**
* Enable the TrapezoidProfileSubsystem's output.
*/
public void enable() {
m_enabled = true;
}
/**
* Disable the TrapezoidProfileSubsystem's output.
*/
public void disable() {
m_enabled = false;
}
/**
* Users should override this to consume the current state of the motion profile.
*

6
wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
View File

@@ -9,8 +9,10 @@
using namespace frc2;
PIDSubsystem::PIDSubsystem(PIDController controller)
: m_controller{controller} {}
PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
: m_controller{controller} {
SetSetpoint(initialPosition);
}
void PIDSubsystem::Periodic() {
if (m_enabled) {

5
wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
View File

@@ -24,8 +24,9 @@ class PIDSubsystem : public SubsystemBase {
* Creates a new PIDSubsystem.
*
* @param controller the PIDController to use
* @param initialPosition the initial setpoint of the subsystem
*/
explicit PIDSubsystem(PIDController controller);
explicit PIDSubsystem(PIDController controller, double initialPosition = 0);
void Periodic() override;
@@ -62,7 +63,7 @@ class PIDSubsystem : public SubsystemBase {
protected:
PIDController m_controller;
bool m_enabled;
bool m_enabled{false};
/**
* Returns the measurement of the process variable used by the PIDController.

8
wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
View File

@@ -32,9 +32,13 @@ class ProfiledPIDSubsystem : public SubsystemBase {
* Creates a new ProfiledPIDSubsystem.
*
* @param controller the ProfiledPIDController to use
* @param initialPosition the initial goal position of the subsystem
*/
explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller)
: m_controller{controller} {}
explicit ProfiledPIDSubsystem(frc::ProfiledPIDController<Distance> controller,
Distance_t initialPosition = Distance_t{0})
: m_controller{controller} {
SetGoal(initialPosition);
}
void Periodic() override {
if (m_enabled) {

2
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
View File

@@ -42,7 +42,7 @@ class TrapezoidProfileCommand
*/
TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
std::function<void(State)> output,
std::initializer_list<Subsystem*> requirements)
std::initializer_list<Subsystem*> requirements = {})
: m_profile(profile), m_output(output) {
this->AddRequirements(requirements);
}

24
wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
View File

@@ -37,18 +37,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
* when the subsystem is constructed.
* @param period The period of the main robot loop, in seconds.
*/
TrapezoidProfileSubsystem(Constraints constraints, Distance_t position,
units::second_t period = 20_ms)
explicit TrapezoidProfileSubsystem(Constraints constraints,
Distance_t initialPosition = Distance_t{0},
units::second_t period = 20_ms)
: m_constraints(constraints),
m_state{position, Velocity_t(0)},
m_goal{position, Velocity_t{0}},
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);
UseState(m_state);
if (m_enabled) {
UseState(m_state);
}
}
/**
@@ -74,10 +77,21 @@ class TrapezoidProfileSubsystem : public SubsystemBase {
*/
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

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
View File

@@ -39,12 +39,11 @@ public class ArmSubsystem extends ProfiledPIDSubsystem {
* Create a new ArmSubsystem.
*/
public ArmSubsystem() {
super(new ProfiledPIDController(
kP,
0,
0,
new TrapezoidProfile.Constraints(kMaxVelocityRadPerSecond,
kMaxAccelerationRadPerSecSquared)));
super(new ProfiledPIDController(kP, 0, 0,
new TrapezoidProfile.Constraints(
kMaxVelocityRadPerSecond,
kMaxAccelerationRadPerSecSquared)),
0);
m_encoder.setDistancePerPulse(kEncoderDistancePerPulse);
// Start arm at rest in neutral position
setGoal(kArmOffsetRads);