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}