001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj2.command; 006 007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; 008 009import edu.wpi.first.math.controller.PIDController; 010 011/** 012 * A subsystem that uses a {@link PIDController} to control an output. The controller is run 013 * synchronously from the subsystem's periodic() method. 014 * 015 * <p>This class is provided by the NewCommands VendorDep 016 */ 017public abstract class PIDSubsystem extends SubsystemBase { 018 protected final PIDController m_controller; 019 protected boolean m_enabled; 020 021 /** 022 * Creates a new PIDSubsystem. 023 * 024 * @param controller the PIDController to use 025 * @param initialPosition the initial setpoint of the subsystem 026 */ 027 @SuppressWarnings("this-escape") 028 public PIDSubsystem(PIDController controller, double initialPosition) { 029 m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem"); 030 setSetpoint(initialPosition); 031 addChild("PID Controller", m_controller); 032 } 033 034 /** 035 * Creates a new PIDSubsystem. Initial setpoint is zero. 036 * 037 * @param controller the PIDController to use 038 */ 039 public PIDSubsystem(PIDController controller) { 040 this(controller, 0); 041 } 042 043 @Override 044 public void periodic() { 045 if (m_enabled) { 046 useOutput(m_controller.calculate(getMeasurement()), getSetpoint()); 047 } 048 } 049 050 public PIDController getController() { 051 return m_controller; 052 } 053 054 /** 055 * Sets the setpoint for the subsystem. 056 * 057 * @param setpoint the setpoint for the subsystem 058 */ 059 public final void setSetpoint(double setpoint) { 060 m_controller.setSetpoint(setpoint); 061 } 062 063 /** 064 * Returns the current setpoint of the subsystem. 065 * 066 * @return The current setpoint 067 */ 068 public double getSetpoint() { 069 return m_controller.getSetpoint(); 070 } 071 072 /** 073 * Uses the output from the PIDController. 074 * 075 * @param output the output of the PIDController 076 * @param setpoint the setpoint of the PIDController (for feedforward) 077 */ 078 protected abstract void useOutput(double output, double setpoint); 079 080 /** 081 * Returns the measurement of the process variable used by the PIDController. 082 * 083 * @return the measurement of the process variable 084 */ 085 protected abstract double getMeasurement(); 086 087 /** Enables the PID control. Resets the controller. */ 088 public void enable() { 089 m_enabled = true; 090 m_controller.reset(); 091 } 092 093 /** Disables the PID control. Sets output to zero. */ 094 public void disable() { 095 m_enabled = false; 096 useOutput(0, 0); 097 } 098 099 /** 100 * Returns whether the controller is enabled. 101 * 102 * @return Whether the controller is enabled. 103 */ 104 public boolean isEnabled() { 105 return m_enabled; 106 } 107}