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.xrp;
006
007import edu.wpi.first.hal.SimDevice;
008import edu.wpi.first.hal.SimDevice.Direction;
009import edu.wpi.first.hal.SimDouble;
010import java.util.HashMap;
011import java.util.HashSet;
012
013/**
014 * XRPServo.
015 *
016 * <p>A SimDevice based servo
017 */
018public class XRPServo {
019  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
020  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
021
022  private static void checkDeviceAllocation(int deviceNum) {
023    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
024      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-7");
025    }
026
027    if (s_registeredDevices.contains(deviceNum)) {
028      throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
029    }
030
031    s_registeredDevices.add(deviceNum);
032  }
033
034  static {
035    s_simDeviceNameMap.put(4, "servo1");
036    s_simDeviceNameMap.put(5, "servo2");
037    s_simDeviceNameMap.put(6, "servo3");
038    s_simDeviceNameMap.put(7, "servo4");
039  }
040
041  private final SimDouble m_simPosition;
042
043  /**
044   * Constructs an XRPServo.
045   *
046   * @param deviceNum the servo channel
047   */
048  public XRPServo(int deviceNum) {
049    checkDeviceAllocation(deviceNum);
050
051    // We want this to appear on WS as type: "XRPServo", device: <servo name>
052    String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
053    SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
054
055    if (xrpServoSimDevice != null) {
056      xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
057      // This should mimic PWM position [0.0, 1.0]
058      m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
059    } else {
060      m_simPosition = null;
061    }
062  }
063
064  /**
065   * Set the servo angle.
066   *
067   * @param angleDegrees Desired angle in degrees
068   */
069  public void setAngle(double angleDegrees) {
070    if (angleDegrees < 0.0) {
071      angleDegrees = 0.0;
072    }
073
074    if (angleDegrees > 180.0) {
075      angleDegrees = 180.0;
076    }
077
078    double pos = angleDegrees / 180.0;
079
080    if (m_simPosition != null) {
081      m_simPosition.set(pos);
082    }
083  }
084
085  /**
086   * Get the servo angle.
087   *
088   * @return Current servo angle
089   */
090  public double getAngle() {
091    if (m_simPosition != null) {
092      return m_simPosition.get() * 180.0;
093    }
094
095    return 90.0;
096  }
097
098  /**
099   * Set the servo position.
100   *
101   * @param position Desired position (Between 0.0 and 1.0)
102   */
103  public void setPosition(double position) {
104    if (position < 0.0) {
105      position = 0.0;
106    }
107
108    if (position > 1.0) {
109      position = 1.0;
110    }
111
112    if (m_simPosition != null) {
113      m_simPosition.set(position);
114    }
115  }
116
117  /**
118   * Get the servo position.
119   *
120   * @return Current servo position
121   */
122  public double getPosition() {
123    if (m_simPosition != null) {
124      return m_simPosition.get();
125    }
126
127    return 0.5;
128  }
129}