worked more on doc

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,29 +89,56 @@ 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
+	\item Pan with WASD and zoom with scroll or +/-
+	\item Control by clicking on screen to set target position
+	\item use buttons/dropdowns to set/zero/get values from the robot
+\end{itemize}
 
 \section{Problem Set}
 
-\subsection{Keeping Upright}
-\subsection{Keeping A Position}
+\subsection{Balancing}
+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}
+Because we intend to map an area with the robot we need to know where we are. This issue is made slightly more challanging due to the fact we are in nearly constant motion keeping ourself upright.
+\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}
+The balancing is done by
+\subsection{Odometry}
+\subsection{Target Positions}
+The target position is acheived through the combination of two PID controllers. One is responsible for turn and simply adds motor speeds directly to the output of the balancing PID controller. One thing to note is the max turn speed is only 15\% of the max motor speed and decreases linearly as the angle goes to $\pm12^\circ$ to increase stability. The second PID controller is responsible for moving forward/backwards. It works by being the input to the balancing PID controller and setting the angle setpoint. it is bound to $\pm2^\circ$ and will bias the robot to move in one direction.\\
+These work together by calculating the heading and displacement needed from the robots current position to the target position and setting those values as the input to the aforementioned PID controllers.
+\subsection{Communication}
+Communication is done through a stateless but sequenced and tagged UDP 
 
 \section{Challanages}
 
 \subsection{Balancing}
-Obviously a robot which stays upright and only has two wheels 
-\subsection{Odometry}
-Keeping track of our position and angle was another challange that required careful consideration. Because the system is so dynamic and in constant motion we needed a system which could account for the constant movement to maintain a stable state.
-\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.
+Balancing took the longest out every task combined. It required research on not only PID control loops but also on different libraries and calibration requirements for our \texttt{MPU6050} gyroscope. These difficulties were compounded by the fact that we did not know which part/section would cause the robot to "randomly flail about and violently crash into the wall/floor". 
 
-\subsection{Efficient 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}
+\subsection{Odometry}
+We origionally didn't have encoders to keep track of our position and instead attempted to use the accelerometer to calculate displacement/heading to save money on parts. This however, did not pan out. The first problem we ran into was our heading drifting. We knew it was going to be an issue after reading docs as the module does not have a magnetometer builtin. That aside we tried a good old fassion "how bad could it possibly be" and oh boy did it drift fast. The second issue was "double integration over a noisy inaccurate signal" lead to our calculated position quickly exiting the stratosphere. These issues very quickly lead us to just get encoders.
+
+\subsection{Target Positions}
+Using the bias towards a direction by setting the target angle was not the first approach we used. We originally directly added a movement speed to the motors like we do for turning. This however caused the robot to become unstable in certain situations. We eventually abandoned the approach in favor of the one used currently.
+
+\subsection{Communication}
+Since we have limited processing power/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}\\
+
+We originally used a TCP+Webserver based library for this as it allowed us to use HTML+JS to display information but it continually had processing requirements the controller could not meet.
 
 \section{Resources Used}
 \begin{itemize}
@@ -121,6 +148,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.2}Keeping A Position}{2}{subsection.3.2}%
-\contentsline {subsection}{\numberline {3.3}Communication}{2}{subsection.3.3}%
-\contentsline {section}{\numberline {4}Approaches}{2}{section.4}%
-\contentsline {section}{\numberline {5}Challanages}{2}{section.5}%
-\contentsline {subsection}{\numberline {5.1}Balancing}{2}{subsection.5.1}%
-\contentsline {subsection}{\numberline {5.2}Odometry}{2}{subsection.5.2}%
-\contentsline {subsection}{\numberline {5.3}Target Positions}{3}{subsection.5.3}%
-\contentsline {subsection}{\numberline {5.4}Efficient Communication}{3}{subsection.5.4}%
-\contentsline {section}{\numberline {6}Resources Used}{3}{section.6}%
+\contentsline {subsection}{\numberline {3.1}Balancing}{2}{subsection.3.1}%
+\contentsline {subsection}{\numberline {3.2}Odometry}{3}{subsection.3.2}%
+\contentsline {subsection}{\numberline {3.3}Target Positions}{3}{subsection.3.3}%
+\contentsline {subsection}{\numberline {3.4}Communication}{3}{subsection.3.4}%
+\contentsline {section}{\numberline {4}Approaches}{3}{section.4}%
+\contentsline {subsection}{\numberline {4.1}Balancing}{3}{subsection.4.1}%
+\contentsline {subsection}{\numberline {4.2}Odometry}{3}{subsection.4.2}%
+\contentsline {subsection}{\numberline {4.3}Target Positions}{3}{subsection.4.3}%
+\contentsline {subsection}{\numberline {4.4}Communication}{4}{subsection.4.4}%
+\contentsline {section}{\numberline {5}Challanages}{4}{section.5}%
+\contentsline {subsection}{\numberline {5.1}Balancing}{4}{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}{5}{subsection.5.4}%
+\contentsline {section}{\numberline {6}Resources Used}{5}{section.6}%

doc/references.bib

@@ -32,4 +32,13 @@
 	author  = {{Brett Beauregard}},
 	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;
@@ -48,12 +50,11 @@ public class Gui extends JFrame  implements KeyListener, MouseWheelListener {
 
         new Thread(() -> {
             while(true){
-                try{
-                    visualPanel.updateData(robot.getDataPlus().await(1000));
-                    Thread.sleep(20);
-                }catch (Exception e){
-                    e.printStackTrace();
-                }
+             try{
+                 visualPanel.updateData(robot.getDataPlus().await(1000));
+             }catch (Exception e){
+                 e.printStackTrace();
+             }
             }
         }).start();
 
@@ -65,7 +66,7 @@ public class Gui extends JFrame  implements KeyListener, MouseWheelListener {
             int number = 500;
             for(int i = 0; i < number; i ++){
                 try {
-                    sum += robot.getEverything().await()[19-1];
+                    sum += robot.getEverything().await()[17-1];
                 } catch (Exception ex) {
                     throw new RuntimeException(ex);
                 }

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 kiSlider = new JSlider(0, (int)(SCALE*150), 0);
+        JSlider kpSlider = new JSlider(0, (int)(SCALE*1), 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

@@ -16,8 +16,7 @@ void initDistance(){
 void updateDistance(){
   if (lox.isRangeComplete()) {
     distanceReading = (float)lox.readRange();
-//    if(distanceReading>2000){
-//      distanceReading = (float)NAN;
-//    }
+  }else{
+    distanceReading = 8000;
   }
 }

robot/headers.h

@@ -32,12 +32,6 @@ extern float angleOffset;
 extern float desiredYaw;
 extern float currentYaw;
 
-struct DebugState{
-  int motorTargetAngle;
-};
-
-extern DebugState dbgState;
-
 //-------- wire
 #include <Wire.h>
 #define Wire1 Wire

robot/pid.cpp

@@ -1,14 +1,9 @@
 #include "headers.h"
 #include <PID_v1.h>
 
-//double angKp=3.5, angKi=80, angKd=0.042;
-//double posKp=20, posKi=0.0, posKd=0.0;
-//double turnKp=2, turnKi=0.0, 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 angKp=0.0444, angKi=0.6666, angKd=0.001;
+double posKp=0.8, posKi=0.2, posKd=1;
+double turnKp=0.014, turnKi=0.0333, 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);
@@ -17,8 +17,6 @@ float currentYaw = 0.0;
 
 FVec2 desiredPos;
 
-DebugState dbgState;
-
 void initSerial(){
   Serial.begin(115200);
   while (!Serial);
@@ -54,6 +52,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 +83,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(D4, map((int)speeds.right, 90-10, -90+10, MAX_REV, MAX_FOR));
+  analogWrite(D3, mapfloat(speeds.left, -1, 1, MAX_FOR, MAX_REV));
+  analogWrite(D4, mapfloat(speeds.right, -1, 1, MAX_FOR, MAX_REV));
 
   long end = millis();
   

robot/webserver.cpp

@@ -6,235 +6,71 @@
 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{
-  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 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{
+// ---------------- response packets
+struct DataPacketResponse{
     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(){
-    DataPacket dp;
+struct ZeroPacket{
+  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;
     dp.position.y = odom.y;
     
     Udp.write((const char*)&dp, sizeof(dp));
-}
+  }
+};
+struct SetTargetPacket{
+  static constexpr uint32_t ID = 2;
+  FVec2 pos;
+  
+  void handle(){
+    desiredPos.x = this->pos.x; 
+    desiredPos.y = this->pos.y; 
+        
+    Udp.write((const char*)&ID, sizeof(ID));
+  }
+};
+struct EverythingPacket{
+  static constexpr uint32_t ID = 3;
 
-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 handle(){
+    Udp.write((const char*)&ID, sizeof(ID));
 
-void respond_everything_packet(){
     float everything[] = {
-       (float)dbgState.motorTargetAngle, 
       (float)distanceReading,
-      0.0, //encoder.position(),
       angleSetpoint, angleInput, angleOutput,
       posSetpoint, posInput, posOutput,
       turnSetpoint, turnInput, turnOutput,
@@ -250,32 +86,82 @@ void respond_everything_packet(){
     };
     
     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);
-}
+};
+struct GetDataPlusPacket{
+  static constexpr uint32_t ID = 4;
 
-void get_pid(GetPIDPacket gpid){
-  if(gpid.index>PID_ARR_COUNT){
-    return;
+  void handle(){
+    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));
   }
-  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));
-}
+};
+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;
+    }
+    
+    Udp.write((const char*)&ID, sizeof(ID));
+    PID& pid = *pids[this->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));
+  }
+};
+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 +176,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: 
-        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;
+      case ZeroPacket::ID: packet->kind.zero.handle();break;  
+      case GetDataPacket::ID: packet->kind.get_data.handle();break;
+      case SetTargetPacket::ID: packet->kind.set_target.handle();break;
+      case EverythingPacket::ID: packet->kind.everything.handle();break;
+      case GetDataPlusPacket::ID: packet->kind.get_data_plus.handle();break;
+      case SetPIDPacket::ID: packet->kind.set_pid_packet.handle();break;
+      case GetPIDPacket::ID: packet->kind.get_pid_packet.handle();break;
     }
     Udp.endPacket();
     return true;