diff --git a/.vscode/tasks.json b/.vscode/tasks.json index cb272ab..5610f4f 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -21,6 +21,9 @@ { "label": "pathPlanner", "type": "shell", + "windows": { + "command": "explorer.exe shell:AppsFolder\\39956MichaelJansen.FRCPathPlanner_x7a2cjep50mda!pathplanner" + }, "osx": { "command": "open -n /Applications/PathPlanner.app" }, @@ -31,7 +34,8 @@ "panel": "dedicated", "showReuseMessage": false, "clear": true - } + }, + "problemMatcher": [] } ] } \ No newline at end of file diff --git a/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 876e42d..c9cab89 100644 --- a/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -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); + } } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 1c2ce22..f883fd3 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -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();