diff --git a/src/main/deploy/nodeselector/index.html b/src/main/deploy/nodeselector/index.html index 9c0c9295..96eadd4a 100644 --- a/src/main/deploy/nodeselector/index.html +++ b/src/main/deploy/nodeselector/index.html @@ -21,55 +21,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
H1H2H3H4H5H6H7H8H9
M1M2M3M4M5M6M7M8M9
L1L2L3L4L5L6L7L8L9
-
-
0:00
-
-
CONFIRM
-
-
- - - -
-
+ \ No newline at end of file diff --git a/src/main/deploy/nodeselector/index.js b/src/main/deploy/nodeselector/index.js index 7aed9e7c..aaaf1237 100644 --- a/src/main/deploy/nodeselector/index.js +++ b/src/main/deploy/nodeselector/index.js @@ -1,86 +1,8 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - import { NT4_Client } from "./NT4.js"; -const nodeTopic = "/nodeselector/node_target"; -const nodeStatusTopic = "/nodeselector/node_status"; -const coneTippeddTopic = "/nodeselector/cone_tipped"; -const matchTimeTopic = "/nodeselector/match_time"; -const isAutoTopic = "/nodeselector/is_auto"; - -let active = null; -let tipped = false; -let matchTime = 0; -let isAuto = false; -let nodeStatus = Array(27).fill(false); - -function rowIdFromId(row_num) { - let a = ["#row-low", "#row-med", "#row-high"]; - return a[row_num]; -} - -function displayActive(index) { - if (index !== null) { - active = index; - $(".node.active").removeClass('active'); - let row = rowIdFromId(index[0]); - $(row).find("td").eq(index[1]).addClass("active"); - } - console.log(active); -} - -function sendTarget(row, column) { - // alert(row + ' ' + column); - if ([row, column] !== active) { - // if (row !== active[0] && column !== active[1]) { - displayActive([row, column]); - client.addSample(nodeTopic, [row, column]); - } -} - -function displayTipped(newTipped) { - // if (newTipped != tipped) { - tipped = newTipped; - if (tipped) { - $(".cone-orientation").addClass("tipped"); - } else { - $(".cone-orientation").removeClass("tipped"); - } - // } -} - -function displayTime(time, isAuto) { - let element = $("#time"); - if (isAuto) { - element.addClass("auto"); - element.removeClass("teleop-1 teleop-2 teleop-3"); - } else if (time > 30 || time == 0) { - element.addClass("teleop-1"); - element.removeClass("auto teleop-2 teleop-3"); - } else if (time > 15) { - element.addClass("teleop-2"); - element.removeClass("teleop-1 auto teleop-3"); - } else { - element.addClass("teleop-3"); - element.removeClass("teleop-1 teleop-2 auto"); - } - let secondsString = (time % 60).toString(); - if (secondsString.length == 1) { - secondsString = "0" + secondsString; - } - element.text(Math.floor(time / 60).toString() + ":" + secondsString); -} - -function toggleTipped() { - tipped = !tipped; - displayTipped(tipped); - client.addSample(coneTippeddTopic, tipped); -} +const keyDownTopic = "/input/keyboardDown"; +const keyUpTopic = "/input/keyboardUp"; +const mouseMoveTopic = "/input/mouseDelta"; let client = new NT4_Client( window.location.hostname, @@ -92,35 +14,7 @@ let client = new NT4_Client( // Topic unannounce }, (topic, timestamp, value) => { - // New data - if (topic.name === nodeTopic) { - document.body.style.backgroundColor = "white"; - displayActive(value); - } else if (topic.name == nodeStatusTopic) { - nodeStatus = value; - console.log(value); - for (let i = 0; i < value.length; i++) { - let row = Math.floor(i / 9); - let col = i - (row * 9); - let rowId = rowIdFromId(row); - let colId = $(rowId).find("td").eq(col); - if (value[i]) { - colId.addClass("confirmed"); - } else { - colId.removeClass("confirmed"); - } - } - } else if (topic.name === coneTippeddTopic) { - console.log(value); - displayTipped(value); - } else if (topic.name === matchTimeTopic) { - matchTime = value; - displayTime(matchTime, isAuto); - console.log(matchTime); - } else if (topic.name === isAutoTopic) { - isAuto = value; - displayTime(matchTime, isAuto); - } + }, () => { // Connected @@ -129,103 +23,51 @@ let client = new NT4_Client( () => { // Disconnected document.body.style.backgroundColor = "red"; - displayActive(null); - displayTipped(false); - displayTime(0, false); } ); window.addEventListener("load", () => { // Start NT connection - client.subscribe( - [ - nodeTopic, - nodeStatusTopic, - coneTippeddTopic, - matchTimeTopic, - isAutoTopic, - ], - false, - false, - 0.02 - ); - client.publishTopic(nodeTopic, "int[]"); - client.publishTopic(nodeStatusTopic, "boolean[]"); - client.publishTopic(coneTippeddTopic, "boolean"); + client.publishTopic(keyDownTopic, "string"); + client.publishTopic(keyUpTopic, "string"); + client.publishTopic(mouseMoveTopic, "double[]"); client.connect(); +}); - // Add node click listeners - $(".node.low").click(function () { - sendTarget(0, $(this).index()); - }); - $(".node.med").click(function () { - sendTarget(1, $(this).index()); - }); - $(".node.high").click(function () { - sendTarget(2, $(this).index()); - }); - $("#confirm-node").click(function () { - let row = active[0]; - let column = active[1]; - let rowId = rowIdFromId(row); - let colId = $(rowId).find("td").eq(column); - let index = row * 9 + column; - nodeStatus[index] = !nodeStatus[index]; - if (nodeStatus[index]) { - colId.addClass("confirmed"); - } else { - colId.removeClass("confirmed"); - } - client.addSample(nodeStatusTopic, nodeStatus); - console.log(nodeStatus); +document.addEventListener("keydown", (ev) => { + client.addSample(keyDownTopic, ""); + client.addSample(keyDownTopic, ev.key.toLowerCase()); +}); + +document.addEventListener("keyup", (ev) => { + client.addSample(keyUpTopic, ""); + client.addSample(keyUpTopic, ev.key.toLowerCase()); +}); + +let lock_button = document.getElementById("lock"); +lock_button.addEventListener("click", () => { + let locked = lock_button.requestPointerLock({ + unadjustedMovement: true }); - // each((cell, index) => { - // cell.addEventListener("click", () => { - // sendActive(index); - // console.log("click node " + JSON.stringify(cell) + ' ' + index) - // }); - // cell.addEventListener("contextmenu", (event) => { - // event.preventDefault(); - // sendActive(index); - // }); - // }); + if (!locked) { + locked = lock_button.requestPointerLock(); + } +}); - // Add node touch listeners - // [("touchstart", "touchmove")].forEach((eventString) => { - // document.body.addEventListener(eventString, (event) => { - // if (event.touches.length > 0) { - // let x = event.touches[0].clientX; - // let y = event.touches[0].clientY; - // Array.from(document.getElementsByClassName("node")).forEach( - // (cell, index) => { - // let rect = cell.getBoundingClientRect(); - // if ( - // x >= rect.left && - // x <= rect.right && - // y >= rect.top && - // y <= rect.bottom - // ) { - // sendActive(index); - // } - // } - // ); - // } - // }); - // }); +lock_button.addEventListener("mousemove", (ev) => { + client.addSample(mouseMoveTopic, []); + client.addSample(mouseMoveTopic, [ev.movementX, ev.movementY]); +}); - // Add cone orientation listeners - const coneOrientationDiv = $(".cone-orientation"); - coneOrientationDiv.click(function () { - toggleTipped(); - }); - // coneOrientationDiv.addEventListener("contextmenu", (event) => { - // event.preventDefault(); - // toggleTipped(); - // }); - // coneOrientationDiv.addEventListener("touchstart", () => { - // event.preventDefault(); - // toggleTipped(); - // }); +document.body.addEventListener("mousedown", (ev) => { + client.addSample(keyDownTopic, ""); + client.addSample(keyDownTopic, "mouse" + ev.button); }); + +document.body.addEventListener("mouseup", (ev) => { + client.addSample(keyUpTopic, ""); + client.addSample(keyUpTopic, "mouse" + ev.button); +}); + diff --git a/src/main/java/frc/lib/util/KeyboardAndMouse.java b/src/main/java/frc/lib/util/KeyboardAndMouse.java new file mode 100644 index 00000000..10eccc76 --- /dev/null +++ b/src/main/java/frc/lib/util/KeyboardAndMouse.java @@ -0,0 +1,138 @@ +package frc.lib.util; + +import java.util.EnumSet; +import java.util.HashMap; +import java.util.Map; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class KeyboardAndMouse { + + private static KeyboardAndMouse keyboard = new KeyboardAndMouse(); + private double deltaX = 0, deltaY = 0; + private double x = 0, y = 0; + + private KeyboardAndMouse() { + NetworkTableInstance instance = NetworkTableInstance.getDefault(); + instance.addListener(instance.getTopic("/input/keyboardDown"), + EnumSet.of(NetworkTableEvent.Kind.kValueRemote), (ev) -> { + String s = ev.valueData.value.getString(); + if (s.isEmpty()) { // an empty value is sent to allow repeat values + return; + } + this.keys.put(s, true); + }); + instance.addListener(instance.getTopic("/input/keyboardUp"), + EnumSet.of(NetworkTableEvent.Kind.kValueRemote), (ev) -> { + String s = ev.valueData.value.getString(); + if (s.isEmpty()) { // an empty value is sent to allow repeat values + return; + } + this.keys.put(s, false); + }); + instance.addListener(instance.getTopic("/input/mouseDelta"), + EnumSet.of(NetworkTableEvent.Kind.kValueRemote), (ev) -> { + double[] s = ev.valueData.value.getDoubleArray(); + if (s.length != 2) { // an empty value is sent to allow repeat values + return; + } + this.deltaX += s[0]; + this.deltaY += s[1]; + }); + } + + public static KeyboardAndMouse getInstance() { + return keyboard; + } + + private Map keys = new HashMap<>(); + private Map lpKeys = new HashMap<>(); + + public Trigger key(String name) { + return new Trigger(() -> { + return this.keys.getOrDefault(name, false); + }); + } + + public LowPassKey lowPassKey(String name) { + lpKeys.putIfAbsent(name, new LowPassKey(key(name))); + return lpKeys.get(name); + } + + public WASD wasd(String up, String left, String down, String right) { + return new WASD(lowPassKey(up), lowPassKey(left), lowPassKey(down), lowPassKey(right)); + } + + public double getX() { + return this.x; + } + + public double getY() { + return this.y; + } + + public void update() { + for (var v : this.lpKeys.values()) { + v.update(); + } + resetMouse(); + } + + public void resetMouse() { + this.x = deltaX; + this.y = deltaY; + this.deltaX = 0; + this.deltaY = 0; + } + + public static class LowPassKey { + + Trigger trigger; + double currentValue; + + private static double filterConstant = 1.0 / 25.0; + + private LowPassKey(Trigger trigger) { + this.trigger = trigger; + this.currentValue = 0.0; + } + + public double get() { + return this.currentValue; + } + + private void update() { + double newValue = 0.0; + if (trigger.getAsBoolean()) { + newValue = 1.0; + } + double dir = newValue - this.currentValue; + this.currentValue = MathUtil.clamp( + this.currentValue + Math.signum(dir) * Math.min(Math.abs(dir), filterConstant), 0.0, + 1.0); + } + + } + + public static class WASD { + LowPassKey left, right, up, down; + + private WASD(LowPassKey up, LowPassKey left, LowPassKey down, LowPassKey right) { + this.left = left; + this.right = right; + this.up = up; + this.down = down; + } + + public double getX() { + return right.get() - left.get(); + } + + public double getY() { + return up.get() - down.get(); + } + } + +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 67738608..6be117d0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,8 +18,11 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.util.KeyboardAndMouse; +import frc.lib.util.KeyboardAndMouse.WASD; import frc.lib.util.Scoring; import frc.lib.util.Scoring.GamePiece; import frc.lib.util.ctre.CTREConfigs; @@ -54,6 +57,8 @@ public class Robot extends TimedRobot { public IntegerPublisher timePublisher = table.getIntegerTopic("match_time").publish(); public BooleanPublisher isAutoPublisher = table.getBooleanTopic("is_auto").publish(); + WASD wasd; + // private Ultrasonic ultrasonic = new Ultrasonic(); /** * This function is run when the robot is first started up and should be used for any @@ -61,6 +66,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + wasd = KeyboardAndMouse.getInstance().wasd("w", "a", "s", "d"); Arrays.fill(node_status, false); ctreConfigs = new CTREConfigs(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our @@ -84,6 +90,9 @@ public void robotInit() { */ @Override public void robotPeriodic() { + KeyboardAndMouse.getInstance().update(); + SmartDashboard.putNumber("LR", wasd.getX()); + SmartDashboard.putNumber("UD", wasd.getY()); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic diff --git a/src/main/java/frc/robot/commands/drive/TeleopSwerve.java b/src/main/java/frc/robot/commands/drive/TeleopSwerve.java index 1f078b8d..1a065575 100644 --- a/src/main/java/frc/robot/commands/drive/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/drive/TeleopSwerve.java @@ -1,10 +1,11 @@ package frc.robot.commands.drive; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.lib.math.Conversions; +import frc.lib.util.KeyboardAndMouse; +import frc.lib.util.KeyboardAndMouse.LowPassKey; +import frc.lib.util.KeyboardAndMouse.WASD; import frc.robot.Constants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Swerve; @@ -19,6 +20,8 @@ public class TeleopSwerve extends CommandBase { private Swerve swerveDrive; private CommandXboxController controller; private Arm arm; + private WASD wasd; + private LowPassKey shift, ctrl; /** * Creates an command for driving the swerve drive during tele-op @@ -29,6 +32,10 @@ public class TeleopSwerve extends CommandBase { */ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolean fieldRelative, boolean openLoop, Arm arm) { + KeyboardAndMouse kam = KeyboardAndMouse.getInstance(); + this.wasd = kam.wasd("w", "a", "s", "d"); + this.shift = kam.lowPassKey("shift"); + this.ctrl = kam.lowPassKey("ctrl"); this.swerveDrive = swerveDrive; addRequirements(swerveDrive); this.fieldRelative = fieldRelative; @@ -39,19 +46,23 @@ public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolea @Override public void execute() { - double yaxis = -controller.getLeftY(); - double xaxis = -controller.getLeftX(); - double raxis = -controller.getRightX(); + double yaxis = wasd.getY(); + double xaxis = wasd.getX(); + double raxis = KeyboardAndMouse.getInstance().getX() * 0.01; /* Deadbands */ // yaxis = MathUtil.applyDeadband(yaxis, Constants.STICK_DEADBAND); - yaxis = Conversions.applySwerveCurve(yaxis, Constants.STICK_DEADBAND); + // yaxis = Conversions.applySwerveCurve(yaxis, Constants.STICK_DEADBAND); // xaxis = MathUtil.applyDeadband(xaxis, Constants.STICK_DEADBAND); - xaxis = Conversions.applySwerveCurve(xaxis, Constants.STICK_DEADBAND); - raxis = MathUtil.applyDeadband(raxis, Constants.STICK_DEADBAND); + // xaxis = Conversions.applySwerveCurve(xaxis, Constants.STICK_DEADBAND); + // raxis = MathUtil.applyDeadband(raxis, Constants.STICK_DEADBAND); double angle_speed = Constants.Swerve.MAX_ANGULAR_VELOCITY; double speed = Constants.Swerve.MAX_SPEED; + + speed *= (1.0 - shift.get()) * 0.2; + speed *= (1.0 - ctrl.get()) * 0.4; + if (arm.getArmAngle() > -70) { angle_speed /= 3; speed *= 0.80;