Skip to content

Commit

Permalink
Stop using DriverStation when nohaljni is passed (for OxplorerGUI)
Browse files Browse the repository at this point in the history
  • Loading branch information
nab138 committed Apr 4, 2024
1 parent 4827a0f commit c2d65e4
Show file tree
Hide file tree
Showing 8 changed files with 68 additions and 28 deletions.
7 changes: 1 addition & 6 deletions Pathfinding/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ plugins {
}

group 'me.nabdev.pathfinding'
version '0.12.0-SNAPSHOT-WITHJNI2'
version '0.12.1'

java {
withSourcesJar()
Expand All @@ -24,11 +24,6 @@ dependencies {
implementation wpi.java.deps.wpilib()
implementation wpi.java.vendor.java()


nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
simulationRelease wpi.sim.enableRelease()

implementation 'org.json:json:20231013'
}

Expand Down
16 changes: 8 additions & 8 deletions Pathfinding/src/main/java/me/nabdev/pathfinding/Pathfinder.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
import me.nabdev.pathfinding.structures.Obstacle;
import me.nabdev.pathfinding.structures.Path;
import me.nabdev.pathfinding.structures.Vertex;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;
import me.nabdev.pathfinding.utilities.FieldLoader.FieldData;
import me.nabdev.pathfinding.utilities.FieldLoader.ObstacleData;

Expand All @@ -18,7 +19,6 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

Expand Down Expand Up @@ -79,9 +79,9 @@ public class Pathfinder {
*/
private SearchAlgorithmType searchAlgorithmType;

private double lastMatchTime = DriverStation.getMatchTime();
private Optional<Alliance> lastAlliance = DriverStation.getAlliance();
private boolean lastIsAuto = DriverStation.isAutonomous();
private double lastMatchTime = DriverStationWrapper.getMatchTime();
private Optional<Alliance> lastAlliance = DriverStationWrapper.getAlliance();
private boolean lastIsAuto = DriverStationWrapper.isAutonomous();
// Every obstacle vertex (ORDER IS IMPORTANT)
ArrayList<Vertex> obstacleVertices = new ArrayList<>();
ArrayList<Vertex> uninflatedObstacleVertices = new ArrayList<>();
Expand Down Expand Up @@ -162,16 +162,16 @@ public Pathfinder(FieldData field, double pointSpacing, double cornerPointSpacin
*/
public void periodic() {
boolean shouldInvalidate = false;
if (DriverStation.getMatchTime() < endgameTime && !(lastMatchTime < endgameTime)) {
if (DriverStationWrapper.getMatchTime() < endgameTime && !(lastMatchTime < endgameTime)) {
shouldInvalidate = true;
}
lastMatchTime = DriverStation.getMatchTime();
Optional<Alliance> alliance = DriverStation.getAlliance();
lastMatchTime = DriverStationWrapper.getMatchTime();
Optional<Alliance> alliance = DriverStationWrapper.getAlliance();
if (!alliance.equals(lastAlliance)) {
lastAlliance = alliance;
shouldInvalidate = true;
}
boolean isAuto = DriverStation.isAutonomous();
boolean isAuto = DriverStationWrapper.isAutonomous();
if (isAuto != lastIsAuto) {
lastIsAuto = isAuto;
shouldInvalidate = true;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
package me.nabdev.pathfinding.modifiers;

import edu.wpi.first.wpilibj.DriverStation;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;

/**
* A modifier that keeps the obstacle active during the autonomous period
*/
public class ActiveAutoModifier extends ObstacleModifier {
@Override
public boolean isActive() {
return DriverStation.isAutonomous();
return DriverStationWrapper.isAutonomous();
}

@Override
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package me.nabdev.pathfinding.modifiers;

import edu.wpi.first.wpilibj.DriverStation;
import me.nabdev.pathfinding.Pathfinder;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;

/**
* A modifier that keeps the obstacle active during the endgame period
Expand All @@ -10,8 +10,8 @@ public class ActiveEndgameModifier extends ObstacleModifier {

@Override
public boolean isActive() {
if (DriverStation.isTeleop()) {
double matchTime = DriverStation.getMatchTime();
if (DriverStationWrapper.isTeleop()) {
double matchTime = DriverStationWrapper.getMatchTime();
if (matchTime <= Pathfinder.getEndgameTime()) {
return true;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

import java.util.Optional;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;

/**
* A modifier that keeps the obstacle active if it is the same alliance as the
Expand All @@ -23,10 +23,10 @@ public ActiveMyAllianceModifier(ModifierCollection collection) {

@Override
public boolean isActive() {
Optional<Alliance> alliance = DriverStation.getAlliance();
Optional<Alliance> alliance = DriverStationWrapper.getAlliance();
if (!alliance.isPresent())
return false;
if (alliance.get() == DriverStation.Alliance.Blue) {
if (alliance.get() == Alliance.Blue) {
return myCollection.hasModifier(ObstacleModifierTypes.BLUE_ALLIANCE);
} else {
return myCollection.hasModifier(ObstacleModifierTypes.RED_ALLIANCE);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

import java.util.Optional;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;

/**
* A modifier that keeps the obstacle active if it is not the same alliance as
Expand All @@ -23,10 +23,10 @@ public ActiveOtherAllianceModifier(ModifierCollection collection) {

@Override
public boolean isActive() {
Optional<Alliance> alliance = DriverStation.getAlliance();
Optional<Alliance> alliance = DriverStationWrapper.getAlliance();
if (!alliance.isPresent())
return false;
if (alliance.get() == DriverStation.Alliance.Blue) {
if (alliance.get() == Alliance.Blue) {
return myCollection.hasModifier(ObstacleModifierTypes.RED_ALLIANCE);
} else {
return myCollection.hasModifier(ObstacleModifierTypes.BLUE_ALLIANCE);
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package me.nabdev.pathfinding.modifiers;

import edu.wpi.first.wpilibj.DriverStation;
import me.nabdev.pathfinding.Pathfinder;
import me.nabdev.pathfinding.utilities.DriverStationWrapper;

/**
* A modifier that keeps the obstacle active during the teleop period, not
Expand All @@ -10,8 +10,8 @@
public class ActiveTeleModifier extends ObstacleModifier {
@Override
public boolean isActive() {
if (DriverStation.isTeleop()) {
double matchTime = DriverStation.getMatchTime();
if (DriverStationWrapper.isTeleop()) {
double matchTime = DriverStationWrapper.getMatchTime();
if (matchTime <= Pathfinder.getEndgameTime()) {
return false;
}
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
package me.nabdev.pathfinding.utilities;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;

import java.util.Optional;

public class DriverStationWrapper {
private static boolean canUseJni;
static {
canUseJni = !Boolean.getBoolean("nohaljni");
}

public static boolean isAutonomous() {
if (canUseJni) {
return DriverStation.isAutonomous();
} else {
return false;
}
}

public static boolean isTeleop() {
if (canUseJni) {
return DriverStation.isTeleop();
} else {
return true;
}
}

public static double getMatchTime() {
if (canUseJni) {
return DriverStation.getMatchTime();
} else {
return 0;
}
}

public static Optional<Alliance> getAlliance() {
if (canUseJni) {
return DriverStation.getAlliance();
} else {
return Optional.empty();
}
}
}

0 comments on commit c2d65e4

Please sign in to comment.