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.Degrees;
008import static org.wpilib.units.Units.Microseconds;
009
010import org.wpilib.hardware.hal.util.AllocationException;
011import org.wpilib.networktables.BooleanPublisher;
012import org.wpilib.networktables.IntegerPublisher;
013import org.wpilib.networktables.NetworkTableInstance;
014import org.wpilib.networktables.PubSubOption;
015import org.wpilib.system.SystemServer;
016import org.wpilib.units.measure.Angle;
017import org.wpilib.units.measure.Time;
018
019/** This class controls a specific servo hooked up to an ExpansionHub. */
020public class ExpansionHubServo implements AutoCloseable {
021  private ExpansionHub m_hub;
022  private final int m_channel;
023
024  private boolean m_reversed;
025  private boolean m_continousMode;
026
027  private final IntegerPublisher m_pulseWidthPublisher;
028  private final IntegerPublisher m_framePeriodPublisher;
029  private final BooleanPublisher m_enabledPublisher;
030
031  private double m_maxServoAngle = 180.0;
032  private double m_minServoAngle;
033
034  private int m_minPwm = 600;
035  private int m_maxPwm = 2400;
036
037  /**
038   * Constructs a servo at the requested channel on a specific USB port.
039   *
040   * @param usbId The USB port ID the hub is connected to
041   * @param channel The servo channel
042   */
043  public ExpansionHubServo(int usbId, int channel) {
044    m_hub = new ExpansionHub(usbId);
045    m_channel = channel;
046
047    if (!m_hub.checkServoChannel(channel)) {
048      m_hub.close();
049      throw new IllegalArgumentException("Channel " + channel + " out of range");
050    }
051
052    if (!m_hub.checkAndReserveServo(channel)) {
053      m_hub.close();
054      throw new AllocationException("ExpansionHub Servo already allocated");
055    }
056
057    m_hub.reportUsage("ExHubServo[" + channel + "]", "ExHubServo");
058
059    NetworkTableInstance systemServer = SystemServer.getSystemServer();
060
061    PubSubOption[] options =
062        new PubSubOption[] {
063          PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES, PubSubOption.periodic(0.005)
064        };
065
066    m_pulseWidthPublisher =
067        systemServer
068            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/pulseWidth")
069            .publish(options);
070
071    m_pulseWidthPublisher.set(1500);
072
073    m_framePeriodPublisher =
074        systemServer
075            .getIntegerTopic("/rhsp/" + usbId + "/servo" + channel + "/framePeriod")
076            .publish(options);
077
078    m_framePeriodPublisher.set(20000);
079
080    m_enabledPublisher =
081        systemServer
082            .getBooleanTopic("/rhsp/" + usbId + "/servo" + channel + "/enabled")
083            .publish(options);
084  }
085
086  /**
087   * Set the servo position.
088   *
089   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. If
090   * continuous rotation mode is enabled, the range is -1.0 to 1.0.
091   *
092   * @param value Position from 0.0 to 1.0 (-1 to 1 in CR mode).
093   */
094  public void set(double value) {
095    if (m_continousMode) {
096      value = Math.clamp(value, -1.0, 1.0);
097      value = (value + 1.0) / 2.0;
098    }
099
100    value = Math.clamp(value, 0.0, 1.0);
101
102    if (m_reversed) {
103      value = 1.0 - value;
104    }
105
106    int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm);
107
108    setEnabled(true);
109    m_pulseWidthPublisher.set(rawValue);
110  }
111
112  /**
113   * Sets the servo angle
114   *
115   * <p>Servo angles range defaults to 0 to 180 degrees, but can be changed with setAngleRange().
116   *
117   * @param angle Position in angle units. Will be scaled between the current angle range.
118   */
119  public void setAngle(Angle angle) {
120    double dAngle = angle.in(Degrees);
121    if (dAngle < m_minServoAngle) {
122      dAngle = m_minServoAngle;
123    } else if (dAngle > m_maxServoAngle) {
124      dAngle = m_maxServoAngle;
125    }
126
127    set((dAngle - m_minServoAngle) / getServoAngleRange());
128  }
129
130  private double getFullRangeScaleFactor() {
131    return m_maxPwm - m_minPwm;
132  }
133
134  private double getServoAngleRange() {
135    return m_maxServoAngle - m_minServoAngle;
136  }
137
138  /**
139   * Sets the raw pulse width output on the servo.
140   *
141   * @param pulseWidth Pulse width
142   */
143  public void setPulseWidth(Time pulseWidth) {
144    setEnabled(true);
145    m_pulseWidthPublisher.set((long) pulseWidth.in(Microseconds));
146  }
147
148  /**
149   * Sets if the servo output is enabled or not. Defaults to false.
150   *
151   * @param enabled True to enable, false to disable
152   */
153  public void setEnabled(boolean enabled) {
154    m_enabledPublisher.set(enabled);
155  }
156
157  /**
158   * Sets the frame period for the servo. Defaults to 20ms.
159   *
160   * @param framePeriod The frame period
161   */
162  public void setFramePeriod(Time framePeriod) {
163    m_framePeriodPublisher.set((long) framePeriod.in(Microseconds));
164  }
165
166  /**
167   * Gets if the underlying ExpansionHub is connected.
168   *
169   * @return True if hub is connected, otherwise false
170   */
171  public boolean isHubConnected() {
172    return m_hub.isHubConnected();
173  }
174
175  /**
176   * Sets whether the servo is reversed.
177   *
178   * <p>This will reverse both set() and setAngle().
179   *
180   * @param reversed True to reverse, false for normal
181   */
182  public void setReversed(boolean reversed) {
183    m_reversed = reversed;
184  }
185
186  /**
187   * Sets the PWM range for the servo. By default, this is 600 to 2400 microseconds.
188   *
189   * <p>Maximum must be greater than minimum.
190   *
191   * @param minPwm Minimum PWM
192   * @param maxPwm Maximum PWM
193   */
194  public void setPWMRange(int minPwm, int maxPwm) {
195    if (maxPwm <= minPwm) {
196      throw new IllegalArgumentException("Maximum PWM must be greater than minimum PWM");
197    }
198    m_minPwm = minPwm;
199    m_maxPwm = maxPwm;
200  }
201
202  /**
203   * Sets the angle range for the setAngle call. By default, this is 0 to 180 degrees.
204   *
205   * <p>Maximum angle must be greater than minimum angle.
206   *
207   * @param minAngle Minimum angle
208   * @param maxAngle Maximum angle
209   */
210  public void setAngleRange(double minAngle, double maxAngle) {
211    if (maxAngle <= minAngle) {
212      throw new IllegalArgumentException("Maximum angle must be greater than minimum angle");
213    }
214    m_minServoAngle = minAngle;
215    m_maxServoAngle = maxAngle;
216  }
217
218  /**
219   * Enables or disables continuous rotation mode.
220   *
221   * <p>In continuous rotation mode, the servo will interpret Set() commands to between -1.0 and
222   * 1.0, instead of 0.0 to 1.0.
223   *
224   * @param enable True to enable continuous rotation mode, false to disable
225   */
226  public void setContinousRotationMode(boolean enable) {
227    m_continousMode = enable;
228  }
229
230  /** Closes a servo so another instance can be constructed. */
231  @Override
232  public void close() {
233    m_hub.unreserveServo(m_channel);
234    m_hub.close();
235    m_hub = null;
236
237    m_pulseWidthPublisher.close();
238    m_framePeriodPublisher.close();
239    m_enabledPublisher.close();
240  }
241}