POVDirection.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.driverstation;
 
import java.util.Optional;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.system.Timer;

public enum POVDirection {
  CENTER(0x00),
  UP(0x01),
  UP_RIGHT(0x01 | 0x02),
  RIGHT(0x02),
  DOWN_RIGHT(0x02 | 0x04),
  DOWN(0x04),
  DOWN_LEFT(0x04 | 0x08),
  LEFT(0x08),
  UP_LEFT(0x01 | 0x08);
 
  private static final double INVALID_POV_VALUE_INTERVAL = 1.0;
  private static double s_nextMessageTime;

  public static POVDirection of(byte value) {
    for (var direction : values()) {
      if (direction.value == value) {
        return direction;
      }
    }
    double currentTime = Timer.getTimestamp();
    if (currentTime > s_nextMessageTime) {
      DriverStationErrors.reportError("Invalid POV value " + value + "!", false);
      s_nextMessageTime = currentTime + INVALID_POV_VALUE_INTERVAL;
    }
    return CENTER;
  }

  public final byte value;
 
  POVDirection(int value) {
    this.value = (byte) value;
  }

  public Optional<Rotation2d> getAngle() {
    return switch (this) {
      case CENTER -> Optional.empty();
      case UP -> Optional.of(Rotation2d.fromDegrees(0));
      case UP_RIGHT -> Optional.of(Rotation2d.fromDegrees(45));
      case RIGHT -> Optional.of(Rotation2d.fromDegrees(90));
      case DOWN_RIGHT -> Optional.of(Rotation2d.fromDegrees(135));
      case DOWN -> Optional.of(Rotation2d.fromDegrees(180));
      case DOWN_LEFT -> Optional.of(Rotation2d.fromDegrees(225));
      case LEFT -> Optional.of(Rotation2d.fromDegrees(270));
      case UP_LEFT -> Optional.of(Rotation2d.fromDegrees(315));
    };
  }
}