Java 类edu.wpi.first.wpilibj.hal.HAL 实例源码

/**
 * Create an instance of a Serial Port class.
 *
 * @param baudRate The baud rate to configure the serial port.
 * @param port     The Serial port to use
 * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
 * @param parity   Select the type of parity checking to use.
 * @param stopBits The number of stop bits to use as defined by the enum StopBits.
 */
public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity,
                  StopBits stopBits) {
  m_port = (byte) port.value;

  SerialPortJNI.serialInitializePort(m_port);
  SerialPortJNI.serialSetBaudRate(m_port, baudRate);
  SerialPortJNI.serialSetDataBits(m_port, (byte) dataBits);
  SerialPortJNI.serialSetParity(m_port, (byte) parity.value);
  SerialPortJNI.serialSetStopBits(m_port, (byte) stopBits.value);

  // Set the default read buffer size to 1 to return bytes immediately
  setReadBufferSize(1);

  // Set the default timeout to 5 seconds.
  setTimeout(5.0);

  // Don't wait until the buffer is full to transmit.
  setWriteBufferMode(WriteBufferMode.kFlushOnAccess);

  disableTermination();

  HAL.report(tResourceType.kResourceType_SerialPort, 0);
}

/**
 * Create an instance of a counter with the given mode.
 */
public Counter(final Mode mode) {
  ByteBuffer index = ByteBuffer.allocateDirect(4);
  // set the byte order
  index.order(ByteOrder.LITTLE_ENDIAN);
  m_counter = CounterJNI.initializeCounter(mode.value, index.asIntBuffer());
  m_index = index.asIntBuffer().get(0);

  m_allocatedUpSource = false;
  m_allocatedDownSource = false;
  m_upSource = null;
  m_downSource = null;

  setMaxPeriod(.5);

  HAL.report(tResourceType.kResourceType_Counter, m_index, mode.value);
}

/**
 * Constructor.
 *
 * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
 *                the MXP port
 */
public Jaguar(final int channel) {
  super(channel);

  /*
   * Input profile defined by Luminary Micro.
   *
   * Full reverse ranges from 0.671325ms to 0.6972211ms Proportional reverse
   * ranges from 0.6972211ms to 1.4482078ms Neutral ranges from 1.4482078ms to
   * 1.5517922ms Proportional forward ranges from 1.5517922ms to 2.3027789ms
   * Full forward ranges from 2.3027789ms to 2.328675ms
   */
  setBounds(2.31, 1.55, 1.507, 1.454, .697);
  setPeriodMultiplier(PeriodMultiplier.k1X);
  setSpeed(0.0);
  setZeroLatch();

  HAL.report(tResourceType.kResourceType_Jaguar, getChannel());
  LiveWindow.addActuator("Jaguar", getChannel(), this);
}

/**
 * Constructor.
 *
 * @param port The SPI port that the gyro is connected to
 */
public ADXRS450_Gyro(SPI.Port port) {
  m_spi = new SPI(port);
  m_spi.setClockRate(3000000);
  m_spi.setMSBFirst();
  m_spi.setSampleDataOnRising();
  m_spi.setClockActiveHigh();
  m_spi.setChipSelectActiveLow();

  // Validate the part ID
  if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
    m_spi.free();
    m_spi = null;
    DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
        false);
    return;
  }

  m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
      true, true);

  calibrate();

  HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
  LiveWindow.addSensor("ADXRS450_Gyro", port.value, this);
}

/**
 * Initialize the Ultrasonic Sensor. This is the common code that initializes the ultrasonic
 * sensor given that there are two digital I/O channels allocated. If the system was running in
 * automatic mode (round robin) when the new sensor is added, it is stopped, the sensor is added,
 * then automatic mode is restored.
 */
private synchronized void initialize() {
  if (m_task == null) {
    m_task = new UltrasonicChecker();
  }
  final boolean originalMode = m_automaticEnabled;
  setAutomaticMode(false); // kill task when adding a new sensor
  m_nextSensor = m_firstSensor;
  m_firstSensor = this;

  m_counter = new Counter(m_echoChannel); // set up counter for this
  // sensor
  m_counter.setMaxPeriod(1.0);
  m_counter.setSemiPeriodMode(true);
  m_counter.reset();
  m_enabled = true; // make it available for round robin scheduling
  setAutomaticMode(originalMode);

  m_instances++;
  HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
  LiveWindow.addSensor("Ultrasonic", m_echoChannel.getChannel(), this);
}

/**
 * DriverStation constructor.
 *
 * <p>The single DriverStation instance is created statically with the instance static member
 * variable.
 */
private DriverStation() {
  m_dataMutex = new ReentrantLock();
  m_dataCond = m_dataMutex.newCondition();
  m_joystickMutex = new Object();
  m_newControlData = new AtomicBoolean(false);
  for (int i = 0; i < kJoystickPorts; i++) {
    m_joystickButtons[i] = new HALJoystickButtons();
    m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
    m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);

    m_joystickButtonsCache[i] = new HALJoystickButtons();
    m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
    m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
  }

  m_controlWordMutex = new Object();
  m_controlWordCache = new ControlWord();
  m_lastControlWordUpdate = 0;

  m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
  m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);

  m_thread.start();
}

private static void reportErrorImpl(boolean isError, int code, String error, boolean
    printTrace) {
  StackTraceElement[] traces = Thread.currentThread().getStackTrace();
  String locString;
  if (traces.length > 3) {
    locString = traces[3].toString();
  } else {
    locString = "";
  }
  boolean haveLoc = false;
  String traceString = " at ";
  for (int i = 3; i < traces.length; i++) {
    String loc = traces[i].toString();
    traceString += loc + "\n";
    // get first user function
    if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
      locString = loc;
      haveLoc = true;
    }
  }
  HAL.sendError(isError, code, false, error, locString, printTrace ? traceString : "", true);
}

/**
 * Get the value of the axis on a joystick. This depends on the mapping of the joystick connected
 * to the specified port.
 *
 * @param stick The joystick to read.
 * @param axis  The analog axis value to read from the joystick.
 * @return The value of the axis on the joystick.
 */
public double getStickAxis(int stick, int axis) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
    throw new RuntimeException("Joystick axis is out of range");
  }

  boolean error = false;
  double retVal = 0.0;
  synchronized (m_joystickMutex) {
    if (axis >= m_joystickAxes[stick].m_count) {
      // set error
      error = true;
      retVal = 0.0;
    } else {
      retVal =  m_joystickAxes[stick].m_axes[axis];
    }
  }
  if (error) {
    reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
        + " not available, check if controller is plugged in");
  }
  return retVal;
}

/**
 * Get the state of a POV on the joystick.
 *
 * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
 */
public int getStickPOV(int stick, int pov) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
    throw new RuntimeException("Joystick POV is out of range");
  }
  boolean error = false;
  int retVal = -1;
  synchronized (m_joystickMutex) {
    if (pov >= m_joystickPOVs[stick].m_count) {
      error = true;
      retVal = -1;
    } else {
      retVal = m_joystickPOVs[stick].m_povs[pov];
    }
  }
  if (error) {
    reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
        + " not available, check if controller is plugged in");
  }
  return retVal;
}

/**
 * Gets the value of isXbox on a joystick.
 *
 * @param stick The joystick port number
 * @return A boolean that returns the value of isXbox
 */
public boolean getJoystickIsXbox(int stick) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  boolean error = false;
  boolean retVal = false;
  synchronized (m_joystickMutex) {
    // TODO: Remove this when calling for descriptor on empty stick no longer
    // crashes
    if (1 > m_joystickButtons[stick].m_count && 1 > m_joystickAxes[stick].m_count) {
      error = true;
      retVal = false;
    } else if (HAL.getJoystickIsXbox((byte) stick) == 1) {
      retVal = true;
    }
  }
  if (error) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
  }
  return retVal;
}

/**
 * Gets the value of type on a joystick.
 *
 * @param stick The joystick port number
 * @return The value of type
 */
public int getJoystickType(int stick) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  boolean error = false;
  int retVal = -1;
  synchronized (m_joystickMutex) {
    // TODO: Remove this when calling for descriptor on empty stick no longer
    // crashes
    if (1 > m_joystickButtons[stick].m_count && 1 > m_joystickAxes[stick].m_count) {
      error = true;
      retVal = -1;
    } else {
      retVal = HAL.getJoystickType((byte) stick);
    }
  }
  if (error) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
  }
  return retVal;
}

/**
 * Gets the name of the joystick at a port.
 *
 * @param stick The joystick port number
 * @return The value of name
 */
public String getJoystickName(int stick) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }
  boolean error = false;
  String retVal = "";
  synchronized (m_joystickMutex) {
    // TODO: Remove this when calling for descriptor on empty stick no longer
    // crashes
    if (1 > m_joystickButtons[stick].m_count && 1 > m_joystickAxes[stick].m_count) {
      error = true;
      retVal = "";
    } else {
      retVal = HAL.getJoystickName((byte) stick);
    }
  }
  if (error) {
    reportJoystickUnpluggedWarning("Joystick on port " + stick
        + " not available, check if controller is plugged in");
  }
  return retVal;
}

/**
 * Get the current alliance from the FMS.
 *
 * @return the current alliance
 */
public Alliance getAlliance() {
  AllianceStationID allianceStationID = HAL.getAllianceStation();
  if (allianceStationID == null) {
    return Alliance.Invalid;
  }

  switch (allianceStationID) {
    case Red1:
    case Red2:
    case Red3:
      return Alliance.Red;

    case Blue1:
    case Blue2:
    case Blue3:
      return Alliance.Blue;

    default:
      return Alliance.Invalid;
  }
}

/**
 * Gets the location of the team's driver station controls.
 *
 * @return the location of the team's driver station controls: 1, 2, or 3
 */
public int getLocation() {
  AllianceStationID allianceStationID = HAL.getAllianceStation();
  if (allianceStationID == null) {
    return 0;
  }
  switch (allianceStationID) {
    case Red1:
    case Blue1:
      return 1;

    case Red2:
    case Blue2:
      return 2;

    case Blue3:
    case Red3:
      return 3;

    default:
      return 0;
  }
}

/**
 * Set SPI bus parameters, bring device out of sleep and set format.
 *
 * @param range The range (+ or -) that the accelerometer will measure.
 */
private void init(Range range) {
  m_spi.setClockRate(500000);
  m_spi.setMSBFirst();
  m_spi.setSampleDataOnFalling();
  m_spi.setClockActiveLow();
  m_spi.setChipSelectActiveHigh();

  // Turn on the measurements
  byte[] commands = new byte[2];
  commands[0] = kPowerCtlRegister;
  commands[1] = kPowerCtl_Measure;
  m_spi.write(commands, 2);

  setRange(range);

  HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
}

/**
 * Common relay initialization method. This code is common to all Relay constructors and
 * initializes the relay and reserves all resources that need to be locked. Initially the relay is
 * set to both lines at 0v.
 */
private void initRelay() {
  SensorBase.checkRelayChannel(m_channel);

  int portHandle = RelayJNI.getPort((byte)m_channel);
  if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
    m_forwardHandle = RelayJNI.initializeRelayPort(portHandle, true);
    HAL.report(tResourceType.kResourceType_Relay, m_channel);
  }
  if (m_direction == Direction.kBoth || m_direction == Direction.kReverse) {
    m_reverseHandle = RelayJNI.initializeRelayPort(portHandle, false);
    HAL.report(tResourceType.kResourceType_Relay, m_channel + 128);
  }

  m_safetyHelper = new MotorSafetyHelper(this);
  m_safetyHelper.setSafetyEnabled(false);

  LiveWindow.addActuator("Relay", m_channel, this);
}

public void startSimulation() throws InstantiationException, IllegalAccessException, ClassNotFoundException
{
    RobotBase.initializeHardwareConfiguration();
    loadConfig(sPROPERTIES_FILE);

    // Do all of the stuff that
    HAL.setWaitTime(.02);

    createSimulator();
    createRobot();

    Thread robotThread = new Thread(createRobotThread(), "RobotThread");
    Runnable guiThread = createGuiThread();

    robotThread.start();
    SwingUtilities.invokeLater(guiThread);
}

public CameraSimulator()
{
    mLoopsBetweenUpdates = (int) Math.ceil((1.0 / sFRAMES_PER_SECOND) / HAL.getCycleTime());
    mLoopsStale = (int) Math.ceil((sLATENCY_MS * 1e-3) / HAL.getCycleTime());
    mRobotPositionHistory = new TreeMap<>();
    mLoopCtr = 0;

    mPegs = new ArrayList<>();
    mPegs.add(new PegCoordinate("Red Loading", -45, -200, 60 + 180, -1000, -1000, -42, -125));
    mPegs.add(new PegCoordinate("Red Center", 0, -230, 0 + 180, -100, -1000, 100, -240));
    mPegs.add(new PegCoordinate("Red Boiler", 45, -200, -60 + 180, 42, -1000, 1000, -125));
    mPegs.add(new PegCoordinate("Blue Loading", -45, 200, 60, -1000, 125, -42, 1000));
    mPegs.add(new PegCoordinate("Blue Center", 0, 230, 0, -1000, 230, 240, 1000));
    mPegs.add(new PegCoordinate("Blue Boiler", 45, 200, -60, 42, 125, 1000, 1000));

    mMockAppConnection = new MockAppConnection();
    mMockAppConnection.start();
}

/**
 * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
 * right rumble.
 *
 * @param type  Which rumble value to set
 * @param value The normalized value (0 to 1) to set the rumble to
 */
public void setRumble(RumbleType type, float value)
{
    if(value < 0.0F) { value = 0.0F; }
    else if(value > 1.0F) { value = 1.0F; }

    if(type == RumbleType.kLeftRumble) { m_leftRumble = (short) (value * 65535); }
    else { m_rightRumble = (short) (value * 65535); }
    HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
}

/**
 * Constructor
 */
public WPI_TalonSRXF(int deviceNumber)
{
    super(deviceNumber);
    HAL.report(66, deviceNumber + 1);
    m_description = "Talon SRX " + deviceNumber;
    /* prep motor safety */
    m_safetyHelper = new MotorSafetyHelper(this);
    m_safetyHelper.setExpiration(0.0);
    m_safetyHelper.setSafetyEnabled(false);

    LiveWindow.add(this);
    setName("Talon SRX ", deviceNumber);
}

/**
 * Construct an instance of a joystick. The joystick index is the USB port on the drivers
 * station.
 *
 * @param port The port on the Driver Station that the joystick is plugged into.
 */
public JoystickF(final int port)
{
    super(port);

    m_axes[Axis.kX.value] = kDefaultXAxis;
    m_axes[Axis.kY.value] = kDefaultYAxis;
    m_axes[Axis.kZ.value] = kDefaultZAxis;
    m_axes[Axis.kTwist.value] = kDefaultTwistAxis;
    m_axes[Axis.kThrottle.value] = kDefaultThrottleAxis;

    HAL.report(FRCNetComm.tResourceType.kResourceType_Joystick, port);
}

/**
 * Tank drive method for differential drive platform.
 *
 * @param leftSpeed     The robot left side's speed along the X axis [-1.0..1.0]. Forward is
 *                      positive.
 * @param rightSpeed    The robot right side's speed along the X axis [-1.0..1.0]. Forward is
 *                      positive.
 * @param squaredInputs If set, decreases the input sensitivity at low speeds.
 */
public void tankDrive(float leftSpeed, float rightSpeed, boolean squaredInputs)
{
    if(!m_reported)
    {
        HAL.report(FRCNetComm.tResourceType.kResourceType_RobotDrive, 2, FRCNetComm.tInstances.kRobotDrive_Tank);
        m_reported = true;
    }

    leftSpeed = limit(leftSpeed);
    leftSpeed = applyDeadband(leftSpeed, m_deadbandF);

    rightSpeed = limit(rightSpeed);
    rightSpeed = applyDeadband(rightSpeed, m_deadbandF);

    // Square the inputs (while preserving the sign) to increase fine control
    // while permitting full power.
    if(squaredInputs)
    {
        leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
        rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
    }

    m_leftMotor.set(leftSpeed * m_maxOutputF);
    m_rightMotor.set(-rightSpeed * m_maxOutputF);

    m_safetyHelper.feed();
}

/**
 * Provide tank steering using the stored robot configuration. This function lets you directly
 * provide joystick values from any source.
 *
 * @param leftValue     The value of the left stick.
 * @param rightValue    The value of the right stick.
 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
 */
public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {

  if (!kTank_Reported) {
    HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
        tInstances.kRobotDrive_Tank);
    kTank_Reported = true;
  }

  // square the inputs (while preserving the sign) to increase fine control
  // while permitting full power
  leftValue = limit(leftValue);
  rightValue = limit(rightValue);
  if (squaredInputs) {
    if (leftValue >= 0.0) {
      leftValue = leftValue * leftValue;
    } else {
      leftValue = -(leftValue * leftValue);
    }
    if (rightValue >= 0.0) {
      rightValue = rightValue * rightValue;
    } else {
      rightValue = -(rightValue * rightValue);
    }
  }
  setLeftRightMotorOutputs(leftValue, rightValue);
}

/**
 * Drive method for Mecanum wheeled robots.
 *
 * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
 * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
 * top, the roller axles should form an X across the robot.
 *
 * <p>This is designed to be directly driven by joystick axes.
 *
 * @param x         The speed that the robot should drive in the X direction. [-1.0..1.0]
 * @param y         The speed that the robot should drive in the Y direction. This input is
 *                  inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
 * @param rotation  The rate of rotation for the robot that is completely independent of the
 *                  translation. [-1.0..1.0]
 * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented
 *                  controls.
 */
@SuppressWarnings("ParameterName")
public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
  if (!kMecanumCartesian_Reported) {
    HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
        tInstances.kRobotDrive_MecanumCartesian);
    kMecanumCartesian_Reported = true;
  }
  @SuppressWarnings("LocalVariableName")
  double xIn = x;
  @SuppressWarnings("LocalVariableName")
  double yIn = y;
  // Negate y for the joystick.
  yIn = -yIn;
  // Compenstate for gyro angle.
  double[] rotated = rotateVector(xIn, yIn, gyroAngle);
  xIn = rotated[0];
  yIn = rotated[1];

  double[] wheelSpeeds = new double[kMaxNumberOfMotors];
  wheelSpeeds[MotorType.kFrontLeft.value] = xIn + yIn + rotation;
  wheelSpeeds[MotorType.kFrontRight.value] = -xIn + yIn - rotation;
  wheelSpeeds[MotorType.kRearLeft.value] = -xIn + yIn + rotation;
  wheelSpeeds[MotorType.kRearRight.value] = xIn + yIn - rotation;

  normalize(wheelSpeeds);
  m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
  m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
  m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
  m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);

  if (m_safetyHelper != null) {
    m_safetyHelper.feed();
  }
}

/**
 * Drive method for Mecanum wheeled robots.
 *
 * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
 * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
 * top, the roller axles should form an X across the robot.
 *
 * @param magnitude The speed that the robot should drive in a given direction.
 * @param direction The direction the robot should drive in degrees. The direction and maginitute
 *                  are independent of the rotation rate.
 * @param rotation  The rate of rotation for the robot that is completely independent of the
 *                  magnitute or direction. [-1.0..1.0]
 */
public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
  if (!kMecanumPolar_Reported) {
    HAL.report(tResourceType.kResourceType_RobotDrive, getNumMotors(),
        tInstances.kRobotDrive_MecanumPolar);
    kMecanumPolar_Reported = true;
  }
  // Normalized for full power along the Cartesian axes.
  magnitude = limit(magnitude) * Math.sqrt(2.0);
  // The rollers are at 45 degree angles.
  double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
  double cosD = Math.cos(dirInRad);
  double sinD = Math.sin(dirInRad);

  double[] wheelSpeeds = new double[kMaxNumberOfMotors];
  wheelSpeeds[MotorType.kFrontLeft.value] = (sinD * magnitude + rotation);
  wheelSpeeds[MotorType.kFrontRight.value] = (cosD * magnitude - rotation);
  wheelSpeeds[MotorType.kRearLeft.value] = (cosD * magnitude + rotation);
  wheelSpeeds[MotorType.kRearRight.value] = (sinD * magnitude - rotation);

  normalize(wheelSpeeds);

  m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
  m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
  m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
  m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);

  if (m_safetyHelper != null) {
    m_safetyHelper.feed();
  }
}

/**
 * Construct an instance of a joystick. The joystick index is the USB port on the drivers
 * station.
 *
 * @param port The port on the Driver Station that the joystick is plugged into.
 */
public Joystick(final int port) {
  this(port, AxisType.kNumAxis.value, ButtonType.kNumButton.value);

  m_axes[AxisType.kX.value] = kDefaultXAxis;
  m_axes[AxisType.kY.value] = kDefaultYAxis;
  m_axes[AxisType.kZ.value] = kDefaultZAxis;
  m_axes[AxisType.kTwist.value] = kDefaultTwistAxis;
  m_axes[AxisType.kThrottle.value] = kDefaultThrottleAxis;

  m_buttons[ButtonType.kTrigger.value] = kDefaultTriggerButton;
  m_buttons[ButtonType.kTop.value] = kDefaultTopButton;

  HAL.report(tResourceType.kResourceType_Joystick, port);
}

@Override
public void setRumble(RumbleType type, double value) {
  if (value < 0) {
    value = 0;
  } else if (value > 1) {
    value = 1;
  }
  if (type == RumbleType.kLeftRumble) {
    m_leftRumble = (short) (value * 65535);
  } else {
    m_rightRumble = (short) (value * 65535);
  }
  HAL.setJoystickOutputs((byte) getPort(), m_outputs, m_leftRumble, m_rightRumble);
}

/**
 * Create an instance of a digital output. Create an instance of a digital output given a
 * channel.
 *
 * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
 *                the MXP
 */
public DigitalOutput(int channel) {
  checkDigitalChannel(channel);
  m_channel = channel;

  m_handle = DIOJNI.initializeDIOPort(DIOJNI.getPort((byte)channel), false);

  HAL.report(tResourceType.kResourceType_DigitalOutput, channel);
}

/**
 * Construct an analog trigger given an analog channel. This should be used in the case of sharing
 * an analog channel between the trigger and an analog input object.
 *
 * @param channel the AnalogInput to use for the analog trigger
 */
public AnalogTrigger(AnalogInput channel) {
  m_analogInput = channel;
  ByteBuffer index = ByteBuffer.allocateDirect(4);
  index.order(ByteOrder.LITTLE_ENDIAN);

  m_port =
      AnalogJNI.initializeAnalogTrigger(channel.m_port, index.asIntBuffer());
  m_index = index.asIntBuffer().get(0);

  HAL.report(tResourceType.kResourceType_AnalogTrigger, channel.getChannel());
}

/**
 * Constructor.
 *
 * @param port  The SPI port that the accelerometer is connected to
 * @param range The range (+ or -) that the accelerometer will measure.
 */
public ADXL362(SPI.Port port, Range range) {
  m_spi = new SPI(port);
  m_spi.setClockRate(3000000);
  m_spi.setMSBFirst();
  m_spi.setSampleDataOnFalling();
  m_spi.setClockActiveLow();
  m_spi.setChipSelectActiveLow();

  // Validate the part ID
  ByteBuffer transferBuffer = ByteBuffer.allocateDirect(3);
  transferBuffer.put(0, kRegRead);
  transferBuffer.put(1, kPartIdRegister);
  m_spi.transaction(transferBuffer, transferBuffer, 3);
  if (transferBuffer.get(2) != (byte) 0xF2) {
    m_spi.free();
    m_spi = null;
    DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
    return;
  }

  setRange(range);

  // Turn on the measurements
  transferBuffer.put(0, kRegWrite);
  transferBuffer.put(1, kPowerCtlRegister);
  transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
  m_spi.write(transferBuffer, 3);

  HAL.report(tResourceType.kResourceType_ADXL362, port.value);
  LiveWindow.addSensor("ADXL362", port.value, this);
}

/**
 * Construct an analog output on a specified MXP channel.
 *
 * @param channel The channel number to represent.
 */
public AnalogOutput(final int channel) {
  m_channel = channel;

  SensorBase.checkAnalogOutputChannel(channel);

  final int portHandle = AnalogJNI.getPort((byte) channel);
  m_port = AnalogJNI.initializeAnalogOutputPort(portHandle);

  LiveWindow.addSensor("AnalogOutput", channel, this);
  HAL.report(tResourceType.kResourceType_AnalogOutput, channel);
}

/**
 * Construct a GearTooth sensor given a channel.
 *
 * @param channel            The DIO channel that the sensor is connected to. 0-9 are on-board,
 *                           10-25 are on the MXP port
 * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
 *                           direction.
 */
public GearTooth(final int channel, boolean directionSensitive) {
  super(channel);
  enableDirectionSensing(directionSensitive);
  if (directionSensitive) {
    HAL.report(tResourceType.kResourceType_GearTooth, channel, 0, "D");
  } else {
    HAL.report(tResourceType.kResourceType_GearTooth, channel, 0);
  }
  LiveWindow.addSensor("GearTooth", channel, this);
}

/**
 * Common initialization code for Encoders. This code allocates resources for Encoders and is
 * common to all constructors.
 *
 * <p>The encoder will start counting immediately.
 *
 * @param reverseDirection If true, counts down instead of up (this is all relative)
 */
private void initEncoder(boolean reverseDirection, final EncodingType type) {
  m_encoder = EncoderJNI.initializeEncoder(m_aSource.getPortHandleForRouting(),
      m_aSource.getAnalogTriggerTypeForRouting(), m_bSource.getPortHandleForRouting(),
      m_bSource.getAnalogTriggerTypeForRouting(), reverseDirection, type.value);

  m_pidSource = PIDSourceType.kDisplacement;

  HAL.report(tResourceType.kResourceType_Encoder, getFPGAIndex(), type.value);
  LiveWindow.addSensor("Encoder", m_aSource.getChannel(), this);
}

/**
 * Initialize the gyro. Calibration is handled by calibrate().
 */
public void initGyro() {

  if (m_gyroHandle == 0) {
    m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
  }

  AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);

  HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
  LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
}

/**
 * Constructs the ADXL345 Accelerometer over I2C.
 *
 * @param port          The I2C port the accelerometer is attached to
 * @param range         The range (+ or -) that the accelerometer will measure.
 * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
 */
public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
  m_i2c = new I2C(port, deviceAddress);

  // Turn on the measurements
  m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);

  setRange(range);

  HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
  LiveWindow.addSensor("ADXL345_I2C", port.value, this);
}

/**
 * Returns the types of Axes on a given joystick port.
 *
 * @param stick The joystick port number
 * @param axis The target axis
 * @return What type of axis the axis is reporting to be
 */
public int getJoystickAxisType(int stick, int axis) {
  if (stick < 0 || stick >= kJoystickPorts) {
    throw new RuntimeException("Joystick index is out of range, should be 0-5");
  }

  int retVal = -1;
  synchronized (m_joystickMutex) {
    retVal = HAL.getJoystickAxisType((byte) stick, (byte) axis);
  }
  return retVal;
}

/**
 * Copy data from the DS task for the user. If no new data exists, it will just be returned,
 * otherwise the data will be copied from the DS polling loop.
 */
protected void getData() {
  // Get the status of all of the joysticks
  for (byte stick = 0; stick < kJoystickPorts; stick++) {
    m_joystickAxesCache[stick].m_count =
        HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
    m_joystickPOVsCache[stick].m_count =
        HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
    m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
    m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
  }
  // Force a control word update, to make sure the data is the newest.
  updateControlWord(true);

  // lock joystick mutex to swap cache data
  synchronized (m_joystickMutex) {
    // move cache to actual data
    HALJoystickAxes[] currentAxes = m_joystickAxes;
    m_joystickAxes = m_joystickAxesCache;
    m_joystickAxesCache = currentAxes;

    HALJoystickButtons[] currentButtons = m_joystickButtons;
    m_joystickButtons = m_joystickButtonsCache;
    m_joystickButtonsCache = currentButtons;

    HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
    m_joystickPOVs = m_joystickPOVsCache;
    m_joystickPOVsCache = currentPOVs;
  }
}

/**
 * Provides the service routine for the DS polling m_thread.
 */
private void run() {
  int safetyCounter = 0;
  while (m_threadKeepAlive) {
    HAL.waitForDSData();
    getData();
    m_dataMutex.lock();
    try {
      m_waitForDataPredicate = true;
      m_dataCond.signalAll();
    } finally {
      m_dataMutex.unlock();
    }
    // notify isNewControlData variable
    m_newControlData.set(true);

    if (++safetyCounter >= 4) {
      MotorSafetyHelper.checkMotors();
      safetyCounter = 0;
    }
    if (m_userInDisabled) {
      HAL.observeUserProgramDisabled();
    }
    if (m_userInAutonomous) {
      HAL.observeUserProgramAutonomous();
    }
    if (m_userInTeleop) {
      HAL.observeUserProgramTeleop();
    }
    if (m_userInTest) {
      HAL.observeUserProgramTest();
    }
  }
}

/**
 * Updates the data in the control word cache. Updates if the force parameter is set, or if
 * 50ms have passed since the last update.
 *
 * @param force True to force an update to the cache, otherwise update if 50ms have passed.
 */
private void updateControlWord(boolean force) {
  long now = System.currentTimeMillis();
  synchronized (m_controlWordMutex) {
    if (now - m_lastControlWordUpdate > 50 || force) {
      HAL.getControlWord(m_controlWordCache);
      m_lastControlWordUpdate = now;
    }
  }
}

/**
 * Constructor.
 *
 * @param moduleNumber   The module number of the solenoid module to use.
 * @param forwardChannel The forward channel on the module to control (0..7).
 * @param reverseChannel The reverse channel on the module to control (0..7).
 */
public DoubleSolenoid(final int moduleNumber, final int forwardChannel,
                      final int reverseChannel) {
  super(moduleNumber);

  checkSolenoidModule(m_moduleNumber);
  checkSolenoidChannel(forwardChannel);
  checkSolenoidChannel(reverseChannel);

  int portHandle = SolenoidJNI.getPortWithModule((byte) m_moduleNumber, (byte) forwardChannel);
  m_forwardHandle = SolenoidJNI.initializeSolenoidPort(portHandle);

  try {
    portHandle = SolenoidJNI.getPortWithModule((byte) m_moduleNumber, (byte) reverseChannel);
    m_reverseHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
  } catch (RuntimeException ex) {
    // free the forward handle on exception, then rethrow
    SolenoidJNI.freeSolenoidPort(m_forwardHandle);
    m_forwardHandle = 0;
    m_reverseHandle = 0;
    throw ex;
  }

  m_forwardMask = (byte) (1 << forwardChannel);
  m_reverseMask = (byte) (1 << reverseChannel);

  HAL.report(tResourceType.kResourceType_Solenoid, forwardChannel,
                                 m_moduleNumber);
  HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel,
                                 m_moduleNumber);
  LiveWindow.addActuator("DoubleSolenoid", m_moduleNumber, forwardChannel, this);
}