Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Remove D-pad test code in favor of edu.wpi.first.wpilibj2.command.button.POVButton

Browse Source 
Update DriverStation overload to include 2022.4.1 changes
Add windows support to PathPlanner task
This commit is contained in:
nathanrsxtn
2022-04-02 20:15:15 -06:00
parent 16ccd86843
commit b33084623e
3 changed files with 225 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+220 -22
View File
@@ -11,6 +11,12 @@ import edu.wpi.first.hal.MatchInfoData;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.util.datalog.BooleanArrayLogEntry;
import edu.wpi.first.util.datalog.BooleanLogEntry;
import edu.wpi.first.util.datalog.DataLog;
import edu.wpi.first.util.datalog.FloatArrayLogEntry;
import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
import java.nio.ByteBuffer;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.locks.Condition;
@@ -235,6 +241,163 @@ public class DriverStation {
}
}
private static class JoystickLogSender {
JoystickLogSender(DataLog log, int stick, long timestamp) {
m_stick = stick;
m_logButtons = new BooleanArrayLogEntry(log, "DS:joystick" + stick + "/buttons", timestamp);
m_logAxes = new FloatArrayLogEntry(log, "DS:joystick" + stick + "/axes", timestamp);
m_logPOVs = new IntegerArrayLogEntry(log, "DS:joystick" + stick + "/povs", timestamp);
appendButtons(timestamp);
appendAxes(timestamp);
appendPOVs(timestamp);
}
public void send(long timestamp) {
if (m_joystickButtonsCache[m_stick].m_count != m_joystickButtons[m_stick].m_count
|| m_joystickButtonsCache[m_stick].m_buttons != m_joystickButtons[m_stick].m_buttons) {
appendButtons(timestamp);
}
if (m_joystickAxesCache[m_stick].m_count != m_joystickAxes[m_stick].m_count) {
appendAxes(timestamp);
} else {
int count = m_joystickAxesCache[m_stick].m_count;
for (int i = 0; i < count; i++) {
if (m_joystickAxesCache[m_stick].m_axes[i] != m_joystickAxes[m_stick].m_axes[i]) {
appendAxes(timestamp);
break;
}
}
}
if (m_joystickPOVsCache[m_stick].m_count != m_joystickPOVs[m_stick].m_count) {
appendPOVs(timestamp);
} else {
int count = m_joystickPOVsCache[m_stick].m_count;
for (int i = 0; i < count; i++) {
if (m_joystickPOVsCache[m_stick].m_povs[i] != m_joystickPOVs[m_stick].m_povs[i]) {
appendPOVs(timestamp);
break;
}
}
}
}
void appendButtons(long timestamp) {
int count = m_joystickButtonsCache[m_stick].m_count;
if (m_sizedButtons == null || m_sizedButtons.length != count) {
m_sizedButtons = new boolean[count];
}
int buttons = m_joystickButtonsCache[m_stick].m_buttons;
for (int i = 0; i < count; i++) {
m_sizedButtons[i] = (buttons & (1 << i)) != 0;
}
m_logButtons.append(m_sizedButtons, timestamp);
}
void appendAxes(long timestamp) {
int count = m_joystickAxesCache[m_stick].m_count;
if (m_sizedAxes == null || m_sizedAxes.length != count) {
m_sizedAxes = new float[count];
}
System.arraycopy(m_joystickAxesCache[m_stick].m_axes, 0, m_sizedAxes, 0, count);
m_logAxes.append(m_sizedAxes, timestamp);
}
void appendPOVs(long timestamp) {
int count = m_joystickPOVsCache[m_stick].m_count;
if (m_sizedPOVs == null || m_sizedPOVs.length != count) {
m_sizedPOVs = new long[count];
}
for (int i = 0; i < count; i++) {
m_sizedPOVs[i] = m_joystickPOVsCache[m_stick].m_povs[i];
}
m_logPOVs.append(m_sizedPOVs, timestamp);
}
final int m_stick;
boolean[] m_sizedButtons;
float[] m_sizedAxes;
long[] m_sizedPOVs;
final BooleanArrayLogEntry m_logButtons;
final FloatArrayLogEntry m_logAxes;
final IntegerArrayLogEntry m_logPOVs;
}
private static class DataLogSender {
DataLogSender(DataLog log, boolean logJoysticks, long timestamp) {
m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp);
m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp);
m_logTest = new BooleanLogEntry(log, "DS:test", timestamp);
m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp);
// append initial control word values
m_wasEnabled = m_controlWordCache.getEnabled();
m_wasAutonomous = m_controlWordCache.getAutonomous();
m_wasTest = m_controlWordCache.getTest();
m_wasEstop = m_controlWordCache.getEStop();
m_logEnabled.append(m_wasEnabled, timestamp);
m_logAutonomous.append(m_wasAutonomous, timestamp);
m_logTest.append(m_wasTest, timestamp);
m_logEstop.append(m_wasEstop, timestamp);
if (logJoysticks) {
m_joysticks = new JoystickLogSender[kJoystickPorts];
for (int i = 0; i < kJoystickPorts; i++) {
m_joysticks[i] = new JoystickLogSender(log, i, timestamp);
}
} else {
m_joysticks = new JoystickLogSender[0];
}
}
public void send(long timestamp) {
// append control word value changes
boolean enabled = m_controlWordCache.getEnabled();
if (enabled != m_wasEnabled) {
m_logEnabled.append(enabled, timestamp);
}
m_wasEnabled = enabled;
boolean autonomous = m_controlWordCache.getAutonomous();
if (autonomous != m_wasAutonomous) {
m_logAutonomous.append(autonomous, timestamp);
}
m_wasAutonomous = autonomous;
boolean test = m_controlWordCache.getTest();
if (test != m_wasTest) {
m_logTest.append(test, timestamp);
}
m_wasTest = test;
boolean estop = m_controlWordCache.getEStop();
if (estop != m_wasEstop) {
m_logEstop.append(estop, timestamp);
}
m_wasEstop = estop;
// append joystick value changes
for (JoystickLogSender joystick : m_joysticks) {
joystick.send(timestamp);
}
}
boolean m_wasEnabled;
boolean m_wasAutonomous;
boolean m_wasTest;
boolean m_wasEstop;
final BooleanLogEntry m_logEnabled;
final BooleanLogEntry m_logAutonomous;
final BooleanLogEntry m_logTest;
final BooleanLogEntry m_logEstop;
final JoystickLogSender[] m_joysticks;
}
private static DriverStation instance = new DriverStation();
// Joystick User Data
@@ -258,6 +421,7 @@ public class DriverStation {
private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
private static final MatchDataSender m_matchDataSender;
private static DataLogSender m_dataLogSender;
// Internal Driver Station thread
private static Thread m_thread;
@@ -399,28 +563,28 @@ public class DriverStation {
boolean printTrace,
StackTraceElement[] stackTrace,
int stackTraceFirst) {
// String locString;
// if (stackTrace.length >= stackTraceFirst + 1) {
// locString = stackTrace[stackTraceFirst].toString();
// } else {
// locString = "";
// }
// StringBuilder traceString = new StringBuilder();
// if (printTrace) {
// boolean haveLoc = false;
// for (int i = stackTraceFirst; i < stackTrace.length; i++) {
// String loc = stackTrace[i].toString();
// traceString.append("\tat ").append(loc).append('\n');
// // get first user function
// if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
// locString = loc;
// haveLoc = true;
// }
// }
// }
// HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
// for (StackTraceElement traceElement : stackTrace)
// System.err.println("\tat " + traceElement);
/*
String locString;
if (stackTrace.length >= stackTraceFirst + 1) {
locString = stackTrace[stackTraceFirst].toString();
} else {
locString = "";
}
StringBuilder traceString = new StringBuilder();
if (printTrace) {
boolean haveLoc = false;
for (int i = stackTraceFirst; i < stackTrace.length; i++) {
String loc = stackTrace[i].toString();
traceString.append("\tat ").append(loc).append('\n');
// get first user function
if (!haveLoc && !loc.startsWith("edu.wpi.first")) {
locString = loc;
haveLoc = true;
}
}
}
HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
*/
java.util.logging.LogRecord logRecord = new java.util.logging.LogRecord(isError ? java.util.logging.Level.SEVERE : java.util.logging.Level.FINER, error.stripTrailing());
java.util.Optional.ofNullable(stackTrace).filter(s -> s.length >= stackTraceFirst + 1).map(s -> java.util.Arrays.copyOfRange(s, Math.min(Math.max(0, stackTraceFirst), s.length - 1), s.length - 1)).ifPresent(presentStackTrace -> {
logRecord.setSourceMethodName(presentStackTrace[0].getMethodName());
@@ -1279,6 +1443,7 @@ public class DriverStation {
m_controlWordMutex.unlock();
}
DataLogSender dataLogSender;
// lock joystick mutex to swap cache data
m_cacheDataMutex.lock();
try {
@@ -1308,12 +1473,17 @@ public class DriverStation {
MatchInfoData currentInfo = m_matchInfo;
m_matchInfo = m_matchInfoCache;
m_matchInfoCache = currentInfo;
dataLogSender = m_dataLogSender;
} finally {
m_cacheDataMutex.unlock();
}
wakeupWaitForData();
m_matchDataSender.sendMatchData();
if (dataLogSender != null) {
dataLogSender.send(WPIUtilJNI.now());
}
}
/**
@@ -1403,4 +1573,32 @@ public class DriverStation {
m_lastControlWordUpdate = now;
}
}
/**
* Starts logging DriverStation data to data log. Repeated calls are ignored.
*
* @param log data log
* @param logJoysticks if true, log joystick data
*/
@SuppressWarnings("PMD.NonThreadSafeSingleton")
public static void startDataLog(DataLog log, boolean logJoysticks) {
m_cacheDataMutex.lock();
try {
if (m_dataLogSender == null) {
m_dataLogSender = new DataLogSender(log, logJoysticks, WPIUtilJNI.now());
}
} finally {
m_cacheDataMutex.unlock();
}
}
/**
* Starts logging DriverStation data to data log, including joystick data. Repeated calls are
* ignored.
*
* @param log data log
*/
public static void startDataLog(DataLog log) {
startDataLog(log, true);
}
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
-21
View File
@@ -13,27 +13,6 @@ public class DeadbandedXboxController extends XboxController {
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
public int getDpadAngle() {
return getPOV(0);
}
public boolean getDPadLeft(){
return (getRawAxis(6) < -0.9);
}
public boolean getDPadRight(){
return (getRawAxis(6) > 0.9);
}
public boolean getDPadTop(){
return (getRawAxis(6) < 0.9);
}
public boolean getDPadBottom(){
return (getRawAxis(6) > -0.9);
}
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();