added java debugging visualizer tool
ParkerTenBroeck
parent
423ff5db8a
commit
9c9583b5a3
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
|
@ -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<String> 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();
|
||||
});
|
||||
}
|
||||
}
|
||||
|
|
@ -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> T packet(int id, Consumer<ByteBuffer> setup, Function<ByteBuffer, T> 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
|
||||
);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue