updated protocol, fixed turning being reversed

jdbg/src/Future.java

@@ -0,0 +1,59 @@
+import java.util.concurrent.TimeoutException;
+import java.util.function.Consumer;
+
+public abstract class Future<T, E extends Throwable> {
+    private T val;
+    private E err;
+    private Consumer<T> then = ignore -> {
+        this.accepted = false;
+        this.notifyAll();
+    };
+    private Consumer<E> 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<T, E> then(Consumer<T> then) {
+        this.then = then;
+        if (this.val != null && !accepted) {
+            accepted = true;
+            this.then.accept(this.val);
+        }
+        return this;
+    }
+
+    public synchronized Future<T, E> error(Consumer<E> 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);
+    }
+}

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<Pos> 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().error(ex -> ex.printStackTrace());
-                robot.sendZero();
+            maps.clear();
-                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);
 
+        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(robot.getDataPlus().await(1000));
-                    visualPanel.updateData(data);
                     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_PLUS, KeyEvent.VK_EQUALS -> visualPanel.zoom *= 1.1f;
-            case KeyEvent.VK_MINUS -> visualPanel.zoom /= 1.1;
+            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);
+                    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;
+            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,17 +146,28 @@ 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);
+            g.setColor(Color.BLACK);
-            int py = Math.round(dpp.y() * zoom);
+            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);
+            g.scale(zoom, -zoom); // Y-up
-            int pt_y = Math.round(dpp.t_y() * zoom);
+
+            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);
@@ -155,6 +177,16 @@ public class Gui extends JFrame  implements KeyListener, MouseWheelListener {
             g.setColor(Color.BLUE);
             g.fillOval(px - 5, py - 5, 10, 10);
 
+            g.setColor(Color.ORANGE);
+
+            ArrayList<Pos> 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);
         }
     }
 }

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);
+            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;
 
-                robot.setPID(index, kp, ki, kd, direction);
+            robot.setPID(index, kp, ki, kd, direction).error(ex -> {
-            } catch (Exception ex) {
                 ex.printStackTrace();
                 JOptionPane.showMessageDialog(this, "Failed to set PID: " + ex.getMessage());
-            }
+            });
         });
         getPidButton.doClick();
         indexDropdown.addActionListener(a -> {

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<Integer, PFuture<?>> seqMap = new HashMap<>();
+
+    public static abstract class PFuture<T> extends Future<T, Exception>{
+        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;
-    }
 
-    public synchronized <T> T packet(int id, Consumer<ByteBuffer> setup, Function<ByteBuffer, T> func) throws IOException {
+        new Thread(() -> {
-        setup.accept(buf.clear().limit(buf.capacity()).putInt(id));
+            var inBuf = ByteBuffer.allocate(4096).order(ByteOrder.LITTLE_ENDIAN);
-        DatagramPacket packet = new DatagramPacket(buf.array(), buf.position(), address, port);
+            while(true){
-        socket.send(packet);
+                try{
-
+                    inBuf.clear();
-        buf.clear();
+                    DatagramPacket receivePacket = new DatagramPacket(inBuf.array(), inBuf.capacity());
-        DatagramPacket receivePacket = new DatagramPacket(buf.array(), buf.capacity());
-        socket.setSoTimeout(1000);
                     socket.receive(receivePacket);
-        buf.limit(receivePacket.getLength());
+                    inBuf.limit(receivePacket.getLength());
 
-        if(buf.getInt() != id)throw new IOException("ID miss match");
+                    int squ = inBuf.getInt();
-        var seq = buf.getInt();
+                    var future = seqMap.remove(squ);
-        return func.apply(buf);
+                    if(future!=null) future.resolve(inBuf);
+                }catch (Exception ignore){}
+            }
+        }).start();
+    }
+
+    public synchronized <T> PFuture<T> packet(int id, Consumer<ByteBuffer> setup, Function<ByteBuffer, T> func) {
+        var f = new PFuture<T>(){
+            @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);
+        }
+
+        return f;
     }
 
 
-    public void sendZero() throws IOException{
+    public PFuture<Void> sendZero() {
-        packet(0, bb -> {}, bb -> null);
+        return packet(0, bb -> {}, bb -> null);
     }
 
     public record DataPacket(float yaw, float distance, float x, float y){}
 
-    public DataPacket getData() throws IOException {
+    public PFuture<DataPacket> 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{
+    public PFuture<Void> sendTargetPos(float x, float y) {
-        packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null);
+        return packet(2, bb -> bb.putFloat(x).putFloat(y), bb -> null);
     }
 
-    public float[] getEverything() throws IOException{
+    public PFuture<float[]> 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<DataPacketPlus> 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<PID> 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{
+    public PFuture<Void> setPID(int index, float kp, float ki, float kd, int direction) {
-        packet(6,
+        return packet(6,
         bb -> bb
                     .putInt(index)
                     .putFloat(kp)

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

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+360, 360);
-    desiredYaw = fmod(desiredYaw+180.0, 360.0);
+  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);

robot/headers.h

@@ -1,5 +1,5 @@
 
-#define LOOP_INTERVAL_MS 20
+#define LOOP_INTERVAL_MS 10
 
 struct FVec2{
   float x,y;

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 angKp=4, angKi=60.0, angKd=0.0958;
-double posKp=0.96, posKi=1.28, posKd=1.0;
+double posKp=0.5, posKi=0.0, posKd=0.5;
-double turnKp=1, turnKi=5.0, turnKd=0.17;
+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;

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);
 }
 
-
+float angleOffset = 2.5033416018486023;
-#include <Servo.h>
-Servo left;
-Servo right;
-
-float angleOffset = 4.620817395210266;
 float desiredYaw = 0.0;
 float currentYaw = 0.0;
 
@@ -33,8 +27,10 @@ void initSerial(){
 }
 
 void initMotors(){
-  left.attach(D3);
+  pinMode(D3, OUTPUT);
-  right.attach(D4);
+  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)); 
   }

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: