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.math.controller;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUsageId;
009import edu.wpi.first.util.sendable.Sendable;
010import edu.wpi.first.util.sendable.SendableBuilder;
011import edu.wpi.first.util.sendable.SendableRegistry;
012
013/**
014 * Implements a bang-bang controller, which outputs either 0 or 1 depending on whether the
015 * measurement is less than the setpoint. This maximally-aggressive control approach works very well
016 * for velocity control of high-inertia mechanisms, and poorly on most other things.
017 *
018 * <p>Note that this is an *asymmetric* bang-bang controller - it will not exert any control effort
019 * in the reverse direction (e.g. it won't try to slow down an over-speeding shooter wheel). This
020 * asymmetry is *extremely important.* Bang-bang control is extremely simple, but also potentially
021 * hazardous. Always ensure that your motor controllers are set to "coast" before attempting to
022 * control them with a bang-bang controller.
023 */
024public class BangBangController implements Sendable {
025  private static int instances;
026
027  private double m_tolerance;
028
029  private double m_setpoint;
030  private double m_measurement;
031
032  /**
033   * Creates a new bang-bang controller.
034   *
035   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
036   * them with a bang-bang controller.
037   *
038   * @param tolerance Tolerance for {@link #atSetpoint() atSetpoint}.
039   */
040  public BangBangController(double tolerance) {
041    instances++;
042
043    setTolerance(tolerance);
044
045    SendableRegistry.addLW(this, "BangBangController", instances);
046
047    MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
048  }
049
050  /**
051   * Creates a new bang-bang controller.
052   *
053   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
054   * them with a bang-bang controller.
055   */
056  public BangBangController() {
057    this(Double.POSITIVE_INFINITY);
058  }
059
060  /**
061   * Sets the setpoint for the bang-bang controller.
062   *
063   * @param setpoint The desired setpoint.
064   */
065  public void setSetpoint(double setpoint) {
066    m_setpoint = setpoint;
067  }
068
069  /**
070   * Returns the current setpoint of the bang-bang controller.
071   *
072   * @return The current setpoint.
073   */
074  public double getSetpoint() {
075    return m_setpoint;
076  }
077
078  /**
079   * Returns true if the error is within the tolerance of the setpoint.
080   *
081   * @return Whether the error is within the acceptable bounds.
082   */
083  public boolean atSetpoint() {
084    return Math.abs(m_setpoint - m_measurement) < m_tolerance;
085  }
086
087  /**
088   * Sets the error within which atSetpoint will return true.
089   *
090   * @param tolerance Position error which is tolerable.
091   */
092  public void setTolerance(double tolerance) {
093    m_tolerance = tolerance;
094  }
095
096  /**
097   * Returns the current tolerance of the controller.
098   *
099   * @return The current tolerance.
100   */
101  public double getTolerance() {
102    return m_tolerance;
103  }
104
105  /**
106   * Returns the current measurement of the process variable.
107   *
108   * @return The current measurement of the process variable.
109   */
110  public double getMeasurement() {
111    return m_measurement;
112  }
113
114  /**
115   * Returns the current error.
116   *
117   * @return The current error.
118   */
119  public double getError() {
120    return m_setpoint - m_measurement;
121  }
122
123  /**
124   * Returns the calculated control output.
125   *
126   * <p>Always ensure that your motor controllers are set to "coast" before attempting to control
127   * them with a bang-bang controller.
128   *
129   * @param measurement The most recent measurement of the process variable.
130   * @param setpoint The setpoint for the process variable.
131   * @return The calculated motor output (0 or 1).
132   */
133  public double calculate(double measurement, double setpoint) {
134    m_measurement = measurement;
135    m_setpoint = setpoint;
136
137    return measurement < setpoint ? 1 : 0;
138  }
139
140  /**
141   * Returns the calculated control output.
142   *
143   * @param measurement The most recent measurement of the process variable.
144   * @return The calculated motor output (0 or 1).
145   */
146  public double calculate(double measurement) {
147    return calculate(measurement, m_setpoint);
148  }
149
150  @Override
151  public void initSendable(SendableBuilder builder) {
152    builder.setSmartDashboardType("BangBangController");
153    builder.addDoubleProperty("tolerance", this::getTolerance, this::setTolerance);
154    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
155    builder.addDoubleProperty("measurement", this::getMeasurement, null);
156    builder.addDoubleProperty("error", this::getError, null);
157    builder.addBooleanProperty("atSetpoint", this::atSetpoint, null);
158  }
159}