From e83243ade5a6befb4f3bae2809d630bccda98f32 Mon Sep 17 00:00:00 2001 From: ParkerTenBroeck <51721964+ParkerTenBroeck@users.noreply.github.com> Date: Wed, 23 Apr 2025 20:39:01 -0400 Subject: [PATCH] updated protocol, fixed turning being reversed --- jdbg/src/Future.java | 59 ++++++++++++++++++++++ jdbg/src/Gui.java | 110 +++++++++++++++++++++++++---------------- jdbg/src/PidPanel.java | 25 +++++----- jdbg/src/Robot.java | 85 +++++++++++++++++++++---------- robot/distance.cpp | 1 - robot/encoder.cpp | 25 ++++++++-- robot/headers.h | 2 +- robot/pid.cpp | 11 ++--- robot/robot.ino | 55 ++++++++++++++------- robot/webserver.cpp | 7 ++- 10 files changed, 264 insertions(+), 116 deletions(-) create mode 100644 jdbg/src/Future.java diff --git a/jdbg/src/Future.java b/jdbg/src/Future.java new file mode 100644 index 0000000..4eecd1e --- /dev/null +++ b/jdbg/src/Future.java @@ -0,0 +1,59 @@ +import java.util.concurrent.TimeoutException; +import java.util.function.Consumer; + +public abstract class Future { + private T val; + private E err; + private Consumer then = ignore -> { + this.accepted = false; + this.notifyAll(); + }; + private Consumer error = ignore -> { + this.accepted = false; + this.notifyAll(); + }; + private boolean accepted; + + protected synchronized void resolve(T val) { + this.val = val; + accepted = true; + then.accept(this.val); + } + + protected synchronized void resolve(E err) { + this.err = err; + accepted = true; + error.accept(this.err); + } + + public synchronized Future then(Consumer then) { + this.then = then; + if (this.val != null && !accepted) { + accepted = true; + this.then.accept(this.val); + } + return this; + } + + public synchronized Future error(Consumer error) { + this.error = error; + if (this.err != null && !accepted) { + accepted = true; + this.error.accept(this.err); + } + return this; + } + + public synchronized T await(long ms) throws InterruptedException, TimeoutException, E { + if (this.val == null && this.err == null) this.wait(ms); + if (this.val == null && this.err == null) throw new TimeoutException(); + if (accepted) throw new IllegalStateException("Value already accepted"); + accepted = true; + if (this.err != null) throw this.err; + return val; + } + + public T await() throws InterruptedException, E, TimeoutException { + return await(0); + } +} diff --git a/jdbg/src/Gui.java b/jdbg/src/Gui.java index 3c7a81d..cebeede 100644 --- a/jdbg/src/Gui.java +++ b/jdbg/src/Gui.java @@ -1,20 +1,24 @@ import javax.swing.*; import java.awt.*; import java.awt.event.*; -import java.io.IOException; import java.net.SocketException; import java.net.UnknownHostException; +import java.util.ArrayList; public class Gui extends JFrame implements KeyListener, MouseWheelListener { private VisualPanel visualPanel; private JButton zeroButton; + private JButton averageButton; private Robot robot; + record Pos(double x, double y){} + private final ArrayList maps = new ArrayList<>(); + public Gui() throws SocketException, UnknownHostException { robot = new Robot(); setTitle("Wrobot"); - setSize(600, 600); + setSize(1200, 600); setDefaultCloseOperation(EXIT_ON_CLOSE); setLayout(new BorderLayout()); @@ -23,27 +27,17 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { 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); - } + robot.sendZero().error(ex -> ex.printStackTrace()); + maps.clear(); }); zeroButton.setFocusable(false); add(zeroButton, BorderLayout.SOUTH); + averageButton = new JButton("Find Average Angle"); + averageButton.addActionListener( e -> averageAngle()); + averageButton.setFocusable(false); + add(averageButton, BorderLayout.NORTH); + setFocusable(true); addKeyListener(this); @@ -55,8 +49,7 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { new Thread(() -> { while(true){ try{ - var data = robot.getDataPlus(); - visualPanel.updateData(data); + visualPanel.updateData(robot.getDataPlus().await(1000)); Thread.sleep(20); }catch (Exception e){ e.printStackTrace(); @@ -66,6 +59,21 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { } + public void averageAngle(){ + new Thread(() -> { + var sum = 0.0; + int number = 500; + for(int i = 0; i < number; i ++){ + try { + sum += robot.getEverything().await()[19-1]; + } catch (Exception ex) { + throw new RuntimeException(ex); + } + } + System.out.println(sum/number); + }).start(); + } + @Override public void keyPressed(KeyEvent e) { @@ -77,8 +85,8 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { 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; + case KeyEvent.VK_PLUS, KeyEvent.VK_EQUALS -> visualPanel.zoom *= 1.1f; + case KeyEvent.VK_MINUS -> visualPanel.zoom /= 1.1f; } repaint(); @@ -108,18 +116,21 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { 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); - } + float worldY = -(screenPoint.y - getHeight() / 2f - offsetY) / zoom; + robot.sendTargetPos(worldX, worldY); } }); } public void updateData(Robot.DataPacketPlus dpp) { this.dpp = dpp; + if(dpp.distance()<2500){ + var x = dpp.x() + 0.0393701*dpp.distance()*Math.cos((dpp.yaw()+90)/180*Math.PI); + var y = dpp.y() + 0.0393701*dpp.distance()*Math.sin((dpp.yaw()+90)/180*Math.PI); + synchronized (maps){ + maps.add(new Pos(x, y)); + } + } repaint(); } @@ -135,26 +146,47 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { int width = getWidth(); int height = getHeight(); + + // Flip back to draw text + g.scale(1, 1); + 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); + g.setColor(Color.BLACK); + g.drawString(String.format("Position: (%.2f, %.2f)", dpp.x(), dpp.y()), -getWidth() / 2.0f + 10 - offsetX, -getHeight() / 2.0f + 20 - offsetY); + g.drawString(String.format("Yaw: %.2f°", dpp.yaw()), -getWidth() / 2.0f + 10 - offsetX, -getHeight() / 2.0f + 40 - offsetY); + g.drawString(String.format("Target Yaw: %.2f°", dpp.t_yaw()), -getWidth() / 2.0f - offsetX + 10, -getHeight() / 2.0f + 60 - offsetY); + g.drawString(String.format("Zoom: %.1f", zoom), -getWidth() / 2.0f - offsetX + 10, -getHeight() / 2.0f + 80 - offsetY); - int pt_x = Math.round(dpp.t_x() * zoom); - int pt_y = Math.round(dpp.t_y() * zoom); + g.scale(zoom, -zoom); // Y-up + + int px = Math.round(dpp.x()); + int py = Math.round(dpp.y()); + + int pt_x = Math.round(dpp.t_x()); + int pt_y = Math.round(dpp.t_y()); // Target g.setColor(Color.GRAY); - g.fillOval(pt_x-3, pt_y-3, 6, 6); + g.fillOval(pt_x - 3, pt_y - 3, 6, 6); // Position dot g.setColor(Color.BLUE); g.fillOval(px - 5, py - 5, 10, 10); + g.setColor(Color.ORANGE); + + ArrayList cln; + synchronized (maps) { + cln = new ArrayList<>(maps); + } + for (var pos : cln) { + g.fillOval((int) (pos.x - 1), (int) (pos.y - 2), 2, 2); + } + // Yaw direction { float yawRad = (float) Math.toRadians(dpp.yaw()); @@ -176,14 +208,6 @@ public class Gui extends JFrame implements KeyListener, MouseWheelListener { // 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/PidPanel.java b/jdbg/src/PidPanel.java index 7c5d073..bc90969 100644 --- a/jdbg/src/PidPanel.java +++ b/jdbg/src/PidPanel.java @@ -81,9 +81,8 @@ public class PidPanel extends JPanel { this.add(buttonRow); getPidButton.addActionListener(e -> { - try { - int index = indexDropdown.getSelectedIndex(); - Robot.PID pid = robot.getPID(index); + int index = indexDropdown.getSelectedIndex(); + robot.getPID(index).then(pid -> { kpSlider.setValue((int) (pid.kp() * SCALE)); kiSlider.setValue((int) (pid.ki() * SCALE)); kdSlider.setValue((int) (pid.kd() * SCALE)); @@ -92,25 +91,23 @@ public class PidPanel extends JPanel { } else { reverseButton.setSelected(true); } - } catch (Exception ex) { + }).error(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; + 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) { + robot.setPID(index, kp, ki, kd, direction).error(ex -> { ex.printStackTrace(); JOptionPane.showMessageDialog(this, "Failed to set PID: " + ex.getMessage()); - } + }); }); getPidButton.doClick(); indexDropdown.addActionListener(a -> { diff --git a/jdbg/src/Robot.java b/jdbg/src/Robot.java index ced7e59..32dbca8 100644 --- a/jdbg/src/Robot.java +++ b/jdbg/src/Robot.java @@ -2,53 +2,86 @@ import java.io.IOException; import java.net.*; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import java.rmi.server.ExportException; +import java.util.HashMap; import java.util.function.Consumer; import java.util.function.Function; public class Robot{ DatagramSocket socket = new DatagramSocket(); + private int sequence = 0; final String ip; final int port; final InetAddress address; - private final ByteBuffer buf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN); + private final ByteBuffer outBuf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN); + + private final HashMap> seqMap = new HashMap<>(); + + public static abstract class PFuture extends Future{ + protected abstract void resolve(ByteBuffer bb); + } public Robot() throws SocketException, UnknownHostException { - this("192.168.5.11", 42069); + this("192.168.4.1", 42069); } public Robot(String ip, int port) throws SocketException, UnknownHostException { this.ip = ip; address = InetAddress.getByName(ip); this.port = port; + + new Thread(() -> { + var inBuf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN); + while(true){ + try{ + inBuf.clear(); + DatagramPacket receivePacket = new DatagramPacket(inBuf.array(), inBuf.capacity()); + socket.receive(receivePacket); + inBuf.limit(receivePacket.getLength()); + + int squ = inBuf.getInt(); + var future = seqMap.remove(squ); + if(future!=null) future.resolve(inBuf); + }catch (Exception ignore){} + } + }).start(); } - 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); + public synchronized PFuture packet(int id, Consumer setup, Function func) { + var f = new PFuture(){ + @Override + protected synchronized void resolve(ByteBuffer bb) { + try{ + if(id!=bb.getInt()) + super.resolve(new Exception("ID Miss Match")); + else + super.resolve(func.apply(bb)); + }catch (Exception e){ + super.resolve(e); + } + } + }; + seqMap.put(sequence, f); + try{ + setup.accept(outBuf.clear().limit(outBuf.capacity()).putInt(sequence++).putInt(id)); + DatagramPacket packet = new DatagramPacket(outBuf.array(), outBuf.position(), address, port); + socket.send(packet); + }catch (Exception e){ + f.resolve(e); + } - 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); + return f; } - public void sendZero() throws IOException{ - packet(0, bb -> {}, bb -> null); + public PFuture sendZero() { + return packet(0, bb -> {}, bb -> null); } public record DataPacket(float yaw, float distance, float x, float y){} - public DataPacket getData() throws IOException { + public PFuture getData() { return packet(1, bb -> {}, bb -> { float yaw = bb.getFloat(); float distance = bb.getFloat(); @@ -59,11 +92,11 @@ public class Robot{ }); } - public void sendTargetPos(float x, float y) throws IOException{ - packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null); + public PFuture sendTargetPos(float x, float y) { + return packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null); } - public float[] getEverything() throws IOException{ + public PFuture getEverything() { return packet(3, bb -> {}, bb -> { float[] arr = new float[bb.remaining()/4]; for(int i = 0; i < arr.length; i ++) @@ -75,7 +108,7 @@ public class Robot{ 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 { + public PFuture getDataPlus() { return packet(4, bb -> {}, bb -> { @@ -92,15 +125,15 @@ public class Robot{ } public record PID(float kp, float ki, float kd, int direction){} - public PID getPID(int index) throws IOException{ + public PFuture getPID(int index) { 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, + public PFuture setPID(int index, float kp, float ki, float kd, int direction) { + return packet(6, bb -> bb .putInt(index) .putFloat(kp) diff --git a/robot/distance.cpp b/robot/distance.cpp index 0267705..bdf77bd 100644 --- a/robot/distance.cpp +++ b/robot/distance.cpp @@ -5,7 +5,6 @@ Adafruit_VL53L0X lox = Adafruit_VL53L0X(); float distanceReading = 0; void initDistance(){ - if (!lox.begin(0x29, false, &Wire1, Adafruit_VL53L0X::VL53L0X_SENSE_LONG_RANGE)) { Serial.println(F("Failed to boot VL53L0X")); while(1); diff --git a/robot/encoder.cpp b/robot/encoder.cpp index 9ddc3eb..f74ce99 100644 --- a/robot/encoder.cpp +++ b/robot/encoder.cpp @@ -62,19 +62,36 @@ void updateEncoder(){ float displacement = (d_left + d_right)/2; float oldAng = odom.angle; - odom.angle += (d_left-d_right)/(WHEEL_DISTANCE); + odom.angle += (d_right-d_left)/(WHEEL_DISTANCE); float ang = (odom.angle+oldAng)/2; + currentYaw=odom.angle*180/M_PI; 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(currentYaw-desiredYaw, 180.0))>90){ - desiredYaw = fmod(desiredYaw+180.0, 360.0); + desiredYaw = fmod(desiredYaw+360, 360); + currentYaw = fmod(fmod(currentYaw, 360)+360, 360); + + double yd = desiredYaw-currentYaw; + if (yd >= 180) + yd -= 360; + else if (yd <= -180) + yd += 360; + + //display + desiredYaw = currentYaw+yd; + turnInput = yd; + if (abs(yd) > 90) { + turnInput = fmod((yd<0?1:-1)*180+yd, 180); posInput = -posInput; + + //display + desiredYaw += 180; } -// Serial.println(desiredYaw); + + posInput *= cos(turnInput*PI/180); odom.x += (float)(cos(ang)*displacement); odom.y += (float)(sin(ang)*displacement); diff --git a/robot/headers.h b/robot/headers.h index 338ad9d..1707192 100644 --- a/robot/headers.h +++ b/robot/headers.h @@ -1,5 +1,5 @@ -#define LOOP_INTERVAL_MS 20 +#define LOOP_INTERVAL_MS 10 struct FVec2{ float x,y; diff --git a/robot/pid.cpp b/robot/pid.cpp index 166f577..b337233 100644 --- a/robot/pid.cpp +++ b/robot/pid.cpp @@ -6,9 +6,9 @@ //double turnKp=2, turnKi=0.0, turnKd=0.0; -double angKp=4.0, angKi=106.75, angKd=0.0472; -double posKp=0.96, posKi=1.28, posKd=1.0; -double turnKp=1, turnKi=5.0, turnKd=0.17; +double angKp=4, angKi=60.0, angKd=0.0958; +double posKp=0.5, posKi=0.0, posKd=0.5; +double turnKp=1.25, turnKi=3.0, turnKd=0.0; double angleInput, angleOutput, angleSetpoint; @@ -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_M, DIRECT); +PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON_E, DIRECT); PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID}; @@ -46,10 +46,9 @@ Speeds updatePID(){ angleSetpoint = posOutput; anglePID.Compute(); + turnSetpoint = 0; float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput)); turnPID.SetOutputLimits(-maxTurn, maxTurn); - turnSetpoint = 0; - turnInput = fmod(currentYaw-desiredYaw, 180); turnPID.Compute(); Speeds speeds; diff --git a/robot/robot.ino b/robot/robot.ino index c67cc85..77c2554 100644 --- a/robot/robot.ino +++ b/robot/robot.ino @@ -1,6 +1,5 @@ - - #include "headers.h" +#define PWM_FREQ 400 void wire1(){ Wire1.begin(SDA, SCL); @@ -12,12 +11,7 @@ void wire2(){ Wire2.setClock(400000); } - -#include -Servo left; -Servo right; - -float angleOffset = 4.620817395210266; +float angleOffset = 2.5033416018486023; float desiredYaw = 0.0; float currentYaw = 0.0; @@ -33,8 +27,10 @@ void initSerial(){ } void initMotors(){ - left.attach(D3); - right.attach(D4); + pinMode(D3, OUTPUT); + pinMode(D4, OUTPUT); + + analogWriteFreq(PWM_FREQ); } void initI2C(){ @@ -46,14 +42,16 @@ void initI2C(){ void setup() { initSerial(); - initWifi(false); - initServer(); initMotors(); initPID(); initI2C(); initDistance(); - initGyro(); initEncoder(); + + initWifi(true); + initServer(); + + initGyro(); } void loop() { @@ -65,25 +63,48 @@ void loop() { angle -= 180; angleInput = angle - angleOffset; } + long gyro = millis(); updateEncoder(); - currentYaw=odom.angle*180/M_PI; + + long encoder = millis(); updateDistance(); + + long distance = millis(); + updateServer(); + long server = millis(); + Speeds speeds = updatePID(); + + long pid = millis(); speeds.left = min(90.0f-10, max(-90.0f+10, speeds.left)); speeds.right = min(90.0f-10, max(-90.0f+10, speeds.right)); - left.write(90+(int)speeds.left); - right.write(90+(int)speeds.right); + + const int MAX_FOR = (int)(0.0010/(1.0/PWM_FREQ)*255); + const int MAX_REV = (int)(0.0020/(1.0/PWM_FREQ)*255); + + analogWrite(D3, map((int)speeds.left, 90-10, -90+10, MAX_REV, MAX_FOR)); + analogWrite(D4, map((int)speeds.right, 90-10, -90+10, MAX_REV, MAX_FOR)); long end = millis(); if(end-start>LOOP_INTERVAL_MS){ Serial.print("Overran "); - Serial.println(end-start); + Serial.print(gyro-start); + Serial.print(" "); + Serial.print(encoder-gyro); + Serial.print(" "); + Serial.print(distance-encoder); + Serial.print(" "); + Serial.print(server-distance); + Serial.print(" "); + Serial.print(pid-server); + Serial.print(" "); + Serial.println(end-pid); }else{ delay(LOOP_INTERVAL_MS-(end-start)); } diff --git a/robot/webserver.cpp b/robot/webserver.cpp index caa91ad..560c6fc 100644 --- a/robot/webserver.cpp +++ b/robot/webserver.cpp @@ -179,6 +179,7 @@ struct SetPIDPacket{ }; struct Packet{ + uint32_t sequence; uint32_t id; union{ ZeroPacket zero; @@ -282,16 +283,14 @@ bool handleUDP(){ constexpr size_t buffer_size = 256; alignas(alignof(float)) char buffer[buffer_size]; - if (size>=4){ + if (size>=8){ 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->sequence, sizeof(packet->sequence)); Udp.write((const char*)&packet->id, sizeof(packet->id)); - Udp.write((const char*)&sequence, sizeof(sequence)); - sequence++; switch(packet->id){ case ZeroPacket::ID: