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.filter;
006
007import edu.wpi.first.math.MathSharedStore;
008
009/**
010 * A rising edge counter for boolean streams. Requires that the boolean change value to true for a
011 * specified number of times within a specified time window after the first rising edge before the
012 * filtered value changes.
013 *
014 * <p>The filter activates when the input has risen (transitioned from false to true) the required
015 * number of times within the time window. Once activated, the output remains true as long as the
016 * input is true. The edge count resets when the time window expires or when the input goes false
017 * after activation.
018 *
019 * <p>Input must be stable; consider using a Debouncer before this filter to avoid counting noise as
020 * multiple edges.
021 */
022public class EdgeCounterFilter {
023  private int m_requiredEdges;
024  private double m_windowTimeSeconds;
025
026  private double m_firstEdgeTimeSeconds;
027  private int m_currentCount;
028
029  private boolean m_lastInput;
030
031  /**
032   * Creates a new EdgeCounterFilter.
033   *
034   * @param requiredEdges The number of rising edges required before the output goes true.
035   * @param windowTime The maximum number of seconds in which all required edges must occur after
036   *     the first rising edge.
037   */
038  public EdgeCounterFilter(int requiredEdges, double windowTime) {
039    m_requiredEdges = requiredEdges;
040    m_windowTimeSeconds = windowTime;
041
042    resetTimer();
043  }
044
045  private void resetTimer() {
046    m_firstEdgeTimeSeconds = MathSharedStore.getTimestamp();
047  }
048
049  private boolean hasElapsed() {
050    return MathSharedStore.getTimestamp() - m_firstEdgeTimeSeconds >= m_windowTimeSeconds;
051  }
052
053  /**
054   * Applies the edge counter filter to the input stream.
055   *
056   * @param input The current value of the input stream.
057   * @return True if the required number of edges have occurred within the time window and the input
058   *     is currently true; false otherwise.
059   */
060  public boolean calculate(boolean input) {
061    boolean enoughEdges = m_currentCount >= m_requiredEdges;
062
063    boolean expired = hasElapsed() && !enoughEdges;
064    boolean activationEnded = !input && enoughEdges;
065
066    if (expired || activationEnded) {
067      m_currentCount = 0;
068    }
069
070    if (input && !m_lastInput) {
071      if (m_currentCount == 0) {
072        resetTimer(); // Start timer on first rising edge
073      }
074
075      m_currentCount++;
076    }
077
078    m_lastInput = input;
079
080    return input && m_currentCount >= m_requiredEdges;
081  }
082
083  /**
084   * Sets the time window duration.
085   *
086   * @param windowTime The maximum window of seconds in which all required edges must occur after
087   *     the first rising edge.
088   */
089  public void setWindowTime(double windowTime) {
090    m_windowTimeSeconds = windowTime;
091  }
092
093  /**
094   * Gets the time window duration.
095   *
096   * @return The maximum window of seconds in which all required edges must occur after the first
097   *     rising edge.
098   */
099  public double getWindowTime() {
100    return m_windowTimeSeconds;
101  }
102
103  /**
104   * Sets the required number of edges.
105   *
106   * @param requiredEdges The number of rising edges required before the output goes true.
107   */
108  public void setRequiredEdges(int requiredEdges) {
109    m_requiredEdges = requiredEdges;
110  }
111
112  /**
113   * Gets the required number of edges.
114   *
115   * @return The number of rising edges required before the output goes true.
116   */
117  public int getRequiredEdges() {
118    return m_requiredEdges;
119  }
120}