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}