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 org.wpilib.xrp;
006
007import java.util.HashMap;
008import java.util.HashSet;
009import org.wpilib.hardware.hal.SimBoolean;
010import org.wpilib.hardware.hal.SimDevice;
011import org.wpilib.hardware.hal.SimDevice.Direction;
012import org.wpilib.hardware.hal.SimDouble;
013import org.wpilib.hardware.motor.MotorController;
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_simVelocity;
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_simVelocity = xrpMotorSimDevice.createDouble("velocity", Direction.kOutput, 0.0);
063    } else {
064      m_simInverted = null;
065      m_simVelocity = null;
066    }
067  }
068
069  @Override
070  public void setDutyCycle(double velocity) {
071    if (m_simVelocity != null) {
072      boolean invert = false;
073      if (m_simInverted != null) {
074        invert = m_simInverted.get();
075      }
076
077      m_simVelocity.set(invert ? -velocity : velocity);
078    }
079  }
080
081  @Override
082  public double getDutyCycle() {
083    if (m_simVelocity != null) {
084      return m_simVelocity.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    setDutyCycle(0.0);
105  }
106}