added java debugging visualizer tool

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

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);
+        }
+    }
+}
+

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);
+            }
+        });
+    }
+}

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<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();
+        });
+    }
+}

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> 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
+        );
+    }
+}

robot/encoder.cpp

@@ -42,8 +42,9 @@ void initEncoder(){
 }
 
 void updateEncoder(){
-  #define WHEEL_CIRCUM (2*PI*4.0625)
-  #define WHEEL_DISTANCE (7 + 12.0/16.0)
+  #define WHEEL_CIRCUM (PI*4.256)
+//  #define WHEEL_CIRCUM (1.0)
+  #define WHEEL_DISTANCE (8-0.125)
 
   wire2();
   int rawL = as5600_0.getCumulativePosition();
@@ -61,18 +62,16 @@ void updateEncoder(){
 
   float displacement = (d_left + d_right)/2;
   float oldAng = odom.angle;
-  odom.angle += (d_left-d_right)/(2*WHEEL_DISTANCE);
+  odom.angle += (d_left-d_right)/(WHEEL_DISTANCE);
   float ang = (odom.angle+oldAng)/2;
 
-  float dispx = odom.x-desiredPos.x;
-  float dispy = odom.y-desiredPos.y;
+  float dispx = desiredPos.x-odom.x;
+  float dispy = desiredPos.y-odom.y;
   posInput = sqrt(dispx*dispx+dispy*dispy);
 
   desiredYaw = atan2(dispy, dispx)*180/PI;
-  if(abs(fmod(desiredYaw-currentYaw, 180.0))<90){
+  if(abs(fmod(currentYaw-desiredYaw, 180.0))>90){
     desiredYaw = fmod(desiredYaw+180.0, 360.0);
-    if(desiredYaw>180)desiredYaw-=180.0;
-
     posInput = -posInput;
   }
 //  Serial.println(desiredYaw);

robot/headers.h

@@ -112,3 +112,4 @@ void initPID();
 
 void initWifi(bool host);
 void initServer();
+void updateServer();

robot/pid.cpp

@@ -7,7 +7,7 @@
 
 
 double angKp=4.0, angKi=106.75, angKd=0.0472;
-double posKp=0.96, posKi=1.28, posKd=0.5;
+double posKp=0.96, posKi=1.28, posKd=1.0;
 double turnKp=1, turnKi=5.0, turnKd=0.17;
 
 
@@ -18,7 +18,7 @@ double posInput, posOutput, posSetpoint;
 PID posPID(&posInput, &posOutput, &posSetpoint, posKp, posKi, posKd, P_ON_E, DIRECT);
 
 double turnInput, turnOutput, turnSetpoint;
-PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT);
+PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_M, DIRECT);
 
 PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
 
@@ -28,8 +28,8 @@ void initPID(){
   anglePID.SetMode(AUTOMATIC);
   anglePID.SetSampleTime(5);
 
-  posSetpoint = 0.5;
-  posPID.SetOutputLimits(-1, 1);
+  posSetpoint = 0;
+  posPID.SetOutputLimits(-2, 2);
   posPID.SetMode(AUTOMATIC);
   posPID.SetSampleTime(5);
   posPID.SetControllerDirection(DIRECT);
@@ -46,7 +46,7 @@ Speeds updatePID(){
   angleSetpoint = posOutput;
   anglePID.Compute();
 
-  float maxTurn = max(0.0f, 15.0f-abs((float)angleOutput));
+  float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput));
   turnPID.SetOutputLimits(-maxTurn, maxTurn);
   turnSetpoint = 0;
   turnInput = fmod(currentYaw-desiredYaw, 180);

robot/robot.ino

@@ -17,7 +17,7 @@ void wire2(){
 Servo left;
 Servo right;
 
-float angleOffset = -2.4241745;
+float angleOffset = 4.620817395210266;
 float desiredYaw = 0.0;
 float currentYaw = 0.0;
 
@@ -63,14 +63,14 @@ void loop() {
     double angle = ypr[1] * 180 / M_PI;
     if(angle>180) 
       angle -= 180;
-    angleInput = angle + angleOffset;
+    angleInput = angle - angleOffset;
   }
   
   updateEncoder();
   currentYaw=odom.angle*180/M_PI;
   
   updateDistance();
-
+  updateServer();
   
   Speeds speeds = updatePID();
 

robot/webserver.cpp

@@ -1,6 +1,14 @@
 #include "headers.h"
 
 #include <ESP8266WiFi.h>
+#include <WiFiUdp.h>
+
+WiFiUDP Udp;
+unsigned int localUdpPort = 42069; 
+
+
+#ifdef DO_WEB_SERVER
+
 #include <ESPAsyncTCP.h>
 #include <ESPAsyncWebServer.h>
 #include <ArduinoJson.h>
@@ -11,30 +19,8 @@
 
 AsyncWebServer server(80);
 
-void initWifi(bool host){
-  if(host){
-    WiFi.softAP("MEOW");
-    IPAddress IP = WiFi.softAPIP();
-    Serial.print("AP IP address: ");
-    Serial.println(IP);
-  }else{
-    Serial.print("Connecting to ");
-    Serial.println("Michael Loves CP");
-    WiFi.enableInsecureWEP(false);
-    WiFi.mode(WIFI_STA);
-    WiFi.setPhyMode(WIFI_PHY_MODE_11G);
-    WiFi.begin("Michael Loves CP", "cockandpavly20");
-    while(WiFi.status() != WL_CONNECTED){
-      delay(500);
-      Serial.print('.');  
-    }
-    Serial.println();
-  }
-  Serial.println(WiFi.localIP());
-}
 
-
-void initServer(){
+void initWebServer(){
   server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
     request->send_P(200, "text/html", index_html);
   });
@@ -85,7 +71,7 @@ void initServer(){
       angleSetpoint, angleInput, angleOutput,
       posSetpoint, posInput, posOutput,
       turnSetpoint, turnInput, turnOutput,
-      odom.left, odom.right, odom.x, odom.y, odom.angle,
+      odom.left, odom.right, odom.x, odom.y, odom.angle*180/M_PI,
       ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI, 
       euler[0]*180/M_PI, euler[1]*180/M_PI, euler[2]*180/M_PI, 
       gravity.x, gravity.y, gravity.z,
@@ -114,6 +100,16 @@ void initServer(){
     );
     request->send(200, "application/json", buff);
   });
+
+  server.on("/set_desired_pos", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
+  [](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
+    StaticJsonDocument<256> json;
+    deserializeJson(json, &bodyData[index], bodyLen);
+    desiredPos.x = json["x"];
+    desiredPos.y = json["y"];
+    
+    request->send(200);
+  });
   
   server.on("/set_pid", HTTP_POST, [](AsyncWebServerRequest *request){}, NULL,
   [](AsyncWebServerRequest *request, uint8_t *bodyData, size_t bodyLen, size_t index, size_t total) {
@@ -144,3 +140,213 @@ void initServer(){
    });
   server.begin();
 }
+
+#endif 
+
+void initServer(){
+  #ifdef DO_WEB_SERVER
+  initWebServer()
+  #endif 
+  
+  Udp.begin(localUdpPort);
+}
+
+struct ZeroPacket{
+  static constexpr uint32_t ID = 0;
+};
+struct GetDataPacket{
+  static constexpr uint32_t ID = 1;
+};
+struct SetTargetPacket{
+  static constexpr uint32_t ID = 2;
+  FVec2 pos;
+};
+struct EverythingPacket{
+  static constexpr uint32_t ID = 3;
+};
+struct GetDataPacketPlus{
+  static constexpr uint32_t ID = 4;
+};
+struct GetPIDPacket{
+  static constexpr uint32_t ID = 5;
+  uint32_t index;
+};
+struct SetPIDPacket{
+  static constexpr uint32_t ID = 6;
+  uint32_t index;
+  float kp,ki,kd;
+  uint32_t direction;
+};
+
+struct Packet{
+  uint32_t id;
+  union{
+    ZeroPacket zero;
+    GetDataPacket get_data;
+    SetTargetPacket set_target;
+    EverythingPacket everything;
+    GetDataPacketPlus get_data_plus;
+    GetPIDPacket get_pid_packet;
+    SetPIDPacket set_pid_packet;
+  } data;
+};
+
+struct DataPacket{
+    float yaw;
+    float distance;
+    FVec2 position;
+};
+
+
+struct DataPacketPlus{
+    float yaw;
+    float desiredYaw;
+    float distance;
+    FVec2 position;
+    FVec2 targetPosition;
+};
+
+void respond_data_packet(){
+    DataPacket dp;
+    dp.yaw = currentYaw;
+    dp.distance = distanceReading;
+    dp.position.x = odom.x;
+    dp.position.y = odom.y;
+    
+    Udp.write((const char*)&dp, sizeof(dp));
+}
+
+void respond_data_packet_plus(){
+    DataPacketPlus dp;
+    dp.yaw = currentYaw;
+    dp.desiredYaw = desiredYaw;
+    dp.distance = distanceReading;
+    dp.position.x = odom.x;
+    dp.position.y = odom.y;
+    dp.targetPosition.x = desiredPos.x;
+    dp.targetPosition.y = desiredPos.y;
+    
+    Udp.write((const char*)&dp, sizeof(dp));
+}
+
+void respond_everything_packet(){
+    float everything[] = {
+       (float)dbgState.motorTargetAngle, 
+      (float)distanceReading,
+      0.0, //encoder.position(),
+      angleSetpoint, angleInput, angleOutput,
+      posSetpoint, posInput, posOutput,
+      turnSetpoint, turnInput, turnOutput,
+      odom.left, odom.right, odom.x, odom.y, odom.angle*180/M_PI,
+      ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI, 
+      euler[0]*180/M_PI, euler[1]*180/M_PI, euler[2]*180/M_PI, 
+      gravity.x, gravity.y, gravity.z,
+      q.w, q.x, q.y, q.z,
+      aa.x, aa.y, aa.z,
+      gy.x, gy.y, gy.z,
+      aaReal.x, aaReal.y, aaReal.z,
+      aaWorld.x, aaWorld.y, aaWorld.z
+    };
+    
+    Udp.write((const char*)everything, sizeof(everything));
+}
+
+void set_pid(SetPIDPacket spid){
+  if(spid.index>PID_ARR_COUNT){
+    return;
+  }
+  
+  PID& pid = *pids[spid.index];
+  pid.SetTunings(spid.kp, spid.ki, spid.kd);
+  pid.SetControllerDirection(spid.direction);
+}
+
+void get_pid(GetPIDPacket gpid){
+  if(gpid.index>PID_ARR_COUNT){
+    return;
+  }
+  PID& pid = *pids[gpid.index];
+  struct {float kp,ki,kd; uint32_t direction;} pidData = {
+    .kp=pid.GetKp(),
+    .ki=pid.GetKi(),
+    .kd=pid.GetKd(),
+    .direction=pid.GetDirection()
+  };
+  
+  Udp.write((const char*)&pidData, sizeof(pidData));
+}
+
+bool handleUDP(){
+  int size = Udp.parsePacket();
+
+  constexpr size_t buffer_size = 256;
+  alignas(alignof(float)) char buffer[buffer_size];
+  
+  if (size>=4){
+    int len = Udp.read(buffer, buffer_size);
+
+    Packet* packet = (Packet*)buffer;
+
+    static uint32_t sequence = 0;
+    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
+    Udp.write((const char*)&packet->id, sizeof(packet->id));
+    Udp.write((const char*)&sequence, sizeof(sequence));
+    sequence++;
+
+    switch(packet->id){
+      case ZeroPacket::ID: 
+        zeroOdom(); 
+        desiredPos.x = 0; 
+        desiredPos.y = 0; 
+        break;  
+      case GetDataPacket::ID: 
+        respond_data_packet(); 
+        break;  
+      case SetTargetPacket::ID: 
+        desiredPos.x = packet->data.set_target.pos.x; 
+        desiredPos.y = packet->data.set_target.pos.y; 
+        break;  
+      case EverythingPacket::ID: 
+        respond_everything_packet(); 
+        break;
+      case GetDataPacketPlus::ID: 
+        respond_data_packet_plus(); 
+        break;   
+      case SetPIDPacket::ID:
+        set_pid(packet->data.set_pid_packet);
+        break;
+      case GetPIDPacket::ID:
+        get_pid(packet->data.get_pid_packet);
+        break;
+    }
+    Udp.endPacket();
+    return true;
+  }
+  return false;
+}
+
+void updateServer(){
+  while(handleUDP());
+}
+
+void initWifi(bool host){
+  if(host){
+    WiFi.softAP("MEOW");
+    IPAddress IP = WiFi.softAPIP();
+    Serial.print("AP IP address: ");
+    Serial.println(IP);
+  }else{
+    Serial.print("Connecting to ");
+    Serial.println("Michael Loves CP");
+    WiFi.enableInsecureWEP(false);
+    WiFi.mode(WIFI_STA);
+    WiFi.setPhyMode(WIFI_PHY_MODE_11G);
+    WiFi.begin("Michael Loves CP", "cockandpavly20");
+    while(WiFi.status() != WL_CONNECTED){
+      delay(500);
+      Serial.print('.');  
+    }
+    Serial.println();
+  }
+  Serial.println(WiFi.localIP());
+}