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 static org.wpilib.units.Units.Microseconds;
008
009import org.wpilib.hardware.hal.util.AllocationException;
010import org.wpilib.networktables.BooleanPublisher;
011import org.wpilib.networktables.IntegerPublisher;
012import org.wpilib.networktables.NetworkTableInstance;
013import org.wpilib.networktables.PubSubOption;
014import org.wpilib.system.SystemServer;
015import org.wpilib.units.measure.Time;
016
017/**
018 * This class controls a specific servo in continuous rotation mode hooked up to an ExpansionHub.
019 */
020public class ExpansionHubCRServo implements AutoCloseable {
021  private ExpansionHub m_hub;
022  private final int m_channel;
023
024  private boolean m_reversed;
025
026  private final IntegerPublisher m_pulseWidthPublisher;
027  private final IntegerPublisher m_framePeriodPublisher;
028  private final BooleanPublisher m_enabledPublisher;
029
030  private int m_minPwm = 600;
031  private int m_maxPwm = 2400;
032
033  /**
034   * Constructs a continuous rotation servo at the requested channel on a specific USB port.
035   *
036   * @param usbId The USB port ID the hub is connected to
037   * @param channel The servo channel
038   */
039  public ExpansionHubCRServo(int usbId, int channel) {
040    m_hub = new ExpansionHub(usbId);
041    m_channel = channel;
042
043    if (!m_hub.checkServoChannel(channel)) {
044      m_hub.close();
045      throw new IllegalArgumentException("Channel " + channel + " out of range");
046    }
047
048    if (!m_hub.checkAndReserveServo(channel)) {
049      m_hub.close();
050      throw new AllocationException("ExpansionHub CR Servo already allocated");
051    }
052
053    m_hub.reportUsage("ExHubCRServo[" + channel + "]", "ExHubCRServo");
054
055    NetworkTableInstance systemServer = SystemServer.getSystemServer();
056
057    PubSubOption[] options =
058        new PubSubOption[] {
059          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
060        };
061
062    m_pulseWidthPublisher =
063        systemServer
064            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/pulseWidth")
065            .publish(options);
066
067    m_pulseWidthPublisher.set(1500);
068
069    m_framePeriodPublisher =
070        systemServer
071            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/framePeriod")
072            .publish(options);
073
074    m_framePeriodPublisher.set(20000);
075
076    m_enabledPublisher =
077        systemServer
078            .getBooleanTopic("/rhsp/" + usbId + "/servo" + channel + "/enabled")
079            .publish(options);
080  }
081
082  /** Closes a servo so another instance can be constructed. */
083  @Override
084  public void close() {
085    m_hub.unreserveServo(m_channel);
086    m_hub.close();
087    m_hub = null;
088
089    m_pulseWidthPublisher.close();
090    m_framePeriodPublisher.close();
091    m_enabledPublisher.close();
092  }
093
094  /**
095   * Set the servo throttle.
096   *
097   * <p>Throttle values range from -1.0 to 1.0 corresponding to full reverse to full forward.
098   *
099   * @param value Throttle from -1.0 to 1.0.
100   */
101  public void setThrottle(double value) {
102    value = Math.clamp(value, -1.0, 1.0);
103    value = (value + 1.0) / 2.0;
104
105    if (m_reversed) {
106      value = 1.0 - value;
107    }
108
109    int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm);
110
111    setPulseWidth(Microseconds.of(rawValue));
112  }
113
114  /**
115   * Sets the raw pulse width output on the servo.
116   *
117   * @param pulseWidth Pulse width
118   */
119  public void setPulseWidth(Time pulseWidth) {
120    setEnabled(true);
121    m_pulseWidthPublisher.set((long) pulseWidth.in(Microseconds));
122  }
123
124  /**
125   * Sets if the servo output is enabled or not. Defaults to false.
126   *
127   * @param enabled True to enable, false to disable
128   */
129  public void setEnabled(boolean enabled) {
130    m_enabledPublisher.set(enabled);
131  }
132
133  /**
134   * Sets the frame period for the servo. Defaults to 20ms.
135   *
136   * @param framePeriod The frame period
137   */
138  public void setFramePeriod(Time framePeriod) {
139    m_framePeriodPublisher.set((long) framePeriod.in(Microseconds));
140  }
141
142  /**
143   * Gets if the underlying ExpansionHub is connected.
144   *
145   * @return True if hub is connected, otherwise false
146   */
147  public boolean isHubConnected() {
148    return m_hub.isHubConnected();
149  }
150
151  /**
152   * Sets whether the servo is reversed.
153   *
154   * <p>This will reverse setThrottle().
155   *
156   * @param reversed True to reverse, false for normal
157   */
158  public void setReversed(boolean reversed) {
159    m_reversed = reversed;
160  }
161
162  /**
163   * Sets the PWM range for the servo. By default, this is 600 to 2400 microseconds.
164   *
165   * <p>Maximum must be greater than minimum.
166   *
167   * @param minPwm Minimum PWM
168   * @param maxPwm Maximum PWM
169   */
170  public void setPWMRange(int minPwm, int maxPwm) {
171    if (maxPwm <= minPwm) {
172      throw new IllegalArgumentException("Maximum PWM must be greater than minimum PWM");
173    }
174    m_minPwm = minPwm;
175    m_maxPwm = maxPwm;
176  }
177
178  private double getFullRangeScaleFactor() {
179    return m_maxPwm - m_minPwm;
180  }
181}