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.SimBoolean; 008import edu.wpi.first.hal.SimDevice; 009import edu.wpi.first.hal.SimDevice.Direction; 010import edu.wpi.first.hal.SimDouble; 011import edu.wpi.first.wpilibj.motorcontrol.MotorController; 012import java.util.HashMap; 013import java.util.HashSet; 014 015/** 016 * XRPMotor. 017 * 018 * <p>A SimDevice based motor controller representing the motors on an XRP robot 019 */ 020@SuppressWarnings("removal") 021public class XRPMotor implements MotorController { 022 private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>(); 023 private static HashSet<Integer> s_registeredDevices = new HashSet<>(); 024 025 private static void checkDeviceAllocation(int deviceNum) { 026 if (!s_simDeviceNameMap.containsKey(deviceNum)) { 027 throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3"); 028 } 029 030 if (s_registeredDevices.contains(deviceNum)) { 031 throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated"); 032 } 033 034 s_registeredDevices.add(deviceNum); 035 } 036 037 static { 038 s_simDeviceNameMap.put(0, "motorL"); 039 s_simDeviceNameMap.put(1, "motorR"); 040 s_simDeviceNameMap.put(2, "motor3"); 041 s_simDeviceNameMap.put(3, "motor4"); 042 } 043 044 private final SimDouble m_simSpeed; 045 private final SimBoolean m_simInverted; 046 047 /** 048 * Constructs an XRPMotor. 049 * 050 * @param deviceNum the motor channel 051 */ 052 public XRPMotor(int deviceNum) { 053 checkDeviceAllocation(deviceNum); 054 055 // We want this to appear on the WS messages as type: "XRPMotor", device: <motor name> 056 String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum); 057 SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName); 058 059 if (xrpMotorSimDevice != null) { 060 xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true); 061 m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false); 062 m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0); 063 } else { 064 m_simInverted = null; 065 m_simSpeed = null; 066 } 067 } 068 069 @Override 070 public void set(double speed) { 071 if (m_simSpeed != null) { 072 boolean invert = false; 073 if (m_simInverted != null) { 074 invert = m_simInverted.get(); 075 } 076 077 m_simSpeed.set(invert ? -speed : speed); 078 } 079 } 080 081 @Override 082 public double get() { 083 if (m_simSpeed != null) { 084 return m_simSpeed.get(); 085 } 086 087 return 0.0; 088 } 089 090 @Override 091 public void setInverted(boolean isInverted) { 092 if (m_simInverted != null) { 093 m_simInverted.set(isInverted); 094 } 095 } 096 097 @Override 098 public boolean getInverted() { 099 return m_simInverted != null && m_simInverted.get(); 100 } 101 102 @Override 103 public void disable() { 104 set(0.0); 105 } 106 107 @Override 108 public void stopMotor() { 109 set(0.0); 110 } 111}