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.FRCNetComm.tResourceType;
008import edu.wpi.first.hal.HAL;
009import edu.wpi.first.util.sendable.SendableBuilder;
010import edu.wpi.first.util.sendable.SendableRegistry;
011
012/**
013 * Standard hobby style servo.
014 *
015 * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
016 * in the FIRST Kit of Parts in 2008.
017 */
018public class Servo extends PWM {
019  private static final double kMaxServoAngle = 180.0;
020  private static final double kMinServoAngle = 0.0;
021
022  protected static final int kDefaultMaxServoPWM = 2400;
023  protected static final int kDefaultMinServoPWM = 600;
024
025  /**
026   * Constructor.<br>
027   *
028   * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br>
029   * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
030   *
031   * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
032   *     the MXP port
033   */
034  public Servo(final int channel) {
035    super(channel);
036    setBoundsMicroseconds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
037    setPeriodMultiplier(PeriodMultiplier.k4X);
038
039    HAL.report(tResourceType.kResourceType_Servo, getChannel() + 1);
040    SendableRegistry.setName(this, "Servo", getChannel());
041  }
042
043  /**
044   * Set the servo position.
045   *
046   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
047   *
048   * @param value Position from 0.0 to 1.0.
049   */
050  public void set(double value) {
051    setPosition(value);
052  }
053
054  /**
055   * Get the servo position.
056   *
057   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
058   * This returns the commanded position, not the position that the servo is actually at, as the
059   * servo does not report its own position.
060   *
061   * @return Position from 0.0 to 1.0.
062   */
063  public double get() {
064    return getPosition();
065  }
066
067  /**
068   * Set the servo angle.
069   *
070   * <p>The angles are based on the HS-322HD Servo, and have a range of 0 to 180 degrees.
071   *
072   * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that
073   * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of
074   * less than X result in an angle of X being set and angles of more than Y degrees result in an
075   * angle of Y being set.
076   *
077   * @param degrees The angle in degrees to set the servo.
078   */
079  public void setAngle(double degrees) {
080    if (degrees < kMinServoAngle) {
081      degrees = kMinServoAngle;
082    } else if (degrees > kMaxServoAngle) {
083      degrees = kMaxServoAngle;
084    }
085
086    setPosition(((degrees - kMinServoAngle)) / getServoAngleRange());
087  }
088
089  /**
090   * Get the servo angle.
091   *
092   * <p>This returns the commanded angle, not the angle that the servo is actually at, as the servo
093   * does not report its own angle.
094   *
095   * @return The angle in degrees to which the servo is set.
096   */
097  public double getAngle() {
098    return getPosition() * getServoAngleRange() + kMinServoAngle;
099  }
100
101  private double getServoAngleRange() {
102    return kMaxServoAngle - kMinServoAngle;
103  }
104
105  @Override
106  public void initSendable(SendableBuilder builder) {
107    builder.setSmartDashboardType("Servo");
108    builder.addDoubleProperty("Value", this::get, this::set);
109  }
110}