From 9c9583b5a32b4f39e8f10d97acd08c845e4e0853 Mon Sep 17 00:00:00 2001 From: ParkerTenBroeck <51721964+ParkerTenBroeck@users.noreply.github.com> Date: Wed, 16 Apr 2025 23:36:41 -0400 Subject: [PATCH] added java debugging visualizer tool --- jdbg/.gitignore | 31 +++++++ jdbg/src/Gui.java | 190 +++++++++++++++++++++++++++++++++++++++++ jdbg/src/Main.java | 12 +++ jdbg/src/PidPanel.java | 120 ++++++++++++++++++++++++++ jdbg/src/Robot.java | 113 ++++++++++++++++++++++++ 5 files changed, 466 insertions(+) create mode 100644 jdbg/.gitignore create mode 100644 jdbg/src/Gui.java create mode 100644 jdbg/src/Main.java create mode 100644 jdbg/src/PidPanel.java create mode 100644 jdbg/src/Robot.java diff --git a/jdbg/.gitignore b/jdbg/.gitignore new file mode 100644 index 0000000..e80d5dc --- /dev/null +++ b/jdbg/.gitignore @@ -0,0 +1,31 @@ +### IntelliJ IDEA ### +out/ +!**/src/main/**/out/ +!**/src/test/**/out/ + +### Eclipse ### +.apt_generated +.classpath +.factorypath +.project +.settings +.springBeans +.sts4-cache +bin/ +!**/src/main/**/bin/ +!**/src/test/**/bin/ + +### NetBeans ### +/nbproject/private/ +/nbbuild/ +/dist/ +/nbdist/ +/.nb-gradle/ + +### VS Code ### +.vscode/ + +### Mac OS ### +.DS_Store + +*.iml diff --git a/jdbg/src/Gui.java b/jdbg/src/Gui.java new file mode 100644 index 0000000..3c7a81d --- /dev/null +++ b/jdbg/src/Gui.java @@ -0,0 +1,190 @@ +import javax.swing.*; +import java.awt.*; +import java.awt.event.*; +import java.io.IOException; +import java.net.SocketException; +import java.net.UnknownHostException; + +public class Gui extends JFrame implements KeyListener, MouseWheelListener { + private VisualPanel visualPanel; + private JButton zeroButton; + private Robot robot; + + public Gui() throws SocketException, UnknownHostException { + robot = new Robot(); + + setTitle("Wrobot"); + setSize(600, 600); + setDefaultCloseOperation(EXIT_ON_CLOSE); + setLayout(new BorderLayout()); + + visualPanel = new VisualPanel(); + add(visualPanel, BorderLayout.CENTER); + + zeroButton = new JButton("Zero"); + zeroButton.addActionListener(e -> { + try { + robot.sendZero(); + new Thread(() -> { + var sum = 0.0; + int number = 500; + for(int i = 0; i < number; i ++){ + try { + sum += robot.getEverything()[19-1]; + } catch (IOException ex) { + throw new RuntimeException(ex); + } + } + System.out.println(sum/number); + }).start(); + } catch (IOException ex) { + throw new RuntimeException(ex); + } + }); + zeroButton.setFocusable(false); + add(zeroButton, BorderLayout.SOUTH); + + + setFocusable(true); + addKeyListener(this); + addMouseWheelListener(this); + add(new PidPanel(robot), BorderLayout.EAST); + + setVisible(true); + + new Thread(() -> { + while(true){ + try{ + var data = robot.getDataPlus(); + visualPanel.updateData(data); + Thread.sleep(20); + }catch (Exception e){ + e.printStackTrace(); + } + } + }).start(); + + } + + + @Override + public void keyPressed(KeyEvent e) { + int key = e.getKeyCode(); + float panStep = 20; + + switch (key) { + case KeyEvent.VK_W -> visualPanel.offsetY += panStep; + case KeyEvent.VK_S -> visualPanel.offsetY -= panStep; + case KeyEvent.VK_A -> visualPanel.offsetX += panStep; + case KeyEvent.VK_D -> visualPanel.offsetX -= panStep; + case KeyEvent.VK_PLUS, KeyEvent.VK_EQUALS -> visualPanel.zoom *= 1.1; + case KeyEvent.VK_MINUS -> visualPanel.zoom /= 1.1; + } + + repaint(); + } + + @Override public void keyReleased(KeyEvent e) {} + @Override public void keyTyped(KeyEvent e) {} + + @Override + public void mouseWheelMoved(MouseWheelEvent e) { + int notches = e.getWheelRotation(); + float factor = (notches > 0) ? 0.9f : 1.1f; + visualPanel.zoom *= factor; + repaint(); + } + + class VisualPanel extends JPanel{ + + private float zoom = 20f; // pixels per unit + private float offsetX = 0, offsetY = 0; // pan + private Robot.DataPacketPlus dpp = new Robot.DataPacketPlus(0,0,0,0,0,0,0); + + public VisualPanel() { + + addMouseListener(new MouseAdapter() { + @Override + public void mouseClicked(MouseEvent e) { + Point screenPoint = e.getPoint(); + float worldX = (screenPoint.x - getWidth() / 2f - offsetX) / zoom; + float worldY = -((screenPoint.y - getHeight() / 2f - offsetY) / zoom); + try { + robot.sendTargetPos(worldX, worldY); + } catch (IOException ex) { + throw new RuntimeException(ex); + } + } + }); + } + + public void updateData(Robot.DataPacketPlus dpp) { + this.dpp = dpp; + repaint(); + } + + @Override + protected void paintComponent(Graphics g) { + super.paintComponent(g); + drawScene((Graphics2D) g); + } + + private void drawScene(Graphics2D g) { + g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); + + int width = getWidth(); + int height = getHeight(); + + g.setColor(Color.WHITE); + g.fillRect(0, 0, width, height); + + g.translate((double) width / 2 + offsetX, (double) height / 2 + offsetY); + g.scale(1, -1); // Y-up + + int px = Math.round(dpp.x() * zoom); + int py = Math.round(dpp.y() * zoom); + + int pt_x = Math.round(dpp.t_x() * zoom); + int pt_y = Math.round(dpp.t_y() * zoom); + + // Target + g.setColor(Color.GRAY); + g.fillOval(pt_x-3, pt_y-3, 6, 6); + + // Position dot + g.setColor(Color.BLUE); + g.fillOval(px - 5, py - 5, 10, 10); + + // Yaw direction + { + float yawRad = (float) Math.toRadians(dpp.yaw()); + int dx = (int) (30 * Math.cos(yawRad)); + int dy = (int) (30 * Math.sin(yawRad)); + g.setColor(Color.RED); + g.drawLine(px, py, px + dx, py + dy); + } + + // Target Yaw direction + { + float rad = (float) Math.toRadians(dpp.t_yaw()); + int dx = (int) (30 * Math.cos(rad)); + int dy = (int) (30 * Math.sin(rad)); + g.setColor(Color.GREEN); + g.drawLine(px, py, px + dx, py + dy); + } + + // Line to origin + g.setColor(Color.GREEN.darker()); + g.drawLine(pt_x, pt_y, px, py); + + // Flip back to draw text + g.scale(1, -1); + g.setColor(Color.BLACK); + g.drawString(String.format("Position: (%.2f, %.2f)", dpp.x(), dpp.y()), -getWidth() / 2 + 10, -getHeight() / 2 + 20); + g.drawString(String.format("Yaw: %.2f°", dpp.yaw()), -getWidth() / 2 + 10, -getHeight() / 2 + 40); + g.drawString(String.format("Target Yaw: %.2f°", dpp.t_yaw()), -getWidth() / 2 + 10, -getHeight() / 2 + 60); + g.drawString(String.format("Zoom: %.1f", zoom), -getWidth() / 2 + 10, -getHeight() / 2 + 80); + } + } +} + diff --git a/jdbg/src/Main.java b/jdbg/src/Main.java new file mode 100644 index 0000000..16258d0 --- /dev/null +++ b/jdbg/src/Main.java @@ -0,0 +1,12 @@ +public class Main { + + public static void main(String[] args) { + javax.swing.SwingUtilities.invokeLater(() -> { + try { + new Gui(); + } catch (Exception e) { + throw new RuntimeException(e); + } + }); + } +} diff --git a/jdbg/src/PidPanel.java b/jdbg/src/PidPanel.java new file mode 100644 index 0000000..7c5d073 --- /dev/null +++ b/jdbg/src/PidPanel.java @@ -0,0 +1,120 @@ +import javax.swing.*; +import java.awt.*; + +public class PidPanel extends JPanel { + + public final double SCALE = 10000.0; + + public PidPanel(Robot robot){ + String[] pidNames = {"angle", "pos", "turn"}; + JComboBox indexDropdown = new JComboBox<>(pidNames); + + JSlider kpSlider = new JSlider(0, (int)(SCALE*10), 0); + JSlider kiSlider = new JSlider(0, (int)(SCALE*150), 0); + JSlider kdSlider = new JSlider(0, (int)(SCALE*2), 0); + + JLabel kpValue = new JLabel("0.00"); + JLabel kiValue = new JLabel("0.00"); + JLabel kdValue = new JLabel("0.00"); + + kpSlider.addChangeListener(e -> kpValue.setText(String.format("%.5f", kpSlider.getValue() / SCALE))); + kiSlider.addChangeListener(e -> kiValue.setText(String.format("%.5f", kiSlider.getValue() / SCALE))); + kdSlider.addChangeListener(e -> kdValue.setText(String.format("%.5f", kdSlider.getValue() / SCALE))); + + JRadioButton directButton = new JRadioButton("Direct", true); + JRadioButton reverseButton = new JRadioButton("Reverse"); + ButtonGroup dirGroup = new ButtonGroup(); + dirGroup.add(directButton); + dirGroup.add(reverseButton); + + JButton getPidButton = new JButton("Get PID"); + JButton setPidButton = new JButton("Set PID"); + + this.setLayout(new BoxLayout(this, BoxLayout.Y_AXIS)); + this.setBorder(BorderFactory.createTitledBorder("PID Settings")); + this.setAlignmentX(Component.LEFT_ALIGNMENT); + this.setPreferredSize(new Dimension(600, 250)); + + // PID index selector + JPanel indexRow = new JPanel(new FlowLayout(FlowLayout.LEFT)); + indexRow.add(new JLabel("Index:")); + indexRow.add(indexDropdown); + this.add(indexRow); + + // Kp row + JPanel kpRow = new JPanel(new BorderLayout()); + kpRow.add(new JLabel("Kp:"), BorderLayout.WEST); + kpRow.add(kpSlider, BorderLayout.CENTER); + kpRow.add(kpValue, BorderLayout.EAST); + kpRow.setBorder(BorderFactory.createEmptyBorder(2, 10, 2, 10)); + this.add(kpRow); + + // Ki row + JPanel kiRow = new JPanel(new BorderLayout()); + kiRow.add(new JLabel("Ki:"), BorderLayout.WEST); + kiRow.add(kiSlider, BorderLayout.CENTER); + kiRow.add(kiValue, BorderLayout.EAST); + kiRow.setBorder(BorderFactory.createEmptyBorder(2, 10, 2, 10)); + this.add(kiRow); + + // Kd row + JPanel kdRow = new JPanel(new BorderLayout()); + kdRow.add(new JLabel("Kd:"), BorderLayout.WEST); + kdRow.add(kdSlider, BorderLayout.CENTER); + kdRow.add(kdValue, BorderLayout.EAST); + kdRow.setBorder(BorderFactory.createEmptyBorder(2, 10, 2, 10)); + this.add(kdRow); + + // Direction row + JPanel dirRow = new JPanel(new FlowLayout(FlowLayout.LEFT)); + dirRow.add(new JLabel("Direction:")); + dirRow.add(directButton); + dirRow.add(reverseButton); + this.add(Box.createVerticalStrut(5)); + this.add(dirRow); + + // Buttons row + JPanel buttonRow = new JPanel(new FlowLayout(FlowLayout.LEFT)); + buttonRow.add(getPidButton); + buttonRow.add(setPidButton); + this.add(Box.createVerticalStrut(5)); + this.add(buttonRow); + + getPidButton.addActionListener(e -> { + try { + int index = indexDropdown.getSelectedIndex(); + Robot.PID pid = robot.getPID(index); + kpSlider.setValue((int) (pid.kp() * SCALE)); + kiSlider.setValue((int) (pid.ki() * SCALE)); + kdSlider.setValue((int) (pid.kd() * SCALE)); + if (pid.direction() == 0) { + directButton.setSelected(true); + } else { + reverseButton.setSelected(true); + } + } catch (Exception ex) { + ex.printStackTrace(); + JOptionPane.showMessageDialog(this, "Failed to get PID: " + ex.getMessage()); + } + }); + + setPidButton.addActionListener(e -> { + try { + int index = indexDropdown.getSelectedIndex(); + float kp = (float) (kpSlider.getValue() / SCALE); + float ki = (float) (kiSlider.getValue() / SCALE); + float kd = (float) (kdSlider.getValue() / SCALE); + int direction = directButton.isSelected() ? 0 : 1; + + robot.setPID(index, kp, ki, kd, direction); + } catch (Exception ex) { + ex.printStackTrace(); + JOptionPane.showMessageDialog(this, "Failed to set PID: " + ex.getMessage()); + } + }); + getPidButton.doClick(); + indexDropdown.addActionListener(a -> { + getPidButton.doClick(); + }); + } +} diff --git a/jdbg/src/Robot.java b/jdbg/src/Robot.java new file mode 100644 index 0000000..ced7e59 --- /dev/null +++ b/jdbg/src/Robot.java @@ -0,0 +1,113 @@ +import java.io.IOException; +import java.net.*; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.rmi.server.ExportException; +import java.util.function.Consumer; +import java.util.function.Function; + +public class Robot{ + DatagramSocket socket = new DatagramSocket(); + + final String ip; + final int port; + final InetAddress address; + private final ByteBuffer buf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN); + + + public Robot() throws SocketException, UnknownHostException { + this("192.168.5.11", 42069); + } + + public Robot(String ip, int port) throws SocketException, UnknownHostException { + this.ip = ip; + address = InetAddress.getByName(ip); + this.port = port; + } + + public synchronized T packet(int id, Consumer setup, Function func) throws IOException { + setup.accept(buf.clear().limit(buf.capacity()).putInt(id)); + DatagramPacket packet = new DatagramPacket(buf.array(), buf.position(), address, port); + socket.send(packet); + + buf.clear(); + DatagramPacket receivePacket = new DatagramPacket(buf.array(), buf.capacity()); + socket.setSoTimeout(1000); + socket.receive(receivePacket); + buf.limit(receivePacket.getLength()); + + if(buf.getInt() != id)throw new IOException("ID miss match"); + var seq = buf.getInt(); + return func.apply(buf); + } + + + public void sendZero() throws IOException{ + packet(0, bb -> {}, bb -> null); + } + + public record DataPacket(float yaw, float distance, float x, float y){} + + public DataPacket getData() throws IOException { + return packet(1, bb -> {}, bb -> { + float yaw = bb.getFloat(); + float distance = bb.getFloat(); + float posX = bb.getFloat(); + float posY = bb.getFloat(); + + return new DataPacket(yaw, distance, posX, posY); + }); + } + + public void sendTargetPos(float x, float y) throws IOException{ + packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null); + } + + public float[] getEverything() throws IOException{ + return packet(3, bb -> {}, bb -> { + float[] arr = new float[bb.remaining()/4]; + for(int i = 0; i < arr.length; i ++) + arr[i] = bb.getFloat(); + return arr; + }); + } + + + public record DataPacketPlus(float yaw, float t_yaw, float distance, float x, float y, float t_x, float t_y){} + + public DataPacketPlus getDataPlus() throws IOException { + return packet(4, + bb -> {}, + bb -> { + float yaw = bb.getFloat(); + float t_yaw = bb.getFloat(); + float distance = bb.getFloat(); + float x = bb.getFloat(); + float y = bb.getFloat(); + float t_x = bb.getFloat(); + float t_y = bb.getFloat(); + + return new DataPacketPlus(yaw, t_yaw, distance, x, y, t_x, t_y); + }); + } + + public record PID(float kp, float ki, float kd, int direction){} + public PID getPID(int index) throws IOException{ + return packet(5, + bb -> bb.putInt(index), + bb -> new PID(bb.getFloat(), bb.getFloat(), bb.getFloat(), bb.getInt()) + ); + } + + public void setPID(int index, float kp, float ki, float kd, int direction) throws IOException{ + packet(6, + bb -> bb + .putInt(index) + .putFloat(kp) + .putFloat(ki) + .putFloat(kd) + .putInt(direction), + bb -> null + ); + } +}