ControlWord.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.hardware.hal;
 
import org.wpilib.hardware.hal.struct.ControlWordStruct;

public class ControlWord {
  private static final long OPMODE_HASH_MASK = 0x00FFFFFFFFFFFFFFL;
  private static final long ROBOT_MODE_MASK = 0x0300000000000000L;
  private static final long ROBOT_MODE_SHIFT = 56;
  private static final long ENABLED_MASK = 0x0400000000000000L;
  private static final long ESTOP_MASK = 0x0800000000000000L;
  private static final long FMS_ATTACHED_MASK = 0x1000000000000000L;
  private static final long DS_ATTACHED_MASK = 0x2000000000000000L;
 
  private long m_word;
  private RobotMode m_robotMode = RobotMode.UNKNOWN;

  public ControlWord() {}

  public void update(
      long opModeHash,
      RobotMode robotMode,
      boolean enabled,
      boolean emergencyStop,
      boolean fmsAttached,
      boolean dsAttached) {
    m_word =
        (opModeHash & OPMODE_HASH_MASK)
            | ((long) robotMode.getValue() << ROBOT_MODE_SHIFT)
            | (enabled ? ENABLED_MASK : 0)
            | (emergencyStop ? ESTOP_MASK : 0)
            | (fmsAttached ? FMS_ATTACHED_MASK : 0)
            | (dsAttached ? DS_ATTACHED_MASK : 0);
    m_robotMode = robotMode;
  }

  public void update(long word) {
    m_word = word;
    m_robotMode = RobotMode.fromInt((int) ((word & ROBOT_MODE_MASK) >> ROBOT_MODE_SHIFT));
  }

  public void update(ControlWord word) {
    m_word = word.m_word;
    m_robotMode = word.m_robotMode;
  }

  public long getOpModeId() {
    // if the hash portion is zero, return 0
    if ((m_word & OPMODE_HASH_MASK) == 0) {
      return 0;
    }
    // otherwise return the full ID (which includes the robot mode)
    return m_word & (OPMODE_HASH_MASK | ROBOT_MODE_MASK);
  }

  public void setOpModeId(long id) {
    m_word &= ~(OPMODE_HASH_MASK | ROBOT_MODE_MASK);
    m_word |= id & (OPMODE_HASH_MASK | ROBOT_MODE_MASK);
    // keep robot mode in sync
    m_robotMode = RobotMode.fromInt((int) ((m_word & ROBOT_MODE_MASK) >> ROBOT_MODE_SHIFT));
  }

  public RobotMode getRobotMode() {
    return m_robotMode;
  }

  public boolean isEnabled() {
    return (m_word & ENABLED_MASK) != 0;
  }

  public boolean isEStopped() {
    return (m_word & ESTOP_MASK) != 0;
  }

  public boolean isFMSAttached() {
    return (m_word & FMS_ATTACHED_MASK) != 0;
  }

  public boolean isDSAttached() {
    return (m_word & DS_ATTACHED_MASK) != 0;
  }

  public boolean isAutonomous() {
    return getRobotMode() == RobotMode.AUTONOMOUS;
  }

  public boolean isAutonomousEnabled() {
    return isAutonomous() && isEnabled() && isDSAttached();
  }

  public boolean isTeleop() {
    return getRobotMode() == RobotMode.TELEOPERATED;
  }

  public boolean isTeleopEnabled() {
    return isTeleop() && isEnabled() && isDSAttached();
  }

  public boolean isUtility() {
    return getRobotMode() == RobotMode.UTILITY;
  }

  public boolean isUtilityEnabled() {
    return isUtility() && isEnabled() && isDSAttached();
  }

  public long getNative() {
    return m_word;
  }
 
  @Override
  public boolean equals(Object obj) {
    return obj instanceof ControlWord word && m_word == word.m_word;
  }
 
  @Override
  public int hashCode() {
    return Long.hashCode(m_word);
  }
 
  public static final ControlWordStruct struct = new ControlWordStruct();
}