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.DoublePublisher;
008import org.wpilib.networktables.NetworkTableInstance;
009import org.wpilib.networktables.PubSubOption;
010import org.wpilib.system.SystemServer;
011
012/** This class contains feedback and feedforward constants for an ExpansionHub motor. */
013public class ExpansionHubVelocityConstants {
014  private final DoublePublisher m_pPublisher;
015  private final DoublePublisher m_iPublisher;
016  private final DoublePublisher m_dPublisher;
017
018  private final DoublePublisher m_sPublisher;
019  private final DoublePublisher m_vPublisher;
020  private final DoublePublisher m_aPublisher;
021
022  ExpansionHubVelocityConstants(int hubNumber, int motorNumber) {
023    NetworkTableInstance systemServer = SystemServer.getSystemServer();
024
025    PubSubOption[] options =
026        new PubSubOption[] {
027          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
028        };
029
030    m_pPublisher =
031        systemServer
032            .getDoubleTopic(
033                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kp")
034            .publish(options);
035
036    m_iPublisher =
037        systemServer
038            .getDoubleTopic(
039                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ki")
040            .publish(options);
041
042    m_dPublisher =
043        systemServer
044            .getDoubleTopic(
045                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kd")
046            .publish(options);
047
048    m_sPublisher =
049        systemServer
050            .getDoubleTopic(
051                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ks")
052            .publish(options);
053
054    m_vPublisher =
055        systemServer
056            .getDoubleTopic(
057                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/kv")
058            .publish(options);
059
060    m_aPublisher =
061        systemServer
062            .getDoubleTopic(
063                "/rhsp/" + hubNumber + "/motor" + motorNumber + "/constants/velocity" + "/ka")
064            .publish(options);
065  }
066
067  /**
068   * Sets the PID Controller gain parameters.
069   *
070   * <p>Set the proportional, integral, and differential coefficients.
071   *
072   * @param p The proportional coefficient.
073   * @param i The integral coefficient.
074   * @param d The derivative coefficient.
075   * @return This object, for method chaining.
076   */
077  public ExpansionHubVelocityConstants setPID(double p, double i, double d) {
078    m_pPublisher.set(p);
079    m_iPublisher.set(i);
080    m_dPublisher.set(d);
081    return this;
082  }
083
084  /**
085   * Sets the feed forward gains to the specified values.
086   *
087   * <p>The units should be radians for angular systems and meters for linear systems.
088   *
089   * <p>The motor control period is 10ms
090   *
091   * @param s The static gain in volts.
092   * @param v The velocity gain in volts per unit per second.
093   * @param a The acceleration gain in volts per unit per second squared.
094   * @return This object, for method chaining.
095   */
096  public ExpansionHubVelocityConstants setFF(double s, double v, double a) {
097    m_sPublisher.set(s);
098    m_vPublisher.set(v);
099    m_aPublisher.set(a);
100    return this;
101  }
102
103  void close() {
104    m_pPublisher.close();
105    m_iPublisher.close();
106    m_dPublisher.close();
107    m_sPublisher.close();
108    m_vPublisher.close();
109    m_aPublisher.close();
110  }
111}