worked on report, fixed distance value being reported incorrectly, cleaned network/pid code up

doc/doc.tex

@@ -44,7 +44,7 @@
 
 \makeatletter
 \begin{titlepage}
-	\def \LOGOPATH {brock.jpg} % Path to Brock logo
+	\def \LOGOPATH {brock.jpg}
 	\def \UNIVERSITY {Brock University}
 	\def \FACULTY {Faculty of Mathematics \& Science}
 	\def \DEPARTMENT {Department of Computer Science}
@@ -89,17 +89,34 @@ What if you wanted to make a robot to map out a room but you only had two wheels
 With only two wheels its purely dynamically stable, will fall over due to its own stupidity but its very cute while it does so! This little guy can wiz around at the speed of a snail your house and (poorly) map out a room to your hearts desire
 
 \section{Instructions}
-
+\begin{itemize}
+	\item Hold robot in a vertical upright position clear of obsticals
+	\item Place battery in top compartment
+	\item Plug battery in and wait for robot to initialize
+	\item Once robot has booted let go and step away
+	\item Connect computer to network \texttt{MEOW}
+	\item launch vidualization and control software
+\end{itemize}
 
 \section{Problem Set}
 
-\subsection{Keeping Upright}
+\subsection{Balancing}
-\subsection{Keeping A Position}
+We have a vertical two wheeled robot I hope its fairly obvious when I say that it is only dynamically stable. This presents a challance because we need some system which keeps the system upright in a variaty of situations and responds dynamically to changes it cannot predict.
+\subsection{Odometry}
+Knowing that our system is very dynamic and having the forsight to know that we will need to keep in constant motion to keep upright the odometry system will need to handle that. 
+\subsection{Target Positions}
+The problem of target positions like odometry is made more challenging due to the fact we are almost constantly in motion. We simply cannot "not move" when we are in the location we want to be as we'd fall over. This means the target position will need to be a "best effort" battle to keep in the same place as best as we can.
 \subsection{Communication}
+The only source of computation used is a single \texttt{ESP8266} microcontroller, this in combination with the strict timing requirements on the duration of the control loop means the communication between the robot and the control/mapping software needs to be quick and efficient.
 
 \section{Approaches}
 
 
+\subsection{Balancing}
+\subsection{Odometry}
+\subsection{Target Positions}
+The target position 
+\subsection{Communication}
 
 \section{Challanages}
 
@@ -110,7 +127,7 @@ Keeping track of our position and angle was another challange that required care
 \subsection{Target Positions}
 Because the robot is in constant motion keeping itself balanced without a "push" towards a single position it will drift around. To solve this we use the odometry system as a input to the movement system. By setting the desired heading of the robot to the vector from its position to the target position, and by biasing the direction the robot will travel to be the direction to the target position we get a crude way of staying in a single position.
 
-\subsection{Efficient Communication}
+\subsection{Communication}
 Since we have limited processing power and time per loop iteration we need to be smart in how we receive and transmit data to our mapping software. For this reason we designed a stateless UDP based network protocol overtop the esp8266 Wifi \& UDP libraries. \cite{wifi_lib}
 
 \section{Resources Used}
@@ -121,6 +138,7 @@ Since we have limited processing power and time per loop iteration we need to be
 	\item The AS5600 library was used to interface with the AS5600 magnetic encoders on each wheel. \cite{enc_lib}
 	\item The MPU6050 library was used to interface and interpret the accelerometer and gyroscope data. \cite{gyro_lib}. It required modification to work with our hardware as it was a knockoff and the device ID was different than what it was expecting.
 	\item A blog by the author of the PID library used was very helpful when tuning and configuring the PID controls in the robot. \cite{pid_help}
+	\item A paper on odometry for robots with differential steering which we based our odometry system off of. \cite{odom_help}
 \end{itemize}
 
 

doc/doc.toc

@@ -1,13 +1,18 @@
 \contentsline {section}{\numberline {1}Introduction}{2}{section.1}%
 \contentsline {section}{\numberline {2}Instructions}{2}{section.2}%
 \contentsline {section}{\numberline {3}Problem Set}{2}{section.3}%
-\contentsline {subsection}{\numberline {3.1}Keeping Upright}{2}{subsection.3.1}%
+\contentsline {subsection}{\numberline {3.1}Balancing}{2}{subsection.3.1}%
-\contentsline {subsection}{\numberline {3.2}Keeping A Position}{2}{subsection.3.2}%
+\contentsline {subsection}{\numberline {3.2}Odometry}{2}{subsection.3.2}%
-\contentsline {subsection}{\numberline {3.3}Communication}{2}{subsection.3.3}%
+\contentsline {subsection}{\numberline {3.3}Target Positions}{3}{subsection.3.3}%
-\contentsline {section}{\numberline {4}Approaches}{2}{section.4}%
+\contentsline {subsection}{\numberline {3.4}Communication}{3}{subsection.3.4}%
-\contentsline {section}{\numberline {5}Challanages}{2}{section.5}%
+\contentsline {section}{\numberline {4}Approaches}{3}{section.4}%
-\contentsline {subsection}{\numberline {5.1}Balancing}{2}{subsection.5.1}%
+\contentsline {subsection}{\numberline {4.1}Balancing}{3}{subsection.4.1}%
-\contentsline {subsection}{\numberline {5.2}Odometry}{2}{subsection.5.2}%
+\contentsline {subsection}{\numberline {4.2}Odometry}{3}{subsection.4.2}%
-\contentsline {subsection}{\numberline {5.3}Target Positions}{3}{subsection.5.3}%
+\contentsline {subsection}{\numberline {4.3}Target Positions}{3}{subsection.4.3}%
-\contentsline {subsection}{\numberline {5.4}Efficient Communication}{3}{subsection.5.4}%
+\contentsline {subsection}{\numberline {4.4}Communication}{3}{subsection.4.4}%
-\contentsline {section}{\numberline {6}Resources Used}{3}{section.6}%
+\contentsline {section}{\numberline {5}Challanages}{3}{section.5}%
+\contentsline {subsection}{\numberline {5.1}Balancing}{3}{subsection.5.1}%
+\contentsline {subsection}{\numberline {5.2}Odometry}{4}{subsection.5.2}%
+\contentsline {subsection}{\numberline {5.3}Target Positions}{4}{subsection.5.3}%
+\contentsline {subsection}{\numberline {5.4}Communication}{4}{subsection.5.4}%
+\contentsline {section}{\numberline {6}Resources Used}{4}{section.6}%

doc/references.bib

@@ -33,3 +33,12 @@
 	date = "June 20, 2017",
 	note  = "\url{http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/}",
 }
+
+@misc{odom_help,
+	key   = "odom_help",
+	title = {{A Tutorial and Elementary Trajectory Model
+	for the Differential Steering System
+	of Robot Wheel Actuators}},
+	author  = {{G.W. Lucas}},
+	note  = "\url{https://rossum.sourceforge.net/papers/DiffSteer/}",
+}

jdbg/src/Gui.java

@@ -4,6 +4,8 @@ import java.awt.event.*;
 import java.net.SocketException;
 import java.net.UnknownHostException;
 import java.util.ArrayList;
+import java.util.Timer;
+import java.util.TimerTask;
 
 public class Gui extends JFrame  implements KeyListener, MouseWheelListener {
     private VisualPanel visualPanel;
@@ -50,7 +52,6 @@ public class Gui extends JFrame  implements KeyListener, MouseWheelListener {
             while(true){
              try{
                  visualPanel.updateData(robot.getDataPlus().await(1000));
-                    Thread.sleep(20);
              }catch (Exception e){
                  e.printStackTrace();
              }

jdbg/src/PidPanel.java

@@ -9,8 +9,8 @@ public class PidPanel extends JPanel {
         String[] pidNames = {"angle", "pos", "turn"};
         JComboBox<String> indexDropdown = new JComboBox<>(pidNames);
 
-        JSlider kpSlider = new JSlider(0, (int)(SCALE*10), 0);
+        JSlider kpSlider = new JSlider(0, (int)(SCALE*1), 0);
-        JSlider kiSlider = new JSlider(0, (int)(SCALE*150), 0);
+        JSlider kiSlider = new JSlider(0, (int)(SCALE*1), 0);
         JSlider kdSlider = new JSlider(0, (int)(SCALE*2), 0);
 
         JLabel kpValue = new JLabel("0.00");

robot/distance.cpp

@@ -19,5 +19,7 @@ void updateDistance(){
 //    if(distanceReading>2000){
 //      distanceReading = (float)NAN;
 //    }
+  }else{
+    distanceReading = 8000;
   }
 }

robot/pid.cpp

@@ -1,14 +1,9 @@
 #include "headers.h"
 #include <PID_v1.h>
 
-//double angKp=3.5, angKi=80, angKd=0.042;
+double angKp=0.0444, angKi=0.6666, angKd=0.001;
-//double posKp=20, posKi=0.0, posKd=0.0;
+double posKp=0.8, posKi=0.2, posKd=1;
-//double turnKp=2, turnKi=0.0, turnKd=0.0;
+double turnKp=0.014, turnKi=0.0333, turnKd=0.0;
-
-
-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;
@@ -22,20 +17,22 @@ PID turnPID(&turnInput, &turnOutput, &turnSetpoint, turnKp, turnKi, turnKd, P_ON
 
 PID* pids[PID_ARR_COUNT] = {&anglePID, &posPID, &turnPID};
 
+const float MAX_TURN_SPEED = 0.15;
+
 void initPID(){
   angleSetpoint = 0;
-  anglePID.SetOutputLimits(-180, 180);
+  anglePID.SetOutputLimits(-1, 1); // speed forward/backward
   anglePID.SetMode(AUTOMATIC);
   anglePID.SetSampleTime(5);
 
   posSetpoint = 0;
-  posPID.SetOutputLimits(-2, 2);
+  posPID.SetOutputLimits(-2, 2); // degrees forward/backward
   posPID.SetMode(AUTOMATIC);
   posPID.SetSampleTime(5);
   posPID.SetControllerDirection(DIRECT);
 
   turnSetpoint = 0;
-  turnPID.SetOutputLimits(-15, 15);
+  turnPID.SetOutputLimits(-MAX_TURN_SPEED, MAX_TURN_SPEED); // speed forward/backward
   turnPID.SetMode(AUTOMATIC);
   turnPID.SetSampleTime(5);
 }
@@ -47,7 +44,7 @@ Speeds updatePID(){
   anglePID.Compute();
 
   turnSetpoint = 0;
-  float maxTurn = max(0.0f, 25.0f-abs((float)angleOutput));
+  float maxTurn = max(0.0f, MAX_TURN_SPEED-abs((float)angleOutput)/90);
   turnPID.SetOutputLimits(-maxTurn, maxTurn);
   turnPID.Compute();
 

robot/robot.ino

@@ -1,5 +1,5 @@
 #include "headers.h"
-#define PWM_FREQ 400
+#define PWM_FREQ 450
 
 void wire1(){
   Wire1.begin(SDA, SCL);
@@ -54,6 +54,10 @@ void setup() {
   initGyro();  
 }
 
+float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 void loop() {
   long start = millis();
   
@@ -81,14 +85,11 @@ void loop() {
   
   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));
-
   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(D3, mapfloat(speeds.left, -1, 1, MAX_FOR, MAX_REV));
-  analogWrite(D4, map((int)speeds.right, 90-10, -90+10, MAX_REV, MAX_FOR));
+  analogWrite(D4, mapfloat(speeds.right, -1, 1, MAX_FOR, MAX_REV));
 
   long end = millis();
   

robot/webserver.cpp

@@ -6,209 +6,44 @@
 WiFiUDP Udp;
 unsigned int localUdpPort = 42069; 
 
-
-#ifdef DO_WEB_SERVER
-
-#include <ESPAsyncTCP.h>
-#include <ESPAsyncWebServer.h>
-#include <ArduinoJson.h>
-#include <AsyncJson.h>
-#include <AsyncMessagePack.h>
-
-#include "page_html.h"
-
-AsyncWebServer server(80);
-
-
-void initWebServer(){
-  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
-    request->send_P(200, "text/html", index_html);
-  });
-  server.on("/get_stuff_bin", HTTP_GET, [](AsyncWebServerRequest *request){
-    static float arr[4];
-    arr[0] = odom.angle;
-    arr[1] = distanceReading;
-    arr[2] = odom.x;
-    arr[3] = odom.y;
-    request->send(200, "application/octet-stream", (uint8_t*)(const char*)arr, sizeof(arr));
-  });
-  server.on("/fuckyou", HTTP_GET, [](AsyncWebServerRequest *request){
-    float arr[] = {
-      odom.angle,
-      distanceReading,
-      odom.x,
-      odom.y
-    };
-    request->send(200, "application/text", "hello");
-  });
-  server.on("/zero", HTTP_GET, [](AsyncWebServerRequest *request){
-    zeroOdom();
-    request->send(200);
-  });
-  server.on("/get_stuff", HTTP_GET, [](AsyncWebServerRequest *request){
-    char buff[1024];
-    int ret = snprintf(buff, sizeof(buff), 
-      R"({
-        "motorTargetAngle": %f,
-        "distanceReading": %f,
-        "position": %f,
-        "anglePID": {"setpoint": %lf, "input": %lf, "output": %lf},
-        "posPID": {"setpoint": %lf, "input": %lf, "output": %lf},
-        "turnPID": {"setpoint": %lf, "input": %lf, "output": %lf},
-        "odom": {"left": %f, "right": %f, "x": %f, "y": %f, "angle": %f},
-        "ypr": {"yaw": %f, "pitch": %f, "roll": %f},
-        "euler": {"psi": %f, "theta": %f, "phi": %f},
-        "gravity": {"x": %f, "y": %f, "z": %f},
-        "q": {"w": %f, "x": %f, "y": %f, "z": %f},
-        "aa": {"x": %hd, "y": %hd, "z": %hd},
-        "gy": {"x": %hd, "y": %hd, "z": %hd},
-        "aaReal": {"x": %hd, "y": %hd, "z": %hd},
-        "aaWorld": {"x": %hd, "y": %hd, "z": %hd}
-      })",
-      (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
-    );
-    request->send(200, "application/json", buff);
-  });
-  server.on("/get_pid", 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);
-    int idx = json["index"];
-    if(idx>3){
-      request->send(400);
-      return;
-    }
-    PID& pid = *pids[idx];
-    char buff[256];
-    int ret = snprintf(buff, sizeof(buff), 
-      R"({"kp": %lf, "ki": %lf, "kd": %lf, "direction": %d})",
-      pid.GetKp(), pid.GetKi(), pid.GetKd(), pid.GetDirection()
-    );
-    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) {
-    StaticJsonDocument<256> json;
-    deserializeJson(json, &bodyData[index], bodyLen);
-    int idx = json["index"];
-    if(idx>3){
-      request->send(400);
-      return;
-    }
-
-    PID& pid = *pids[idx];
-    if(json.containsKey("kp"))
-      pid.SetTunings(json["kp"], json["ki"], json["kd"]);
-    if(json.containsKey("direction"))
-      pid.SetControllerDirection(json["direction"]);
-      
-    request->send(200);
-  });
-
-   server.on("/set_desired_yaw", 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);
-      desiredYaw = json["yaw"];
-      Serial.print(desiredYaw);
-      request->send(200);
-   });
-  server.begin();
-}
-
-#endif 
-
 void initServer(){
-  #ifdef DO_WEB_SERVER
-  initWebServer()
-  #endif 
-  
   Udp.begin(localUdpPort);
 }
 
-struct ZeroPacket{
+// ---------------- response packets
-  static constexpr uint32_t ID = 0;
+struct DataPacketResponse{
-};
-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 sequence;
-  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{
+struct DataPlusPacketResponse{
     float yaw;
     float desiredYaw;
     float distance;
     FVec2 position;
     FVec2 targetPosition;
 };
+// ---------------- response packets
 
-void respond_data_packet(){
+struct ZeroPacket{
-    DataPacket dp;
+  static constexpr uint32_t ID = 0;
+
+  void handle(){
+    Udp.write((const char*)&ID, sizeof(ID));
+    zeroOdom(); 
+    desiredPos.x = 0; 
+    desiredPos.y = 0; 
+  }
+};
+struct GetDataPacket{
+  static constexpr uint32_t ID = 1;
+  
+  void handle(){
+    Udp.write((const char*)&ID, sizeof(ID));
+
+    DataPacketResponse dp;
     dp.yaw = currentYaw;
     dp.distance = distanceReading;
     dp.position.x = odom.x;
@@ -216,21 +51,24 @@ void respond_data_packet(){
     
     Udp.write((const char*)&dp, sizeof(dp));
   }
+};
+struct SetTargetPacket{
+  static constexpr uint32_t ID = 2;
+  FVec2 pos;
   
-void respond_data_packet_plus(){
+  void handle(){
-    DataPacketPlus dp;
+    desiredPos.x = this->pos.x; 
-    dp.yaw = currentYaw;
+    desiredPos.y = this->pos.y; 
-    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));
+    Udp.write((const char*)&ID, sizeof(ID));
   }
+};
+struct EverythingPacket{
+  static constexpr uint32_t ID = 3;
+
+  void handle(){
+    Udp.write((const char*)&ID, sizeof(ID));
 
-void respond_everything_packet(){
     float everything[] = {
       (float)dbgState.motorTargetAngle, 
       (float)distanceReading,
@@ -251,22 +89,38 @@ void respond_everything_packet(){
     
     Udp.write((const char*)everything, sizeof(everything));
   }
+};
+struct GetDataPlusPacket{
+  static constexpr uint32_t ID = 4;
 
-void set_pid(SetPIDPacket spid){
+  void handle(){
-  if(spid.index>PID_ARR_COUNT){
+    Udp.write((const char*)&ID, sizeof(ID));
+
+    DataPlusPacketResponse 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));
+  }
+};
+struct GetPIDPacket{
+  static constexpr uint32_t ID = 5;
+  uint32_t index;
+
+  void handle(){
+    if(this->index>PID_ARR_COUNT){
+      uint32_t e = -1;
+      Udp.write((const char*)&e, sizeof(e));
       return;
     }
     
-  PID& pid = *pids[spid.index];
+    Udp.write((const char*)&ID, sizeof(ID));
-  pid.SetTunings(spid.kp, spid.ki, spid.kd);
+    PID& pid = *pids[this->index];
-  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(),
@@ -276,6 +130,40 @@ void get_pid(GetPIDPacket gpid){
     
     Udp.write((const char*)&pidData, sizeof(pidData));
   }
+};
+struct SetPIDPacket{
+  static constexpr uint32_t ID = 6;
+  uint32_t index;
+  float kp,ki,kd;
+  uint32_t direction;
+
+  void handle(){
+    if(this->index>PID_ARR_COUNT){
+      uint32_t e = -1;
+      Udp.write((const char*)&e, sizeof(e));
+      return;
+    }
+    Udp.write((const char*)&ID, sizeof(ID));
+    
+    PID& pid = *pids[this->index];
+    pid.SetTunings(this->kp, this->ki, this->kd);
+    pid.SetControllerDirection(this->direction);
+  }
+};
+
+struct Packet{
+  uint32_t sequence;
+  uint32_t id;
+  union{
+    ZeroPacket zero;
+    GetDataPacket get_data;
+    SetTargetPacket set_target;
+    EverythingPacket everything;
+    GetDataPlusPacket get_data_plus;
+    GetPIDPacket get_pid_packet;
+    SetPIDPacket set_pid_packet;
+  } kind;
+};
 
 bool handleUDP(){
   int size = Udp.parsePacket();
@@ -290,33 +178,15 @@ bool handleUDP(){
 
     Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
     Udp.write((const char*)&packet->sequence, sizeof(packet->sequence));
-    Udp.write((const char*)&packet->id, sizeof(packet->id));
 
     switch(packet->id){
-      case ZeroPacket::ID: 
+      case ZeroPacket::ID: packet->kind.zero.handle();break;  
-        zeroOdom(); 
+      case GetDataPacket::ID: packet->kind.get_data.handle();break;
-        desiredPos.x = 0; 
+      case SetTargetPacket::ID: packet->kind.set_target.handle();break;
-        desiredPos.y = 0; 
+      case EverythingPacket::ID: packet->kind.everything.handle();break;
-        break;  
+      case GetDataPlusPacket::ID: packet->kind.get_data_plus.handle();break;
-      case GetDataPacket::ID: 
+      case SetPIDPacket::ID: packet->kind.set_pid_packet.handle();break;
-        respond_data_packet(); 
+      case GetPIDPacket::ID: packet->kind.get_pid_packet.handle();break;
-        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;