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