mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
Initial commit
This commit is contained in:
C4llSqin
@@ -0,0 +1,13 @@
|
||||
package frc4388.utility;
|
||||
|
||||
// This is a seperate class in case I want to encode rotation or other
|
||||
// information about the tag
|
||||
public class AprilTag {
|
||||
public final double x, y, z;
|
||||
|
||||
public AprilTag(double _x, double _y, double _z) {
|
||||
x = _x;
|
||||
y = _y;
|
||||
z = _z;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.hal.CANData;
|
||||
import edu.wpi.first.hal.can.CANJNI;
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class CanDevice {
|
||||
public static List<CanDevice> devices = new ArrayList<>();
|
||||
|
||||
public String name;
|
||||
public int id;
|
||||
|
||||
public CanDevice(String name, int id) {
|
||||
this.name = name;
|
||||
this.id = id;
|
||||
|
||||
devices.add(this);
|
||||
}
|
||||
|
||||
|
||||
private boolean isAlive() {
|
||||
return true; //TODO: Link this with Device Finder
|
||||
}
|
||||
|
||||
public String getName() {
|
||||
return "CAN ID " + this.id + " ( " + this.name + " ) ";
|
||||
}
|
||||
|
||||
public void Log(String str){
|
||||
System.out.println(getName() + " - " + str);
|
||||
}
|
||||
|
||||
public Status queryStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
s.addReport(ReportLevel.INFO, "TODO");
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
//TODO
|
||||
s.addReport(ReportLevel.INFO, "Add CAN magic here");
|
||||
boolean isAlive = isAlive();
|
||||
s.addReport(isAlive ? ReportLevel.INFO : ReportLevel.ERROR, "Is Alive: " + isAlive);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
public class DataUtils {
|
||||
public static byte[] doubleToByteArray(double value) {
|
||||
byte[] bytes = new byte[8];
|
||||
ByteBuffer.wrap(bytes).putDouble(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static double byteArrayToDouble(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getDouble();
|
||||
}
|
||||
|
||||
public static byte[] intToByteArray(int value) {
|
||||
byte[] bytes = new byte[4];
|
||||
ByteBuffer.wrap(bytes).putInt(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static int byteArrayToInt(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getInt();
|
||||
}
|
||||
|
||||
public static byte[] shortToByteArray(short value) {
|
||||
byte[] bytes = new byte[2];
|
||||
ByteBuffer.wrap(bytes).putShort(value);
|
||||
return bytes;
|
||||
}
|
||||
|
||||
public static short byteArrayToShort(byte[] bytes) {
|
||||
return ByteBuffer.wrap(bytes).getShort();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class DeferredBlock {
|
||||
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
|
||||
private static boolean m_hasRun = false;
|
||||
|
||||
public DeferredBlock(Runnable block) {
|
||||
m_blocks.add(block);
|
||||
}
|
||||
|
||||
public static void execute() {
|
||||
if (m_hasRun) return;
|
||||
|
||||
for (Runnable block : m_blocks) {
|
||||
block.run();
|
||||
}
|
||||
|
||||
m_blocks.clear(); // for garbage collection
|
||||
m_hasRun = true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
// 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 frc4388.utility;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class Gains {
|
||||
public double kP;
|
||||
public double kI;
|
||||
public double kD;
|
||||
public double kF;
|
||||
public int kIZone;
|
||||
public double kPeakOutput;
|
||||
public double kMaxOutput;
|
||||
public double kMinOutput;
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = kPeakOutput;
|
||||
this.kMaxOutput = kPeakOutput;
|
||||
this.kMinOutput = -kPeakOutput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kPeakOutput = 1.0;
|
||||
this.kMaxOutput = 1.0;
|
||||
this.kMinOutput = -1.0;
|
||||
}
|
||||
|
||||
public Gains(double kP, double kI, double kD) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates Gains object for PIDs
|
||||
* @param kP The P value.
|
||||
* @param kI The I value.
|
||||
* @param kD The D value.
|
||||
* @param kF The F value.
|
||||
* @param kIZone The zone of the I value.
|
||||
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
|
||||
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
|
||||
*/
|
||||
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
|
||||
this.kP = kP;
|
||||
this.kI = kI;
|
||||
this.kD = kD;
|
||||
this.kF = kF;
|
||||
this.kIZone = kIZone;
|
||||
this.kMaxOutput = kMaxOutput;
|
||||
this.kMinOutput = kMinOutput;
|
||||
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
package frc4388.utility;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public enum LEDPatterns {
|
||||
/* PALLETTE PATTERNS */
|
||||
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
|
||||
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
|
||||
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
|
||||
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
|
||||
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
|
||||
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
|
||||
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
|
||||
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
|
||||
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
|
||||
|
||||
/* COLOR 1 PATTERNS */
|
||||
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
|
||||
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
|
||||
|
||||
/* COLOR 2 PATTERNS */
|
||||
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
|
||||
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
|
||||
|
||||
/* COLOR 1 AND 2 PATTERNS */
|
||||
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
|
||||
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
|
||||
|
||||
/* SOLID COLORS */
|
||||
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
|
||||
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
|
||||
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
|
||||
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
|
||||
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
|
||||
|
||||
/* GETTERS/SETTERS */
|
||||
private final float id;
|
||||
LEDPatterns(float id) {
|
||||
this.id = id;
|
||||
}
|
||||
public float getValue() {
|
||||
return id;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,269 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
// import com.ctre.phoenix.sensors.WPI_Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.kauailabs.navx.frc.AHRS;
|
||||
|
||||
// import edu.wpi.first.wpilibj.GyroBase;
|
||||
// import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
|
||||
/**
|
||||
* Gyro class that allows for interchangeable use between a pigeon and a navX
|
||||
*/
|
||||
public class RobotGyro {
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
|
||||
private Pigeon2 m_pigeon = null;
|
||||
private AHRS m_navX = null;
|
||||
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
|
||||
|
||||
private double m_lastPigeonAngle;
|
||||
private double m_deltaPigeonAngle;
|
||||
|
||||
private double pitchZero = 0;
|
||||
private double rollZero = 0;
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a pigeon
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(Pigeon2 gyro) {
|
||||
m_pigeon = gyro;
|
||||
m_isGyroAPigeon = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a Gyro based on a navX
|
||||
* @param gyro the gyroscope to use for Gyro
|
||||
*/
|
||||
public RobotGyro(AHRS gyro){
|
||||
m_navX = gyro;
|
||||
m_isGyroAPigeon = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets yaw, pitch, and roll.
|
||||
*/
|
||||
public void resetZeroValues() {
|
||||
if (!m_isGyroAPigeon) return;
|
||||
|
||||
// pitchZero = m_pigeon.getPitch();
|
||||
// rollZero = m_pigeon.getRoll();
|
||||
}
|
||||
|
||||
/**
|
||||
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
|
||||
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
|
||||
*/
|
||||
public void updatePigeonDeltas() {
|
||||
double currentPigeonAngle = getAngle();
|
||||
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
|
||||
m_lastPigeonAngle = currentPigeonAngle;
|
||||
}
|
||||
|
||||
/**
|
||||
* <p>NavX:
|
||||
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
|
||||
* the center value as the Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*
|
||||
* <p>Pigeon:
|
||||
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
|
||||
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
|
||||
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
|
||||
* make sure that the robot is not moving while the tempurature calculations are in progress, this
|
||||
* is typically done when the robot is first turned on while it's sitting at rest before the
|
||||
* competition starts.
|
||||
*/
|
||||
public void calibrate() {
|
||||
return;
|
||||
// if (m_isGyroAPigeon) {
|
||||
// m_pigeon.calibrate();
|
||||
// } else {
|
||||
// m_navX.calibrate();
|
||||
// }
|
||||
}
|
||||
|
||||
public void reset() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(0);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void reset(double val) {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(val);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetFlip() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(180);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetNinety() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(90);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetTwoSeventy() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(270);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetRightSideBlue() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(60);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void resetAmpSide() {
|
||||
resetZeroValues();
|
||||
|
||||
if (m_isGyroAPigeon) {
|
||||
m_pigeon.setYaw(-60);
|
||||
} else {
|
||||
m_navX.reset();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get Yaw, Pitch, and Roll data.
|
||||
*
|
||||
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
|
||||
* Yaw is within [-368,640, +368,640] degrees.
|
||||
* Pitch is within [-90,+90] degrees.
|
||||
* Roll is within [-90,+90] degrees.
|
||||
*/
|
||||
private double[] getPigeonAngles() {
|
||||
//m_pigeon.getAngle(); // This appeared to not do anything?
|
||||
var rotation = m_pigeon.getRotation3d();
|
||||
|
||||
return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
|
||||
}
|
||||
|
||||
public Rotation2d getRotation2d() {
|
||||
return m_pigeon.getRotation2d();
|
||||
}
|
||||
|
||||
public double getAngle() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return getPigeonAngles()[2];
|
||||
} else {
|
||||
return m_navX.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
public double getYaw() {
|
||||
return this.getAngle();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading() {
|
||||
return getHeading(getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets an absolute heading of the robot
|
||||
* @return heading from -180 to 180 degrees
|
||||
*/
|
||||
public double getHeading(double angle) {
|
||||
return Math.IEEEremainder(angle, 360);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current pitch value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Pitch is a measure of rotation around
|
||||
* the Y Axis.
|
||||
* @return The current pitch value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getPitch() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current roll value (in degrees, from -90 to 90)
|
||||
* reported by the sensor. Roll is a measure of rotation around
|
||||
* the X Axis.
|
||||
* @return The current roll value in degrees (-90 to 90).
|
||||
*/
|
||||
public double getRoll() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
|
||||
} else {
|
||||
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
|
||||
}
|
||||
}
|
||||
|
||||
public double getRate() {
|
||||
if (m_isGyroAPigeon) {
|
||||
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
|
||||
} else {
|
||||
return m_navX.getRate();
|
||||
}
|
||||
}
|
||||
|
||||
public Pigeon2 getPigeon(){
|
||||
return m_pigeon;
|
||||
}
|
||||
|
||||
public AHRS getNavX(){
|
||||
return m_navX;
|
||||
}
|
||||
|
||||
public void close() throws Exception {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
/**
|
||||
* <p>Keeps track of Robot times like time passed, delta time, etc
|
||||
* <p>All times are in milliseconds
|
||||
*/
|
||||
public class RobotTime {
|
||||
private long m_currTime = System.currentTimeMillis();
|
||||
public long m_deltaTime = 0;
|
||||
|
||||
private long m_startRobotTime = m_currTime;
|
||||
public long m_robotTime = 0;
|
||||
public long m_lastRobotTime = 0;
|
||||
|
||||
private long m_startMatchTime = 0;
|
||||
public long m_matchTime = 0;
|
||||
public long m_lastMatchTime = 0;
|
||||
|
||||
public long m_frameNumber = 0;
|
||||
|
||||
/**
|
||||
* Private constructor prevents other classes from instantiating
|
||||
*/
|
||||
private RobotTime(){}
|
||||
|
||||
private static RobotTime instance = null;
|
||||
|
||||
/**
|
||||
* Gets the instance of Robot Time. If there is no instance running one will be created.
|
||||
* @return instance of Robot Time
|
||||
*/
|
||||
public static RobotTime getInstance() {
|
||||
if (instance == null) {
|
||||
instance = new RobotTime();
|
||||
}
|
||||
return instance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this once per periodic loop.
|
||||
*/
|
||||
public void updateTimes() {
|
||||
m_lastRobotTime = m_robotTime;
|
||||
m_lastMatchTime = m_matchTime;
|
||||
|
||||
m_currTime = System.currentTimeMillis();
|
||||
m_robotTime = m_currTime - m_startRobotTime;
|
||||
m_deltaTime = m_robotTime - m_lastRobotTime;
|
||||
m_frameNumber++;
|
||||
|
||||
if (m_startMatchTime != 0) {
|
||||
m_matchTime = m_currTime - m_startMatchTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in both the auto and periodic inits
|
||||
*/
|
||||
public void startMatchTime() {
|
||||
if (m_startMatchTime == 0) {
|
||||
m_startMatchTime = m_currTime;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this in disabled init
|
||||
*/
|
||||
public void endMatchTime() {
|
||||
m_startMatchTime = 0;
|
||||
m_matchTime = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
// 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 frc4388.utility;
|
||||
|
||||
/** Aarav's good units class (better than WPILib)
|
||||
* @author Aarav Shah */
|
||||
|
||||
public class RobotUnits {
|
||||
// constants
|
||||
|
||||
// angle conversions
|
||||
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
|
||||
|
||||
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
|
||||
|
||||
// falcon conversions
|
||||
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
|
||||
|
||||
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
|
||||
|
||||
// distance conversions
|
||||
public static double metersToFeet(final double meters) {return meters * 3.28084;}
|
||||
|
||||
public static double feetToMeters(final double feet) {return feet / 3.28084;}
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix6.StatusCode;
|
||||
import com.ctre.phoenix6.controls.EmptyControl;
|
||||
import com.ctre.phoenix6.hardware.CANcoder;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
public class Status {
|
||||
public enum ReportLevel {
|
||||
INFO,
|
||||
WARNING,
|
||||
ERROR
|
||||
}
|
||||
|
||||
public class Report {
|
||||
public ReportLevel reportLevel;
|
||||
public String description;
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return this.reportLevel.name() + ": " + this.description;
|
||||
}
|
||||
}
|
||||
|
||||
public List<Report> reports;
|
||||
|
||||
public Status() {
|
||||
this.reports = new ArrayList<>();
|
||||
}
|
||||
|
||||
public void addReport(ReportLevel level, String description) {
|
||||
Report r = new Report();
|
||||
r.reportLevel = level;
|
||||
r.description = description;
|
||||
this.reports.add(r);
|
||||
}
|
||||
|
||||
private String printStatusCode(StatusCode status){
|
||||
return status.getName() + " (" + status.value + ")";
|
||||
}
|
||||
|
||||
public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
|
||||
if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
|
||||
else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
|
||||
}
|
||||
|
||||
public void diagnoseHardwareCTRE(String deviceName, CANcoder coder) {
|
||||
// Because the Cancoder has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
|
||||
// If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
|
||||
// TODO: validate that a CANCoder can actually do `EmptyControl`s
|
||||
StatusCode status = coder.setControl(new EmptyControl());
|
||||
if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Cancoder Alive?: Alive. " + printStatusCode(status));
|
||||
else addReport(ReportLevel.ERROR, deviceName + " Cancoder Alive?: Dead! " + printStatusCode(status));
|
||||
|
||||
|
||||
|
||||
// StatusSignal<MagnetHealthValue> -> MagnetHealthValue -> int
|
||||
int coderMagHealth = coder.getMagnetHealth().getValue().value;
|
||||
if (coderMagHealth == 3) addReport(ReportLevel.INFO, deviceName + " Cancoder Magnet Strength?: Ideal."); // why is 3 the 'good value'?
|
||||
if (coderMagHealth == 2) addReport(ReportLevel.WARNING, deviceName + " Cancoder Magnet Strength?: Subpar.");
|
||||
if (coderMagHealth == 1) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Too Close or Far!");
|
||||
if (coderMagHealth == 0) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Unkown!");
|
||||
}
|
||||
|
||||
public void diagnoseHardwareCTRE(String deviceName, Pigeon2 pigeon) {
|
||||
// Because the Pigeon has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
|
||||
// If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
|
||||
// TODO: validate that a Pigeon2 can actually do `EmptyControl`s
|
||||
StatusCode status = pigeon.setControl(new EmptyControl());
|
||||
if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Pigeon2 Alive?: Alive. " + printStatusCode(status));
|
||||
else addReport(ReportLevel.ERROR, deviceName + " Pigeon2 Alive?: Dead! " + printStatusCode(status));
|
||||
}
|
||||
|
||||
public boolean hasReport() {
|
||||
return reports.size() == 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public abstract class Subsystem extends SubsystemBase {
|
||||
public static List<Subsystem> subsystems = new ArrayList<>();
|
||||
|
||||
public Subsystem(){
|
||||
subsystems.add(this);
|
||||
}
|
||||
|
||||
public void Log(String str) {
|
||||
System.out.println(getSubsystemName() + " - " + str);
|
||||
}
|
||||
|
||||
// Get name of subsystem, for use in log.
|
||||
public abstract String getSubsystemName();
|
||||
// Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard
|
||||
public abstract void queryStatus();
|
||||
// Proactivly search for any errors in each subsystem
|
||||
public abstract Status diagnosticStatus();
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
package frc4388.utility;
|
||||
|
||||
public class UtilityStructs {
|
||||
public static class TimedOutput {
|
||||
public double leftX = 0.0;
|
||||
public double leftY = 0.0;
|
||||
public double rightX = 0.0;
|
||||
public double rightY = 0.0;
|
||||
|
||||
public boolean OPLB;
|
||||
public boolean OPRB;
|
||||
|
||||
|
||||
public long timedOffset = 0;
|
||||
}
|
||||
public static class AutoRecordingControllerFrame {
|
||||
public double[] axes = new double[6];
|
||||
public short button = 0;
|
||||
public short[] POV = new short[1];
|
||||
|
||||
}
|
||||
public static class AutoRecordingFrame {
|
||||
public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2];
|
||||
public int timeStamp;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.utility.configurable;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class ConfigurableDouble {
|
||||
private double defualtValue;
|
||||
private String name;
|
||||
|
||||
/**
|
||||
* Creates an new ConfigurableDouble through Smart Dashboard.
|
||||
* @param name the name of the Smart Dashboard key.
|
||||
* @param defualtValue the initilization value
|
||||
*/
|
||||
public ConfigurableDouble(String name, double defualtValue) {
|
||||
this.name = name;
|
||||
this.defualtValue = defualtValue;
|
||||
SmartDashboard.putNumber(name, defualtValue);
|
||||
}
|
||||
|
||||
public double get() {
|
||||
return SmartDashboard.getNumber(name, defualtValue);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package frc4388.utility.configurable;
|
||||
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class ConfigurableString {
|
||||
private String defualtValue;
|
||||
private String name;
|
||||
|
||||
/**
|
||||
* Creates an new ConfigurableString through Smart Dashboard.
|
||||
* @param name the name of the Smart Dashboard key.
|
||||
* @param defualtValue the initilization value
|
||||
*/
|
||||
public ConfigurableString(String name, String defualtValue) {
|
||||
this.name = name;
|
||||
this.defualtValue = defualtValue;
|
||||
SmartDashboard.putString(name, defualtValue);
|
||||
}
|
||||
|
||||
public String get() {
|
||||
return SmartDashboard.getString(name, defualtValue);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class DeadbandedXboxController extends XboxController {
|
||||
public DeadbandedXboxController(int port) { super(port); }
|
||||
|
||||
@Override public double getLeftX() { return getLeft().getX(); }
|
||||
@Override public double getLeftY() { return getLeft().getY(); }
|
||||
@Override public double getRightX() { return getRight().getX(); }
|
||||
@Override public double getRightY() { return getRight().getY(); }
|
||||
|
||||
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
|
||||
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
|
||||
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public interface IHandController {
|
||||
|
||||
public double getLeftXAxis();
|
||||
|
||||
public double getLeftYAxis();
|
||||
|
||||
public double getRightXAxis();
|
||||
|
||||
public double getRightYAxis();
|
||||
|
||||
public double getLeftTriggerAxis();
|
||||
|
||||
public double getRightTriggerAxis();
|
||||
|
||||
public int getDpadAngle();
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
|
||||
/**
|
||||
* A virtual controller that can be bound like an standard controller.
|
||||
* @author Zachary Wilke
|
||||
*/
|
||||
public class VirtualController extends GenericHID {
|
||||
private short m_buttonStates = 0;
|
||||
private short m_buttonStatesLastFrame = 0;
|
||||
private double[] m_axes = new double[6];
|
||||
private short[] m_pov = new short[1];
|
||||
|
||||
/**
|
||||
* Create an virtual controller
|
||||
* @param port virtual port (merely a formality).
|
||||
*/
|
||||
public VirtualController(int port) {
|
||||
super(port);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the curent inputs to the new frames.
|
||||
* @param axes joystick axes, (i.e. joysticks and triggers).
|
||||
* @param buttonFlags the bit packed button states.
|
||||
* @param pov the array of dpads.
|
||||
*/
|
||||
public void setFrame(double[] axes, short buttonFlags, short[] pov) {
|
||||
m_axes = axes;
|
||||
setOutputs(buttonFlags);
|
||||
m_pov = pov;
|
||||
}
|
||||
|
||||
/**
|
||||
* Zero outs the controls.
|
||||
*/
|
||||
public void zeroControls() {
|
||||
m_axes = new double[6];
|
||||
m_buttonStates = 0;
|
||||
m_buttonStatesLastFrame = 0;
|
||||
m_pov = new short[] {-1};
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the value of a bitflag from an int
|
||||
* @param value int to search
|
||||
* @param index index of bit
|
||||
* @return if the bit is set
|
||||
*/
|
||||
public static boolean getFlag(int value, int index) {
|
||||
return ((value & 1 << index) != 0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButton(int button) { // man why are buttons indexed at 1.
|
||||
return getFlag(m_buttonStates, button - 1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonPressed(int button) {
|
||||
return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button));
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonReleased(int button) {
|
||||
return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button));
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRawAxis(int axis) {
|
||||
return m_axes[axis];
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPOV(int pov) {
|
||||
return m_pov[pov];
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getAxisCount() {
|
||||
return m_axes.length;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPOVCount() {
|
||||
return m_pov.length;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getButtonCount() {
|
||||
return 10;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isConnected() {
|
||||
return true;
|
||||
}
|
||||
|
||||
@Override
|
||||
public HIDType getType() {
|
||||
return HIDType.kXInputGamepad;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getName() {
|
||||
return "Virtual Controller";
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getAxisType(int axis) {
|
||||
return 1; /* ! Warning, does not return accurate data.
|
||||
Hopefully this isn't a problem */
|
||||
}
|
||||
|
||||
/**
|
||||
* Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
|
||||
* this is an no-op overide.
|
||||
*/
|
||||
@Override
|
||||
public void setOutput(int outputNumber, boolean value) {
|
||||
// do not use
|
||||
//m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1];
|
||||
//m_buttonStates[outputNumber - 1] = value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame}
|
||||
*/
|
||||
@Override
|
||||
public void setOutputs(int value) {
|
||||
m_buttonStatesLastFrame = m_buttonStates;
|
||||
m_buttonStates = (short) value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Why are you Setting rumble on an virtual controller?
|
||||
* @param type the rumble type (even though it won't do anything)
|
||||
* @param value the rumble strength (always multiplyed by 0.0)
|
||||
*/
|
||||
@Override
|
||||
public void setRumble(RumbleType type, double value) {
|
||||
System.out.println("Why are you Setting rumble on an virtual controller?");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,218 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
|
||||
/**
|
||||
* This is a wrapper for the WPILib Joystick class that represents an XBox
|
||||
* controller.
|
||||
* @author frc1675
|
||||
*/
|
||||
public class XboxController implements IHandController
|
||||
{
|
||||
public static final int LEFT_X_AXIS = 0;
|
||||
public static final int LEFT_Y_AXIS = 1;
|
||||
public static final int LEFT_TRIGGER_AXIS = 2;
|
||||
public static final int RIGHT_TRIGGER_AXIS = 3;
|
||||
public static final int RIGHT_X_AXIS = 4;
|
||||
public static final int RIGHT_Y_AXIS = 5;
|
||||
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
|
||||
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
|
||||
|
||||
public static final int A_BUTTON = 1;
|
||||
public static final int B_BUTTON = 2;
|
||||
public static final int X_BUTTON = 3;
|
||||
public static final int Y_BUTTON = 4;
|
||||
public static final int LEFT_BUMPER_BUTTON = 5;
|
||||
public static final int RIGHT_BUMPER_BUTTON = 6;
|
||||
public static final int BACK_BUTTON = 7;
|
||||
public static final int START_BUTTON = 8;
|
||||
|
||||
public static final int LEFT_JOYSTICK_BUTTON = 9;
|
||||
public static final int RIGHT_JOYSTICK_BUTTON = 10;
|
||||
|
||||
private static final double LEFT_DPAD_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
|
||||
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
|
||||
private static final double TOP_DPAD_TOLERANCE = 0.9;
|
||||
|
||||
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
|
||||
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
|
||||
|
||||
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double DEADZONE = 0.1;
|
||||
|
||||
private Joystick m_stick;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public XboxController(int portNumber){
|
||||
m_stick = new Joystick(portNumber);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getJoyStick() {
|
||||
return m_stick;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the input falls within the deadzone.
|
||||
* @param input from an axis on the controller
|
||||
* @return true if input falls in deadzone, false if input falls outside deadzone
|
||||
*/
|
||||
private boolean inDeadZone(double input){
|
||||
return (Math.abs(input) < DEADZONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates an input to have a deadzone around the 0 position
|
||||
* @param input from an axis on the controller
|
||||
* @return updated input
|
||||
*/
|
||||
private double getAxisWithDeadZoneCheck(double input){
|
||||
if(inDeadZone(input)){
|
||||
return 0.0;
|
||||
} else {
|
||||
return input;
|
||||
}
|
||||
}
|
||||
|
||||
public boolean getAButton(){
|
||||
return m_stick.getRawButton(A_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getXButton(){
|
||||
return m_stick.getRawButton(X_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBButton(){
|
||||
return m_stick.getRawButton(B_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getYButton(){
|
||||
return m_stick.getRawButton(Y_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBackButton(){
|
||||
return m_stick.getRawButton(BACK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getStartButton(){
|
||||
return m_stick.getRawButton(START_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftBumperButton(){
|
||||
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightBumperButton(){
|
||||
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftJoystickButton(){
|
||||
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightJoystickButton(){
|
||||
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public double getLeftXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getRightXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getRightYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
public double getRightTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle input from the dpad.
|
||||
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
|
||||
*/
|
||||
public int getDpadAngle() {
|
||||
return m_stick.getPOV(0);
|
||||
}
|
||||
|
||||
public boolean getDPadLeft(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadRight(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadTop(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadBottom(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftTrigger(){
|
||||
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightTrigger(){
|
||||
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisUpTrigger(){
|
||||
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisDownTrigger(){
|
||||
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisLeftTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisRightTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisUpTrigger(){
|
||||
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisDownTrigger(){
|
||||
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisLeftTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisRightTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
//import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
|
||||
/**
|
||||
* Mapping for the Xbox controller triggers to allow triggers to be defined as
|
||||
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
|
||||
* exceeds a tolerance defined in {@link XboxController}.
|
||||
*/
|
||||
public class XboxTriggerButton {//extends Trigger {
|
||||
public static final int RIGHT_TRIGGER = 0;
|
||||
public static final int LEFT_TRIGGER = 1;
|
||||
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
|
||||
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
|
||||
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
|
||||
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
|
||||
public static final int LEFT_AXIS_UP_TRIGGER = 6;
|
||||
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
|
||||
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
|
||||
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
|
||||
|
||||
private XboxController m_controller;
|
||||
private int m_trigger;
|
||||
|
||||
/**
|
||||
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
|
||||
*/
|
||||
public XboxTriggerButton(XboxController controller, int trigger) {
|
||||
m_controller = controller;
|
||||
m_trigger = trigger;
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
// @Override
|
||||
public boolean get() {
|
||||
if (m_trigger == RIGHT_TRIGGER) {
|
||||
return m_controller.getRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_TRIGGER) {
|
||||
return m_controller.getLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getRightAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getRightAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getRightAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getRightAxisLeftTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
|
||||
return m_controller.getLeftAxisUpTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
|
||||
return m_controller.getLeftAxisDownTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
|
||||
return m_controller.getLeftAxisRightTrigger();
|
||||
}
|
||||
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
|
||||
return m_controller.getLeftAxisLeftTrigger();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user