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 org.wpilib.hardware.expansionhub;
006
007import org.wpilib.networktables.BooleanPublisher;
008import org.wpilib.networktables.DoublePublisher;
009import org.wpilib.networktables.NetworkTableInstance;
010import org.wpilib.networktables.PubSubOption;
011import org.wpilib.system.SystemServer;
012
013/** This class contains PID constants for an ExpansionHub motor. */
014public class ExpansionHubPidConstants {
015  private final DoublePublisher m_pPublisher;
016  private final DoublePublisher m_iPublisher;
017  private final DoublePublisher m_dPublisher;
018  private final DoublePublisher m_sPublisher;
019  private final DoublePublisher m_vPublisher;
020  private final DoublePublisher m_aPublisher;
021
022  private final BooleanPublisher m_continuousPublisher;
023  private final DoublePublisher m_continuousMinimumPublisher;
024  private final DoublePublisher m_continuousMaximumPublisher;
025
026  ExpansionHubPidConstants(int hubNumber, int motorNumber, boolean isVelocityPid) {
027    NetworkTableInstance systemServer = SystemServer.getSystemServer();
028
029    PubSubOption[] options =
030        new PubSubOption[] {
031          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
032        };
033
034    String pidType = isVelocityPid ? "velocity" : "position";
035
036    m_pPublisher =
037        systemServer
038            .getDoubleTopic(
039                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kp")
040            .publish(options);
041
042    m_iPublisher =
043        systemServer
044            .getDoubleTopic(
045                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ki")
046            .publish(options);
047
048    m_dPublisher =
049        systemServer
050            .getDoubleTopic(
051                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kd")
052            .publish(options);
053
054    m_aPublisher =
055        systemServer
056            .getDoubleTopic(
057                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ka")
058            .publish(options);
059
060    m_vPublisher =
061        systemServer
062            .getDoubleTopic(
063                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/kv")
064            .publish(options);
065
066    m_sPublisher =
067        systemServer
068            .getDoubleTopic(
069                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/ks")
070            .publish(options);
071
072    m_continuousPublisher =
073        systemServer
074            .getBooleanTopic(
075                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/pid/" + pidType + "/continuous")
076            .publish(options);
077
078    m_continuousMinimumPublisher =
079        systemServer
080            .getDoubleTopic(
081                "/rhsp/"
082                    + hubNumber
083                    + "/motor"
084                    + motorNumber
085                    + "/pid/"
086                    + pidType
087                    + "/continuousMinimum")
088            .publish(options);
089
090    m_continuousMaximumPublisher =
091        systemServer
092            .getDoubleTopic(
093                "/rhsp/"
094                    + hubNumber
095                    + "/motor"
096                    + motorNumber
097                    + "/pid/"
098                    + pidType
099                    + "/continousMaximum")
100            .publish(options);
101  }
102
103  /**
104   * Sets the PID Controller gain parameters.
105   *
106   * <p>Set the proportional, integral, and differential coefficients.
107   *
108   * @param p The proportional coefficient.
109   * @param i The integral coefficient.
110   * @param d The derivative coefficient.
111   */
112  public void setPID(double p, double i, double d) {
113    m_pPublisher.set(p);
114    m_iPublisher.set(i);
115    m_dPublisher.set(d);
116  }
117
118  /**
119   * Sets the feed forward gains to the specified values.
120   *
121   * <p>The units should be radians for angular systems and meters for linear systems.
122   *
123   * <p>The PID control period is 10ms
124   *
125   * @param s The static gain in volts.
126   * @param v The velocity gain in V/(units/s).
127   * @param a The acceleration gain in V/(units/s²).
128   */
129  public void setFF(double s, double v, double a) {
130    m_sPublisher.set(s);
131    m_vPublisher.set(v);
132    m_aPublisher.set(a);
133  }
134
135  /**
136   * Enables continuous input.
137   *
138   * <p>Rather then using the max and min input range as constraints, it considers them to be the
139   * same point and automatically calculates the shortest route to the setpoint.
140   *
141   * @param minimumInput The minimum value expected from the input.
142   * @param maximumInput The maximum value expected from the input.
143   */
144  public void enableContinuousInput(double minimumInput, double maximumInput) {
145    m_continuousMaximumPublisher.set(maximumInput);
146    m_continuousMinimumPublisher.set(minimumInput);
147    m_continuousPublisher.set(true);
148  }
149
150  /** Disable continous input mode. */
151  public void disableContinuousInput() {
152    m_continuousPublisher.set(false);
153  }
154
155  void close() {
156    m_iPublisher.close();
157    m_dPublisher.close();
158    m_sPublisher.close();
159    m_vPublisher.close();
160    m_aPublisher.close();
161    m_continuousPublisher.close();
162    m_continuousMinimumPublisher.close();
163    m_continuousMaximumPublisher.close();
164  }
165}