001/*----------------------------------------------------------------------------*/
002/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
003/* Open Source Software - may be modified and shared by FRC teams. The code   */
004/* must be accompanied by the FIRST BSD license file in the root directory of */
005/* the project.                                                               */
006/*----------------------------------------------------------------------------*/
007
008package edu.wpi.first.wpilibj;
009
010import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
011import edu.wpi.first.wpilibj.hal.HAL;
012import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
013
014/**
015 * Standard hobby style servo.
016 *
017 * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
018 * in the FIRST Kit of Parts in 2008.
019 */
020public class Servo extends PWM {
021  private static final double kMaxServoAngle = 180.0;
022  private static final double kMinServoAngle = 0.0;
023
024  protected static final double kDefaultMaxServoPWM = 2.4;
025  protected static final double kDefaultMinServoPWM = .6;
026
027  /**
028   * Constructor.<br>
029   *
030   * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br> By default
031   * {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
032   *
033   * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
034   *                the MXP port
035   */
036  public Servo(final int channel) {
037    super(channel);
038    setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
039    setPeriodMultiplier(PeriodMultiplier.k4X);
040
041    HAL.report(tResourceType.kResourceType_Servo, getChannel());
042    setName("Servo", getChannel());
043  }
044
045
046  /**
047   * Set the servo position.
048   *
049   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
050   *
051   * @param value Position from 0.0 to 1.0.
052   */
053  public void set(double value) {
054    setPosition(value);
055  }
056
057  /**
058   * Get the servo position.
059   *
060   * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
061   *
062   * @return Position from 0.0 to 1.0.
063   */
064  public double get() {
065    return getPosition();
066  }
067
068  /**
069   * Set the servo angle.
070   *
071   * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
072   * test).
073   *
074   * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that
075   * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of
076   * less than X result in an angle of X being set and angles of more than Y degrees result in an
077   * angle of Y being set.
078   *
079   * @param degrees The angle in degrees to set the servo.
080   */
081  public void setAngle(double degrees) {
082    if (degrees < kMinServoAngle) {
083      degrees = kMinServoAngle;
084    } else if (degrees > kMaxServoAngle) {
085      degrees = kMaxServoAngle;
086    }
087
088    setPosition(((degrees - kMinServoAngle)) / getServoAngleRange());
089  }
090
091  /**
092   * Get the servo angle.
093   *
094   * <p>Assume that the servo angle is linear with respect to the PWM value (big assumption, need to
095   * test).
096   *
097   * @return The angle in degrees to which the servo is set.
098   */
099  public double getAngle() {
100    return getPosition() * getServoAngleRange() + kMinServoAngle;
101  }
102
103  private double getServoAngleRange() {
104    return kMaxServoAngle - kMinServoAngle;
105  }
106
107  @Override
108  public void initSendable(SendableBuilder builder) {
109    builder.setSmartDashboardType("Servo");
110    builder.addDoubleProperty("Value", this::get, this::set);
111  }
112}