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 edu.wpi.first.wpilibj;
006
007import edu.wpi.first.hal.HAL;
008import edu.wpi.first.hal.SimDevice;
009import edu.wpi.first.hal.SimDevice.Direction;
010import edu.wpi.first.hal.SimDouble;
011import edu.wpi.first.math.MathUtil;
012import edu.wpi.first.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
015import edu.wpi.first.wpilibj.PWM.OutputPeriod;
016
017/**
018 * Standard hobby style servo.
019 *
020 * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
021 * in the FIRST Kit of Parts in 2008.
022 */
023public class Servo implements Sendable, AutoCloseable {
024  private static final double kMaxServoAngle = 180.0;
025  private static final double kMinServoAngle = 0.0;
026
027  private static final int kDefaultMaxServoPWM = 2400;
028  private static final int kDefaultMinServoPWM = 600;
029
030  private final PWM m_pwm;
031
032  private SimDevice m_simDevice;
033  private SimDouble m_simPosition;
034
035  private static final int m_minPwm = kDefaultMinServoPWM;
036  private static final int m_maxPwm = kDefaultMaxServoPWM;
037
038  /**
039   * Constructor.
040   *
041   * <p>By default, {@value #kDefaultMaxServoPWM} ms is used as the max PWM value and {@value
042   * #kDefaultMinServoPWM} ms is used as the minPWM value.
043   *
044   * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
045   *     the MXP port
046   */
047  @SuppressWarnings("this-escape")
048  public Servo(final int channel) {
049    m_pwm = new PWM(channel, false);
050    SendableRegistry.add(this, "Servo", channel);
051
052    m_pwm.setOutputPeriod(OutputPeriod.k20Ms);
053
054    HAL.reportUsage("IO", channel, "Servo");
055
056    m_simDevice = SimDevice.create("Servo", channel);
057    if (m_simDevice != null) {
058      m_simPosition = m_simDevice.createDouble("Position", Direction.kOutput, 0.0);
059      m_pwm.setSimDevice(m_simDevice);
060    }
061  }
062
063  /** Free the resource associated with the PWM channel and set the value to 0. */
064  @Override
065  public void close() {
066    SendableRegistry.remove(this);
067    m_pwm.close();
068
069    if (m_simDevice != null) {
070      m_simDevice.close();
071      m_simDevice = null;
072      m_simPosition = null;
073    }
074  }
075
076  /**
077   * Set the servo position.
078   *
079   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
080   *
081   * @param value Position from 0.0 to 1.0.
082   */
083  public void set(double value) {
084    value = MathUtil.clamp(value, 0.0, 1.0);
085
086    if (m_simPosition != null) {
087      m_simPosition.set(value);
088    }
089
090    int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm);
091
092    m_pwm.setPulseTimeMicroseconds(rawValue);
093  }
094
095  /**
096   * Get the servo position.
097   *
098   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
099   * This returns the commanded position, not the position that the servo is actually at, as the
100   * servo does not report its own position.
101   *
102   * @return Position from 0.0 to 1.0.
103   */
104  public double get() {
105    int rawValue = m_pwm.getPulseTimeMicroseconds();
106
107    if (rawValue < m_minPwm) {
108      return 0.0;
109    } else if (rawValue > m_maxPwm) {
110      return 1.0;
111    }
112    return (rawValue - m_minPwm) / getFullRangeScaleFactor();
113  }
114
115  /**
116   * Set the servo angle.
117   *
118   * <p>The angles are based on the HS-322HD Servo, and have a range of 0 to 180 degrees.
119   *
120   * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that
121   * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of
122   * less than X result in an angle of X being set and angles of more than Y degrees result in an
123   * angle of Y being set.
124   *
125   * @param degrees The angle in degrees to set the servo.
126   */
127  public void setAngle(double degrees) {
128    if (degrees < kMinServoAngle) {
129      degrees = kMinServoAngle;
130    } else if (degrees > kMaxServoAngle) {
131      degrees = kMaxServoAngle;
132    }
133
134    set((degrees - kMinServoAngle) / getServoAngleRange());
135  }
136
137  /**
138   * Get the servo angle.
139   *
140   * <p>This returns the commanded angle, not the angle that the servo is actually at, as the servo
141   * does not report its own angle.
142   *
143   * @return The angle in degrees to which the servo is set.
144   */
145  public double getAngle() {
146    return get() * getServoAngleRange() + kMinServoAngle;
147  }
148
149  /**
150   * Gets the backing PWM handle.
151   *
152   * @return The pwm handle.
153   */
154  public int getPwmHandle() {
155    return m_pwm.getHandle();
156  }
157
158  /**
159   * Gets the PWM channel number.
160   *
161   * @return The channel number.
162   */
163  public int getChannel() {
164    return m_pwm.getChannel();
165  }
166
167  private double getFullRangeScaleFactor() {
168    return m_maxPwm - m_minPwm;
169  }
170
171  private double getServoAngleRange() {
172    return kMaxServoAngle - kMinServoAngle;
173  }
174
175  @Override
176  public void initSendable(SendableBuilder builder) {
177    builder.setSmartDashboardType("Servo");
178    builder.addDoubleProperty("Value", this::get, this::set);
179  }
180}