diff --git a/.DS_Store b/.DS_Store
deleted file mode 100644
index 1e2babc..0000000
Binary files a/.DS_Store and /dev/null differ
diff --git a/.gitignore b/.gitignore
index 29bfe19..1bb0e28 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,4 +5,5 @@
 .vscode/settings.json
 .vscode/ipch
 env.h
-/.history
\ No newline at end of file
+/.history
+.DS_Store
diff --git a/.vscode/extensions.json b/.vscode/extensions.json
deleted file mode 100644
index 080e70d..0000000
--- a/.vscode/extensions.json
+++ /dev/null
@@ -1,10 +0,0 @@
-{
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
-    "recommendations": [
-        "platformio.platformio-ide"
-    ],
-    "unwantedRecommendations": [
-        "ms-vscode.cpptools-extension-pack"
-    ]
-}
diff --git a/env.h.schema b/env.h.schema
index 62117bb..71c32fa 100644
--- a/env.h.schema
+++ b/env.h.schema
@@ -4,7 +4,7 @@
 #define WIFI_SSID "RoboNet-Legacy"
 #define WIFI_PASSWORD ""
 
-#define RUN_OFFLINE false
+#define ONLINE false
 
 #define STARTUP_DELAY 0
 
@@ -16,7 +16,7 @@
 #define PING_TIMEOUT 1000
 #define PING_MAX_MISSES 2
 
-// The amount of serialLogging to do
+// The amount of debugging to do
 // 0: None
 // 1: Error Logging (Logs only when an error occurs)
 // 2: General Logging (Logs when important things happen)
diff --git a/include/robot/battery.h b/include/robot/battery.h
deleted file mode 100644
index cd2e704..0000000
--- a/include/robot/battery.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef CHESSBOT_BATTERY_H
-#define CHESSBOT_BATTERY_H
-
-int getBatteryLevel();
-
-#endif
\ No newline at end of file
diff --git a/include/robot/control.h b/include/robot/control.h
deleted file mode 100644
index f10a906..0000000
--- a/include/robot/control.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#ifndef CHESSBOT_CONTROL_H
-#define CHESSBOT_CONTROL_H
-
-#include "Arduino.h"
-#include "utils/config.h"
-#include "robot/trapezoidalProfile.h"
-
-enum OperatingMode
-{
-    POSITION,
-    VELOCITY
-};
-
-struct ControlSetting
-{
-    OperatingMode mode;
-    float value;
-};
-
-static ControlSetting leftMotorControl;
-static ControlSetting rightMotorControl;
-
-static MotionProfile profileA = {MAX_VELOCITY_TPS, MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
-static MotionProfile profileB = {MAX_VELOCITY_TPS, MAX_ACCELERATION_TPSPS, 0, 0, 0, 0, 75.0}; // maxVelocity, maxAcceleration, currentPosition, currentVelocity, targetPosition, targetVelocity
-
-void setLeftMotorControl(ControlSetting control);
-void setRightMotorControl(ControlSetting control);
-ControlSetting getLeftMotorControl();
-ControlSetting getRightMotorControl();
-
-void setupBot();
-void drive(float tiles, std::string id);
-void drive(float leftPower, float rightPower, std::string id);
-void driveTicks(int tickDistance, std::string id);
-void turn(float angleRadians, std::string id);
-void stop();
-void sendPacketOnPidComplete(std::string id);
-void readLight(int loopDelayMs);
-void startMotorAndEncoderTest();
-bool checkMoveFinished();
-void startDriveTest();
-void startCentering();
-bool checkIfCanUpdateMovement();
-void resetSpeed();
-void createDriveUntilNewTile();
-void determineNextChoice();
-uint8_t driveUntilNewTile();
-void driveTestOff();
-boolean isRobotPidAtTarget();
-void updateCritRange();
-void controlLoop(int loopDelayMs, int8_t framesUntilPrint);
-void updateCentering();
-void updateToNextDistance();
-
-#endif
\ No newline at end of file
diff --git a/include/robot/driveTest.h b/include/robot/driveTest.h
deleted file mode 100644
index a9538c0..0000000
--- a/include/robot/driveTest.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef CHESSBOT_DRIVETEST_H
-#define CHESSBOT_DRIVETEST_H
-
-class MotorEncoderTest
-{
-public:
-    inline void startMotorAndEncoderTest() { init(); }
-
-private:
-    int startEncoderA;
-    int _startEncoderB;
-    int startEncoderB;
-    int prevEncA;
-    int prevEncB;
-    float maxEncoderVelocity = 0;
-    float prevEncVelA;
-    float prevEncVelB;
-    float maxEncoderAccel = 0;
-    unsigned long encoderCheckTimerId;
-    bool testSuccessful;
-    const int ENCODER_TOLERANCE = 100;
-
-    void init();
-    void testLeftMotor();
-    void testWait();
-    void testRightMotor();
-    void testDriveForward();
-    void checkMotorDeadzone(bool leftMotor);
-    void testMotorDeadzones();
-    void checkEncoderVelocity();
-    void testDriveDone();
-};
-
-#endif
\ No newline at end of file
diff --git a/include/robot/encoder.h b/include/robot/encoder.h
deleted file mode 100644
index 743e3ce..0000000
--- a/include/robot/encoder.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#ifndef CHESSBOT_ENCODER_H
-#define CHESSBOT_ENCODER_H
-
-void setupEncodersNew();
-void resetEncoders();
-void encoderLoop();
-
-int readLeftEncoder();
-int readRightEncoder();
-
-#endif
\ No newline at end of file
diff --git a/include/robot/lightSensor.h b/include/robot/lightSensor.h
deleted file mode 100644
index c2dc89e..0000000
--- a/include/robot/lightSensor.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef CHESSBOT_LIGHT_SENSOR_H
-#define CHESSBOT_LIGHT_SENSOR_H
-
-// Built-In Libraries
-#include "Arduino.h"
-
-void setupIR();
-void readGarbageVals();
-void activateIR();
-void deactivateIR();
-void startLightReading(bool* onFirstTile, bool* waitingForLight);
-void readLightLevels();
-
-#endif
\ No newline at end of file
diff --git a/include/robot/lights.h b/include/robot/lights.h
new file mode 100644
index 0000000..67c3c48
--- /dev/null
+++ b/include/robot/lights.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <Arduino.h>
+
+class Light {
+    public:
+        Light(gpio_num_t pin);
+
+        void tick();
+        
+        short raw_value();
+        bool value();
+        bool held_value();
+        void reset();
+
+        bool changed_this_tick();
+        unsigned long last_changed_time();
+
+    private:
+        gpio_num_t pin;
+
+        short _raw_value;
+
+        bool _value;
+        bool _held_value;
+        
+        bool _changed_this_tick;
+        unsigned long _last_changed_time;
+};
+
+void setupIR();
+void activateIR();
+void deactivateIR();
\ No newline at end of file
diff --git a/include/robot/motion-controller.h b/include/robot/motion-controller.h
new file mode 100644
index 0000000..47a2c85
--- /dev/null
+++ b/include/robot/motion-controller.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <optional>
+#include <string>
+
+#include "robot/pid.h"
+#include "utils/geometry.h"
+
+enum CenteringStatus {
+    NOT_CENTERING,
+    
+    // The robot has started centering
+    STARTED,
+
+    // Has hit the first edge, and is now aligning to it
+    ALIGNING_EDGE_1,
+
+    ALIGNED_EDGE_1,
+
+    CENTERED_Y_AXIS,
+
+    ALIGNED_EDGE_2,
+};
+
+class MotionController {
+    public:
+        enum MotionPhase {
+            // Traveling to the destination
+            TRAVELLING,
+
+            // Aligning to the correct rotation
+            ALIGNING,
+            ARRIVED
+        };
+
+        MotionController();
+
+        MotionPhase phase();
+        void set_goal(Coordinate2D goal_destination, double goal_angle, std::optional<std::string> id);
+        void tick(uint32_t delta);
+        
+        void print_status();
+        void reset();
+    private:
+        std::optional<std::string> actionID;
+
+        PIDController DistVelocityController; 
+        PIDController AVelocityController;
+
+        MotionPhase _phase;
+        MotionPhase _prev_phase;
+        
+        double goal_angle;
+        Coordinate2D goal_position;
+};
\ No newline at end of file
diff --git a/include/robot/motor.h b/include/robot/motor.h
index 6a5d914..3829947 100644
--- a/include/robot/motor.h
+++ b/include/robot/motor.h
@@ -1,8 +1,55 @@
-#ifndef CHESSBOT_MOTOR_H
-#define CHESSBOT_MOTOR_H
+#pragma once
 
-void setupMotors();
-void setLeftPower(float power);
-void setRightPower(float power);
+#include <Encoder.h>
+#include <stdint.h>
 
-#endif
\ No newline at end of file
+#include "robot/pid.h"
+#include "utils/functions.h"
+#include "utils/config.h"
+
+
+const double TIRE_RADIUS = 5.9;
+const double TIRE_CIRCUMFERENCE = M_PI * 2 * TIRE_RADIUS;
+
+class Motor {
+    public:
+        Motor(bool inverted, int motor_pin_a, int motor_pin_b, uint8_t enc_pin_a, uint8_t enc_pin_b);
+
+        void tick(uint32_t delay);
+
+        /* Read-only stats */ 
+
+        int duty();
+        double power();
+        double speed();
+        double target_speed();
+        double dist();      // Total distance in cm
+        double tick_dist(); // Distance this tick in cm
+        int32_t raw_dist(); // Read the raw encoder value in ticks
+        double get_saved();
+
+        /* Affect the motor */
+
+        double save_dist();
+        void encoder_reset();
+        void set_power(double power);
+        void set_target_speed(double speed);
+
+    private:
+        int pin_a;
+        int pin_b;
+
+        bool inverted;
+
+        PIDController speed_controller;
+        double _power;
+        RingBuf _speeds;
+        double _target_speed;
+
+        int32_t raw_enc_value;
+        int32_t prev_raw_enc_value;
+        double saved_dist;
+        Encoder encoder;
+};
+
+int power_to_duty(double power);
\ No newline at end of file
diff --git a/include/robot/pid.h b/include/robot/pid.h
new file mode 100644
index 0000000..46209d1
--- /dev/null
+++ b/include/robot/pid.h
@@ -0,0 +1,46 @@
+#pragma once
+
+class PIDController
+{
+public:
+    // PIDController(double kp, double ki, double kd, double min, double max);
+    PIDController(double kp, double ki, double kd, double min, double max, double errorTolerance);
+
+    double Compute(double setpoint, double actual_value, double dt);
+    void Reset();
+
+    double kp, ki, kd;           // PID gains
+    double minOutput, maxOutput; // Output limits
+
+    double prev_error, prev_velocity_error; // Previous error
+    double errorTolerance; // Allowed error before returning 0
+    double integral;   // Integral accumulator
+
+protected:
+    virtual double getError(double setpoint, double actual_value) { return setpoint - actual_value; }
+};
+// Manually making the clamp function
+// template <typename T>
+// T clamp(T value, T minValue, T maxValue) {
+//     return (value < minValue) ? minValue : (value > maxValue) ? maxValue : value;
+// }
+
+class ContinuousPIDController : public PIDController
+{
+public:
+    ContinuousPIDController(double kp, double ki, double kd, double min, double max, double errorTolerance, double minInput, double maxInput)
+        : PIDController(kp, ki, kd, min, max, errorTolerance), minInput(minInput), maxInput(maxInput) {}
+protected:
+    double getError(double setpoint, double actual_value) override {
+        double error = setpoint - actual_value;
+        double range = maxInput - minInput;
+        if (error > range / 2) {
+            error -= range;
+        } else if (error < -range / 2) {
+            error += range;
+        }
+        return error;
+    }
+private:
+    double minInput, maxInput; // Input range for continuous wrapping
+};
\ No newline at end of file
diff --git a/include/robot/pidController.h b/include/robot/pidController.h
deleted file mode 100644
index 299a0da..0000000
--- a/include/robot/pidController.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef PID_CONTROLLER_H
-#define PID_CONTROLLER_H
-
-class PIDController
-{
-public:
-    // PIDController(double kp, double ki, double kd, double min, double max);
-    PIDController(double kp, double ki, double kd, double min, double max) : kp(kp), ki(ki), kd(kd), minOutput(min), maxOutput(max), prev_error(0), integral(0) {}
-
-    double Compute(double setpoint, double actual_value, double dt);
-    void Reset(); // Extra space between void and reset because I can't see uneven indent and spacing after doing python for a long time
-
-    double kp, ki, kd;           // PID gains
-    double minOutput, maxOutput; // Output limits
-
-    double prev_error, prev_velocity_error; // Previous error
-    double integral;   // Integral accumulator
-};
-// Manually making the clamp function
-// template <typename T>
-// T clamp(T value, T minValue, T maxValue) {
-//     return (value < minValue) ? minValue : (value > maxValue) ? maxValue : value;
-// }
-
-#endif // PID_CONTROLLER_H
diff --git a/include/robot/pwm.h b/include/robot/pwm.h
deleted file mode 100644
index 7dea316..0000000
--- a/include/robot/pwm.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef CHESSBOT_PWM_H
-#define CHESSBOT_PWM_H
-
-void setupPWM(int pin);
-void writePWM(int channel, int dutyCycle);
-int mapPowerToDuty(float power);
-
-#endif
\ No newline at end of file
diff --git a/include/robot/robot.h b/include/robot/robot.h
new file mode 100644
index 0000000..32f5a2a
--- /dev/null
+++ b/include/robot/robot.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#include <Arduino.h>
+#include <optional>
+
+#include "robot/lights.h"
+#include "robot/motion-controller.h"
+#include "robot/motor.h"
+#include "robot/pid.h"
+#include "utils/config.h"
+#include "utils/geometry.h"
+
+enum DriveType
+{
+    STOPPED,
+    MANUAL,
+    MOTION_CONTROL,
+};
+
+class Robot {
+    public:
+        Robot();
+
+        static int batteryLevel();
+        MotionController::MotionPhase motion_status();
+
+        // Runs all the necessary processing for each tick of the global event loop
+        void tick(uint32_t frame, uint32_t delay);
+        
+        void center(std::optional<std::string> id);
+        void drive(double tiles, std::string id);
+        void drive(Coordinate2D goal_pos, double goal_angle);
+        void drive(std::tuple<double, double>& powers);
+
+        void turn(double angleRadians, std::string id);
+        
+        void start();
+        void stop();
+
+        friend class MotionController;
+
+    private:
+        // Components
+        Motor left;
+        Motor right;
+
+        Light front_left_light;
+        Light front_right_light;
+        Light back_left_light;
+        Light back_right_light;
+
+        // State
+        double rotation;
+        Coordinate2D position;
+        MotionController motion_controller;
+        
+        DriveType drive_mode;
+        CenteringStatus centeringStatus;
+        std::optional<std::string> centeringID;
+
+        void center_tick(uint32_t delay);
+        void pid_tick(uint32_t delay);
+
+        void print_status(uint32_t delay);
+};
+
+extern Robot robot;
\ No newline at end of file
diff --git a/include/robot/splines.h b/include/robot/splines.h
deleted file mode 100644
index 67d5960..0000000
--- a/include/robot/splines.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef CHESSBOT_SPLINES_H
-#define CHESSBOT_SPLINES_H
-
-#include <string>
-#include <queue>
-#include <tuple>
-#include "wifi/packet.h"
-#include "trapezoidalProfile.h"
-
-struct Point {
-    float x;
-    float y;
-};
-
-static std::queue<std::tuple<float, float>> timeSlicesToExecute;
-
-void velocityUpdateTimerFunction(std::string id);
-void danceMonkeyQuadratic(std::string id, Point start, Point control, Point end, float totalTime);
-void danceMonkeyCubic(std::string id, Point start, Point control1, Point control2, Point end, float totalTime);
-void startCustomMotionProfileTimer(int leftPositionTarget, int rightPositionTarget, double profileDuration, std::string id);
-void customMotionProfileTimerFunction(MotionProfile &customProfileA, MotionProfile &customProfileB, double dt, std::string id);
-
-#endif
\ No newline at end of file
diff --git a/include/robot/trapezoidalProfile.h b/include/robot/trapezoidalProfile.h
deleted file mode 100644
index 24b1fde..0000000
--- a/include/robot/trapezoidalProfile.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef CHESSBOT_TRAPEZOIDAL_PROFILE_H
-#define CHESSBOT_TRAPEZOIDAL_PROFILE_H
-
-#include <cstdint>
-
-struct MotionProfile {
-    double maxVelocity;
-    double maxAcceleration;
-    double currentPosition;
-    double currentVelocity;
-    double targetPosition;
-    double targetVelocity;
-    double criticalRange;
-};
-
-double updateTrapezoidalProfile(MotionProfile &profile, double dt, int8_t framesUntilPrint);
-
-template <typename T>
-T clamp(T value, T minValue, T maxValue) {
-    return (value < minValue) ? minValue : (value > maxValue) ? maxValue : value;
-}
-
-#endif
\ No newline at end of file
diff --git a/include/tests.h b/include/tests.h
new file mode 100644
index 0000000..361816a
--- /dev/null
+++ b/include/tests.h
@@ -0,0 +1,16 @@
+#include "robot/robot.h"
+
+void center_test(Robot& r);
+
+// Turns off the motors, so we can easily read data
+void sleepy_test(Robot& r);
+
+void line_test(Robot& r);
+
+// Moves to 0 -> 2PI -> 0 -> 4PI -> 0 on a clock to test angular PID
+void circle_test(Robot& r);
+
+// Moves to the points on a 1x1 meter square
+void square_test(Robot& r);
+
+void hardware_test(Robot& r);
\ No newline at end of file
diff --git a/include/utils/config.h b/include/utils/config.h
index b98a5cb..1b4b40a 100644
--- a/include/utils/config.h
+++ b/include/utils/config.h
@@ -1,13 +1,10 @@
-#ifndef CHESSBOT_CONFIG_H
-#define CHESSBOT_CONFIG_H
+#pragma once
 
-// Built-In Libraries
-#include "Arduino.h"
-
-// external Libraries
+#include <Arduino.h>
 #include <ArduinoJson.h>
 
 extern int loopDelayMilliseconds;
+extern uint32_t frame;
 
 // These variables are declared here, and defined in config.cpp
 // config.cpp is the only file that should be modifying these values. Everything else is read-only
@@ -28,18 +25,26 @@ extern gpio_num_t PHOTODIODE_C_PIN;
 extern gpio_num_t PHOTODIODE_D_PIN;
 
 extern gpio_num_t BATTERY_VOLTAGE_PIN;
+extern int BATTERY_VOLTAGE_OFFSET;
+
 extern gpio_num_t ONBOARD_LED_PIN;
 
 extern int TICKS_PER_ROTATION;
 extern float TRACK_WIDTH_INCHES;
+extern float TRACK_WIDTH_CM;
 extern float WHEEL_DIAMETER_INCHES;
-extern float MAX_VELOCITY_TPS;
-extern float MAX_ACCELERATION_TPSPS;
+extern float WHEEL_RADIUS_CM;
+extern float THEORETICAL_MAX_VELOCITY_TPS;
+extern float VELOCITY_LIMIT_TPS;
+extern float THEORETICAL_MAX_ACCELERATION_TPSPS;
+extern float ACCELERATION_LIMIT_TPSPS;
+extern float MIN_MOTOR_POWER;
+extern float MIN_MOTOR_VELOCITY_TPS;
 extern float TILES_TO_TICKS;
 
+extern int MAGNET_CCW_IS_POSITIVE; // Set to 1 if counterclockwise rotation is positive, -1 if clockwise rotation is positive
+
 extern float PID_POSITION_TOLERANCE;
 extern float PID_VELOCITY_TOLERANCE;
 
-void setConfig(JsonObject config);
-
-#endif
\ No newline at end of file
+void setConfig(JsonObject config);
\ No newline at end of file
diff --git a/include/utils/functions.h b/include/utils/functions.h
index af12db3..45e5acb 100644
--- a/include/utils/functions.h
+++ b/include/utils/functions.h
@@ -1,13 +1,22 @@
-#ifndef CHESSBOT_FUNCTIONS_H
-#define CHESSBOT_FUNCTIONS_H
+#pragma once
 
-// Built-In Libraries
-#include "Arduino.h"
+#include <Arduino.h>
 
-float fmap(float x, float in_min, float in_max, float out_min, float out_max);
+#include <array>
+
+double fmap(double x, double in_min, double in_max, double out_min, double out_max);
 bool approxEquals(int x, int y, int epsilon);
 bool approxEquals(double x, double y, double epsilon);
 int radiansToTicks(double angle);
 std::string unint8ArrayToHexString(uint8_t* oldArray, int len);
 
-#endif
\ No newline at end of file
+class RingBuf {
+private:
+    std::array<double, 20> buf;
+    std::size_t head;
+
+public:
+    RingBuf();
+    double insert(double val);
+    double average();
+};
\ No newline at end of file
diff --git a/include/utils/geometry.h b/include/utils/geometry.h
new file mode 100644
index 0000000..1217a29
--- /dev/null
+++ b/include/utils/geometry.h
@@ -0,0 +1,31 @@
+#pragma once
+
+class Coordinate2D {
+    public:
+        double x;
+        double y;
+    
+        Coordinate2D();
+
+        // Unit vector from angle
+        Coordinate2D(double angle);
+        Coordinate2D(double x, double y);
+
+        // Transform (same as addition)
+        Coordinate2D transform(Coordinate2D offset) const;
+
+        // Transform with polar coordinate
+        Coordinate2D transform(double theta, double distance) const;
+
+        // Rotate around origin
+        Coordinate2D rotate(double theta) const;
+        // Scale (same as multiplication)
+        Coordinate2D scale(double theta) const;
+
+        double distance_to(const Coordinate2D destination) const;
+        double angle_to(const Coordinate2D destination) const;
+        Coordinate2D vector_to(const Coordinate2D destination) const;
+
+        double dot_product(const Coordinate2D other) const;
+        bool is_behind(double radians, Coordinate2D other) const;
+};
\ No newline at end of file
diff --git a/include/utils/logging.h b/include/utils/logging.h
index ab1a3ed..2e68e1d 100644
--- a/include/utils/logging.h
+++ b/include/utils/logging.h
@@ -1,19 +1,17 @@
-#ifndef CHESSBOT_LOGGING_H
-#define CHESSBOT_LOGGING_H
+#pragma once
 
-// Built-In Libraries
 #include <string>
 
-void serialLog(const char *message, int serialLoggingLevel);
-void serialLog(int value, int serialLoggingLevel);
-void serialLog(double value, int serialLoggingLevel);
-void serialLog(std::string value, int serialLoggingLevel);
+enum DebugLevel {
+    NONE,
+    INFO,
+    DEBUG,
+    TRACE,
+    RIDICULOUS, // Use if insane
+};
 
-void serialLogln(const char *message, int serialLoggingLevel);
-void serialLogln(int value, int serialLoggingLevel);
-void serialLogln(double value, int serialLoggingLevel);
-void serialLogln(std::string value, int serialLoggingLevel);
+#define SERIAL_CLEAR "\033[3J\033[H\033[2J"
+#define SERIAL_WHITE "\e[0m"
+#define SERIAL_RED "\e[31m"
 
-void serialLogError(char message[], int error);
-
-#endif
\ No newline at end of file
+void serial_printf(enum DebugLevel level, const char* fmt, ...);
\ No newline at end of file
diff --git a/include/utils/status.h b/include/utils/status.h
deleted file mode 100644
index 8b74e45..0000000
--- a/include/utils/status.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef CHESSBOT_STATUS_H
-#define CHESSBOT_STATUS_H
-
-bool getServerConnectionStatus();
-void setServerConnectionStatus(bool value);
-
-bool getWiFiConnectionStatus();
-void setWiFiConnectionStatus(bool value);
-
-bool getStoppedStatus();
-void setStoppedStatus(bool value);
-
-#endif
\ No newline at end of file
diff --git a/include/utils/timer.h b/include/utils/timer.h
deleted file mode 100644
index 93071b2..0000000
--- a/include/utils/timer.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef CHESSBOT_TIMER_H
-#define CHESSBOT_TIMER_H
-
-// Built-In Libraries
-#include <vector>
-//we're gonna need this if we need to use delays which require functions
-#include <functional>
-
-// This callback allows you to just pass in the pointer of a function as a parameter
-using TimerCallback = std::function<void()>;
-struct Timer {
-    // Whether the timer is cancelled after it finishes, or set to run again
-    bool oneOff;
-    // The amount of time until the timer expires
-    int delay;
-    // The time that the timer started (or refreshed in the case of non one-off timers)
-    unsigned long lastMillis;
-    // The ID of the timer (Timestamp timer was created. Same as lastMillis for oneOffs)
-    unsigned long id;
-    // The function to be run after the timer expires
-    TimerCallback func;
-};
-
-unsigned long timerDelay(int delay, TimerCallback func);
-unsigned long timerInterval(int interval, TimerCallback func);
-
-Timer getTimer(unsigned long id);
-
-void timerReset(unsigned long id);
-void timerCancel(unsigned long id);
-void timerCancelAll();
-
-void timerStep();
-
-#endif
\ No newline at end of file
diff --git a/include/wifi/connection.h b/include/wifi/connection.h
index d151f05..83907a1 100644
--- a/include/wifi/connection.h
+++ b/include/wifi/connection.h
@@ -1,19 +1,17 @@
-#ifndef CHESSBOT_CONNECTION_H
-#define CHESSBOT_CONNECTION_H
+#pragma once
 
-#include "Arduino.h"
+#include <Arduino.h>
 #include <ArduinoJson.h>
+#include <WiFi.h>
 
-void connectServer();
-void disconnectServer();
-void reconnectServer();
-bool checkServerConnection();
-void initiateHandshake();
+#include <optional>
 
-void acceptData();
-void sendPacket(JsonDocument& packet);
-void sendActionSuccess(std::string messageId);
-void sendActionFail(std::string messageId);
-void sendPingResponse();
+extern WiFiClient client;
 
-#endif
\ No newline at end of file
+void connection_check_reconnect();
+std::optional<JsonDocument> recv_packet();
+
+void send_packet(JsonDocument packet);
+void send_handshake();
+void send_success(std::string id);
+void send_ping();
\ No newline at end of file
diff --git a/include/wifi/packet.h b/include/wifi/packet.h
index 7389a51..3a5eb64 100644
--- a/include/wifi/packet.h
+++ b/include/wifi/packet.h
@@ -1,15 +1,42 @@
-#ifndef CHESSBOT_PACKET_H
-#define CHESSBOT_PACKET_H
+#pragma once
 
-#include "Arduino.h"
+#include <Arduino.h>
 #include <ArduinoJson.h>
 
+#include "robot/robot.h"
 
-void handlePacket(JsonDocument packet);
-std::string unint8ArrayToHexString(uint8_t* oldArray, int len);
+enum PacketType : uint8_t {
+    CLIENT_HELLO,
+    SERVER_HELLO,
+    PING_SEND,
+    PING_RESPONSE,
+    QUERY_VAR,
+    QUERY_RESPONSE,
+    SET_VAR,
+    TURN_BY_ANGLE,
+    DRIVE_TILES,
+    DRIVE_TICKS,
+    ACTION_SUCCESS,
+    ACTION_FAIL,
+    DRIVE_TANK,
+    ESTOP,
+    CUBIC,
+    QUADRATIC,
+    SPIN_RADIANS,
+    BS_MOVE,
+    CENTER_SEND,
+
+    ERROR // Not a valid packet type
+};
 
-void constructHelloPacket(JsonDocument& packet);
-void constructSuccessPacket(JsonDocument& packet, std::string messageId);
-void constructFailPacket(JsonDocument& packet, std::string messageId);
-void constructPingPacket(JsonDocument& packet);
-#endif
\ No newline at end of file
+// Checks that the json document has the correct field and type
+// If it doesn't contain the correct field, returns false
+#define ASSERT_FIELD(packet, key, _type) \
+  if (!packet[key].is<_type>()) { \
+    serial_printf(DebugLevel::NONE, "Received a packet that did not contain field %s\n", key); \
+    return false; \
+  }
+
+PacketType parse_packet_type(const char * type);
+bool handle_packet(Robot& r, JsonDocument packet);
+std::string unint8ArrayToHexString(uint8_t* oldArray, int len);
diff --git a/include/wifi/wireless.h b/include/wifi/wireless.h
deleted file mode 100644
index 3f87e5b..0000000
--- a/include/wifi/wireless.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef CHESSBOT_WIRELESS_H
-#define CHESSBOT_WIRELESS_H
-
-const char* getWifiStatus(int status);
-bool checkWiFiConnection();
-void confirmWiFi();
-void connectWiFI();
-void reconnectWiFI();
-void disconnectWiFI();
-void createWiFi();
-
-#endif
\ No newline at end of file
diff --git a/pid_test.csv b/pid_test.csv
index 11adc7a..84b9113 100644
--- a/pid_test.csv
+++ b/pid_test.csv
@@ -1,755 +1,727 @@
-EncoderLeft,EncoderRight,DesiredVelocityLeft,DesiredVelocityRight,CurrentVelocityLeft,CurrentVelocityRight,LeftPower,RightPower,EncoderTargetLeft,EncoderTargetRight
-0,0,100.00,100.00,0.00,0.00,0.21,0.19,120000,120000
-6,5,400.00,350.00,300.00,250.00,0.21,0.19,120000,120000
-18,14,700.00,550.00,600.00,450.00,0.21,0.19,120000,120000
-26,16,500.00,200.00,400.00,100.00,0.21,0.19,120000,120000
-39,16,750.00,100.00,650.00,0.00,0.21,0.19,120000,120000
-86,16,2450.00,100.00,2350.00,0.00,0.21,0.19,120000,120000
-134,16,2500.00,100.00,2400.00,0.00,0.21,0.19,120000,120000
-191,17,2950.00,150.00,2850.00,50.00,0.21,0.19,120000,120000
-264,17,3750.00,100.00,3650.00,0.00,0.21,0.19,120000,120000
-343,17,4000.00,100.00,3950.00,0.00,0.10,0.19,120000,120000
-420,17,3950.00,100.00,3850.00,0.00,0.21,0.19,120000,120000
-487,17,3450.00,100.00,3350.00,0.00,0.21,0.19,120000,120000
-570,17,4000.00,100.00,4150.00,0.00,-0.31,0.19,120000,120000
-624,18,2800.00,150.00,2700.00,50.00,0.21,0.19,120000,120000
-616,18,-300.00,100.00,-400.00,0.00,0.21,0.19,120000,120000
-618,19,200.00,150.00,100.00,50.00,0.21,0.19,120000,120000
-619,19,150.00,100.00,50.00,0.00,0.21,0.19,120000,120000
-621,19,200.00,100.00,100.00,0.00,0.21,0.19,120000,120000
-625,19,300.00,100.00,200.00,0.00,0.21,0.19,120000,120000
-635,19,600.00,100.00,500.00,0.00,0.21,0.19,120000,120000
-675,19,2100.00,100.00,2000.00,0.00,0.21,0.19,120000,120000
-726,19,2650.00,100.00,2550.00,0.00,0.21,0.19,120000,120000
-787,20,3150.00,150.00,3050.00,50.00,0.21,0.19,120000,120000
-857,21,3600.00,150.00,3500.00,50.00,0.21,0.19,120000,120000
-933,21,3900.00,100.00,3800.00,0.00,0.21,0.19,120000,120000
-1018,21,4000.00,100.00,4250.00,0.00,-0.52,0.19,120000,120000
-1066,23,2500.00,200.00,2400.00,100.00,0.21,0.19,120000,120000
-1049,25,-750.00,200.00,-850.00,100.00,0.21,0.19,120000,120000
-1074,32,1350.00,450.00,1250.00,350.00,0.21,0.19,120000,120000
-1098,55,1300.00,1250.00,1200.00,1150.00,0.21,0.19,120000,120000
-1141,96,2250.00,2150.00,2150.00,2050.00,0.21,0.19,120000,120000
-1196,145,2850.00,2550.00,2750.00,2450.00,0.21,0.19,120000,120000
-1260,197,3300.00,2700.00,3200.00,2600.00,0.21,0.19,120000,120000
-1338,267,4000.00,3600.00,3900.00,3500.00,0.21,0.19,120000,120000
-1419,337,4000.00,3600.00,4050.00,3500.00,-0.10,0.19,120000,120000
-1502,415,4000.00,4000.00,4150.00,3900.00,-0.31,0.19,120000,120000
-1550,496,2500.00,4000.00,2400.00,4050.00,0.21,-0.10,120000,120000
-1535,580,-650.00,4000.00,-750.00,4200.00,0.21,-0.38,120000,120000
-1546,613,650.00,1750.00,550.00,1650.00,0.21,0.19,120000,120000
-1553,579,450.00,-1600.00,350.00,-1700.00,0.21,0.19,120000,120000
-1556,597,250.00,1000.00,150.00,900.00,0.21,0.19,120000,120000
-1559,632,250.00,1850.00,150.00,1750.00,0.21,0.19,120000,120000
-1568,684,550.00,2700.00,450.00,2600.00,0.21,0.19,120000,120000
-1600,739,1700.00,2850.00,1600.00,2750.00,0.21,0.19,120000,120000
-1644,805,2300.00,3400.00,2200.00,3300.00,0.21,0.19,120000,120000
-1702,875,3000.00,3600.00,2900.00,3500.00,0.21,0.19,120000,120000
-1770,956,3500.00,4000.00,3400.00,4050.00,0.21,-0.10,120000,120000
-1847,1035,3950.00,4000.00,3850.00,3950.00,0.21,0.10,120000,120000
-1934,1100,4000.00,3350.00,4350.00,3250.00,-0.72,0.19,120000,120000
-1960,1156,1400.00,2900.00,1300.00,2800.00,0.21,0.19,120000,120000
-1880,1223,-4000.00,3450.00,-4000.00,3350.00,0.00,0.19,120000,120000
-1838,1297,-2000.00,3800.00,-2100.00,3700.00,0.21,0.19,120000,120000
-1820,1373,-800.00,3900.00,-900.00,3800.00,0.21,0.19,120000,120000
-1823,1458,250.00,4000.00,150.00,4250.00,0.21,-0.48,120000,120000
-1840,1506,950.00,2500.00,850.00,2400.00,0.21,0.19,120000,120000
-1882,1473,2200.00,-1550.00,2100.00,-1650.00,0.21,0.19,120000,120000
-1934,1466,2700.00,-250.00,2600.00,-350.00,0.21,0.19,120000,120000
-1993,1481,3050.00,850.00,2950.00,750.00,0.21,0.19,120000,120000
-2065,1492,3700.00,650.00,3600.00,550.00,0.21,0.19,120000,120000
-2144,1505,4000.00,750.00,3950.00,650.00,0.10,0.19,120000,120000
-2226,1541,4000.00,1900.00,4100.00,1800.00,-0.21,0.19,120000,120000
-2281,1585,2850.00,2300.00,2750.00,2200.00,0.21,0.19,120000,120000
-2290,1639,550.00,2800.00,450.00,2700.00,0.21,0.19,120000,120000
-2304,1707,800.00,3500.00,700.00,3400.00,0.21,0.19,120000,120000
-2341,1776,1950.00,3550.00,1850.00,3450.00,0.21,0.19,120000,120000
-2388,1852,2450.00,3900.00,2350.00,3800.00,0.21,0.19,120000,120000
-2447,1937,3050.00,4000.00,2950.00,4250.00,0.21,-0.48,120000,120000
-2514,1980,3450.00,2250.00,3350.00,2150.00,0.21,0.19,120000,120000
-2593,1956,4000.00,-1100.00,3950.00,-1200.00,0.10,0.19,120000,120000
-2678,1953,4000.00,-50.00,4250.00,-150.00,-0.52,0.19,120000,120000
-2700,1961,1200.00,500.00,1100.00,400.00,0.21,0.19,120000,120000
-2642,1983,-2800.00,1200.00,-2900.00,1100.00,0.21,0.19,120000,120000
-2646,2028,300.00,2350.00,200.00,2250.00,0.21,0.19,120000,120000
-2686,2077,2100.00,2550.00,2000.00,2450.00,0.21,0.19,120000,120000
-2736,2134,2600.00,2950.00,2500.00,2850.00,0.21,0.19,120000,120000
-2797,2205,3150.00,3650.00,3050.00,3550.00,0.21,0.19,120000,120000
-2868,2279,3650.00,3800.00,3550.00,3700.00,0.21,0.19,120000,120000
-2948,2361,4000.00,4000.00,4000.00,4100.00,0.00,-0.19,120000,120000
-3027,2425,4000.00,3300.00,3950.00,3200.00,0.10,0.19,120000,120000
-3097,2450,3600.00,1350.00,3500.00,1250.00,0.21,0.19,120000,120000
-3157,2490,3100.00,2100.00,3000.00,2000.00,0.21,0.19,120000,120000
-3239,2549,4000.00,3050.00,4100.00,2950.00,-0.21,0.19,120000,120000
-3294,2604,2850.00,2850.00,2750.00,2750.00,0.21,0.19,120000,120000
-3307,2673,750.00,3550.00,650.00,3450.00,0.21,0.19,120000,120000
-3348,2744,2150.00,3650.00,2050.00,3550.00,0.21,0.19,120000,120000
-3390,2825,2200.00,4000.00,2100.00,4050.00,0.21,-0.10,120000,120000
-3458,2905,3500.00,4000.00,3400.00,4000.00,0.21,0.00,120000,120000
-3534,2970,3900.00,3350.00,3800.00,3250.00,0.21,0.19,120000,120000
-3615,3026,4000.00,2900.00,4050.00,2800.00,-0.10,0.19,120000,120000
-3696,3094,4000.00,3500.00,4050.00,3400.00,-0.10,0.19,120000,120000
-3762,3167,3400.00,3750.00,3300.00,3650.00,0.21,0.19,120000,120000
-3821,3243,3050.00,3900.00,2950.00,3800.00,0.21,0.19,120000,120000
-3896,3327,3850.00,4000.00,3750.00,4200.00,0.21,-0.38,120000,120000
-3974,3374,4000.00,2450.00,3900.00,2350.00,0.21,0.19,120000,120000
-4056,3363,4000.00,-450.00,4100.00,-550.00,-0.21,0.19,120000,120000
-4126,3371,3600.00,500.00,3500.00,400.00,0.21,0.19,120000,120000
-4159,3405,1750.00,1800.00,1650.00,1700.00,0.21,0.19,120000,120000
-4202,3447,2250.00,2200.00,2150.00,2100.00,0.21,0.19,120000,120000
-4251,3504,2550.00,2950.00,2450.00,2850.00,0.21,0.19,120000,120000
-4322,3571,3650.00,3450.00,3550.00,3350.00,0.21,0.19,120000,120000
-4396,3643,3800.00,3700.00,3700.00,3600.00,0.21,0.19,120000,120000
-4478,3722,4000.00,4000.00,4100.00,3950.00,-0.21,0.10,120000,120000
-4544,3797,3400.00,3850.00,3300.00,3750.00,0.21,0.19,120000,120000
-4566,3863,1200.00,3400.00,1100.00,3300.00,0.21,0.19,120000,120000
-4602,3941,1900.00,4000.00,1800.00,3900.00,0.21,0.19,120000,120000
-4652,4021,2600.00,4000.00,2500.00,4000.00,0.21,0.00,120000,120000
-4718,4101,3400.00,4000.00,3300.00,4000.00,0.21,0.00,120000,120000
-4790,4164,3700.00,3250.00,3600.00,3150.00,0.21,0.19,120000,120000
-4870,4220,4000.00,2900.00,4000.00,2800.00,0.00,0.19,120000,120000
-4952,4291,4000.00,3650.00,4100.00,3550.00,-0.21,0.19,120000,120000
-5005,4361,2750.00,3600.00,2650.00,3500.00,0.21,0.19,120000,120000
-5021,4441,900.00,4000.00,800.00,4000.00,0.21,0.00,120000,120000
-5038,4520,950.00,4000.00,850.00,3950.00,0.21,0.10,120000,120000
-5081,4585,2250.00,3350.00,2150.00,3250.00,0.21,0.19,120000,120000
-5135,4643,2800.00,3000.00,2700.00,2900.00,0.21,0.19,120000,120000
-5196,4709,3150.00,3400.00,3050.00,3300.00,0.21,0.19,120000,120000
-5267,4781,3650.00,3700.00,3550.00,3600.00,0.21,0.19,120000,120000
-5346,4857,4000.00,3900.00,3950.00,3800.00,0.10,0.19,120000,120000
-5426,4940,4000.00,4000.00,4000.00,4150.00,0.00,-0.29,120000,120000
-5493,4999,3450.00,3050.00,3350.00,2950.00,0.21,0.19,120000,120000
-5555,5005,3200.00,400.00,3100.00,300.00,0.21,0.19,120000,120000
-5628,5011,3750.00,400.00,3650.00,300.00,0.21,0.19,120000,120000
-5704,5013,3900.00,200.00,3800.00,100.00,0.21,0.19,120000,120000
-5789,5013,4000.00,100.00,4250.00,0.00,-0.52,0.19,120000,120000
-5831,5013,2200.00,100.00,2100.00,0.00,0.21,0.19,120000,120000
-5785,5015,-2200.00,200.00,-2300.00,100.00,0.21,0.19,120000,120000
-5792,5017,450.00,200.00,350.00,100.00,0.21,0.19,120000,120000
-5826,5023,1800.00,400.00,1700.00,300.00,0.21,0.19,120000,120000
-5875,5041,2550.00,1000.00,2450.00,900.00,0.21,0.19,120000,120000
-5934,5083,3050.00,2200.00,2950.00,2100.00,0.21,0.19,120000,120000
-6003,5134,3550.00,2650.00,3450.00,2550.00,0.21,0.19,120000,120000
-6082,5190,4000.00,2900.00,3950.00,2800.00,0.10,0.19,120000,120000
-6160,5261,4000.00,3650.00,3900.00,3550.00,0.21,0.19,120000,120000
-6231,5333,3650.00,3700.00,3550.00,3600.00,0.21,0.19,120000,120000
-6309,5413,4000.00,4000.00,3900.00,4000.00,0.21,0.00,120000,120000
-6395,5493,4000.00,4000.00,4300.00,4000.00,-0.62,0.00,120000,120000
-6430,5559,1850.00,3400.00,1750.00,3300.00,0.21,0.19,120000,120000
-6362,5618,-3300.00,3050.00,-3400.00,2950.00,0.21,0.19,120000,120000
-6336,5685,-1200.00,3450.00,-1300.00,3350.00,0.21,0.19,120000,120000
-6343,5755,450.00,3600.00,350.00,3500.00,0.21,0.19,120000,120000
-6354,5832,650.00,3950.00,550.00,3850.00,0.21,0.19,120000,120000
-6366,5917,700.00,4000.00,600.00,4250.00,0.21,-0.48,120000,120000
-6400,5960,1800.00,2250.00,1700.00,2150.00,0.21,0.19,120000,120000
-6442,5916,2200.00,-2100.00,2100.00,-2200.00,0.21,0.19,120000,120000
-6504,5924,3200.00,500.00,3100.00,400.00,0.21,0.19,120000,120000
-6570,5957,3400.00,1750.00,3300.00,1650.00,0.21,0.19,120000,120000
-6650,6000,4000.00,2250.00,4000.00,2150.00,0.00,0.19,120000,120000
-6732,6052,4000.00,2700.00,4100.00,2600.00,-0.21,0.19,120000,120000
-6780,6110,2500.00,3000.00,2400.00,2900.00,0.21,0.19,120000,120000
-6813,6179,1750.00,3550.00,1650.00,3450.00,0.21,0.19,120000,120000
-6846,6253,1750.00,3800.00,1650.00,3700.00,0.21,0.19,120000,120000
-6898,6333,2700.00,4000.00,2600.00,4000.00,0.21,0.00,120000,120000
-6959,6415,3150.00,4000.00,3050.00,4100.00,0.21,-0.19,120000,120000
-7031,6465,3700.00,2600.00,3600.00,2500.00,0.21,0.19,120000,120000
-7114,6483,4000.00,1000.00,4150.00,900.00,-0.31,0.19,120000,120000
-7172,6501,3000.00,1000.00,2900.00,900.00,0.21,0.19,120000,120000
-7175,6535,250.00,1800.00,150.00,1700.00,0.21,0.19,120000,120000
-7218,6585,2250.00,2600.00,2150.00,2500.00,0.21,0.19,120000,120000
-7259,6639,2150.00,2800.00,2050.00,2700.00,0.21,0.19,120000,120000
-7319,6704,3100.00,3350.00,3000.00,3250.00,0.21,0.19,120000,120000
-7386,6773,3450.00,3550.00,3350.00,3450.00,0.21,0.19,120000,120000
-7462,6847,3900.00,3800.00,3800.00,3700.00,0.21,0.19,120000,120000
-7550,6929,4000.00,4000.00,4400.00,4100.00,-0.83,-0.19,120000,120000
-7586,7000,1900.00,3650.00,1800.00,3550.00,0.21,0.19,120000,120000
-7504,7029,-4000.00,1550.00,-4100.00,1450.00,0.21,0.19,120000,120000
-7475,7076,-1350.00,2450.00,-1450.00,2350.00,0.21,0.19,120000,120000
-7476,7123,150.00,2450.00,50.00,2350.00,0.21,0.19,120000,120000
-7484,7182,500.00,3050.00,400.00,2950.00,0.21,0.19,120000,120000
-7494,7247,600.00,3350.00,500.00,3250.00,0.21,0.19,120000,120000
-7513,7318,1050.00,3650.00,950.00,3550.00,0.21,0.19,120000,120000
-7558,7400,2350.00,4000.00,2250.00,4100.00,0.21,-0.19,120000,120000
-7612,7463,2800.00,3250.00,2700.00,3150.00,0.21,0.19,120000,120000
-7677,7491,3350.00,1500.00,3250.00,1400.00,0.21,0.19,120000,120000
-7748,7525,3650.00,1800.00,3550.00,1700.00,0.21,0.19,120000,120000
-7826,7563,4000.00,2000.00,3900.00,1900.00,0.21,0.19,120000,120000
-7914,7614,4000.00,2650.00,4400.00,2550.00,-0.83,0.19,120000,120000
-7937,7675,1250.00,3150.00,1150.00,3050.00,0.21,0.19,120000,120000
-7840,7735,-4000.00,3100.00,-4850.00,3000.00,1.00,0.19,120000,120000
-7846,7806,400.00,3650.00,300.00,3550.00,0.21,0.19,120000,120000
-8014,7886,4000.00,4000.00,8400.00,4000.00,-1.00,0.00,120000,120000
-8110,7957,4000.00,3650.00,4800.00,3550.00,-1.00,0.19,120000,120000
-7954,8021,-4000.00,3300.00,-7800.00,3200.00,1.00,0.19,120000,120000
-7718,8092,-4000.00,3650.00,-11800.00,3550.00,1.00,0.19,120000,120000
-7768,8165,2600.00,3750.00,2500.00,3650.00,0.21,0.19,120000,120000
-7988,8235,4000.00,3600.00,11000.00,3500.00,-1.00,0.19,120000,120000
-8138,8314,4000.00,4000.00,7500.00,3950.00,-1.00,0.10,120000,120000
-8031,8387,-4000.00,3750.00,-5350.00,3650.00,1.00,0.19,120000,120000
-7839,8452,-4000.00,3350.00,-9600.00,3250.00,1.00,0.19,120000,120000
-7924,8520,4000.00,3500.00,4250.00,3400.00,-0.52,0.19,120000,120000
-8117,8590,4000.00,3600.00,9650.00,3500.00,-1.00,0.19,120000,120000
-8118,8661,150.00,3650.00,50.00,3550.00,0.21,0.19,120000,120000
-7970,8739,-4000.00,4000.00,-7400.00,3900.00,1.00,0.19,120000,120000
-7938,8822,-1500.00,4000.00,-1600.00,4150.00,0.21,-0.29,120000,120000
-8074,8876,4000.00,2800.00,6800.00,2700.00,-1.00,0.19,120000,120000
-8148,8873,3800.00,-50.00,3700.00,-150.00,0.21,0.19,120000,120000
-8062,8872,-4000.00,50.00,-4300.00,-50.00,0.62,0.19,120000,120000
-8067,8877,350.00,350.00,250.00,250.00,0.21,0.19,120000,120000
-8170,8879,4000.00,200.00,5150.00,100.00,-1.00,0.19,120000,120000
-8212,8880,2200.00,150.00,2100.00,50.00,0.21,0.19,120000,120000
-8096,8880,-4000.00,100.00,-5800.00,0.00,1.00,0.19,120000,120000
-8082,8882,-600.00,200.00,-700.00,100.00,0.21,0.19,120000,120000
-8227,8883,4000.00,150.00,7250.00,50.00,-1.00,0.19,120000,120000
-8304,8885,3950.00,200.00,3850.00,100.00,0.21,0.19,120000,120000
-8214,8890,-4000.00,350.00,-4500.00,250.00,1.00,0.19,120000,120000
-8224,8904,600.00,800.00,500.00,700.00,0.21,0.19,120000,120000
-8382,8936,4000.00,1700.00,7900.00,1600.00,-1.00,0.19,120000,120000
-8477,8971,4000.00,1850.00,4750.00,1750.00,-1.00,0.19,120000,120000
-8339,9017,-4000.00,2400.00,-6900.00,2300.00,1.00,0.19,120000,120000
-8138,9067,-4000.00,2600.00,-10050.00,2500.00,1.00,0.19,120000,120000
-8188,9121,2600.00,2800.00,2500.00,2700.00,0.21,0.19,120000,120000
-8405,9174,4000.00,2750.00,10850.00,2650.00,-1.00,0.19,120000,120000
-8550,9237,4000.00,3250.00,7250.00,3150.00,-1.00,0.19,120000,120000
-8443,9301,-4000.00,3300.00,-5350.00,3200.00,1.00,0.19,120000,120000
-8266,9366,-4000.00,3350.00,-8850.00,3250.00,1.00,0.19,120000,120000
-8338,9433,3700.00,3450.00,3600.00,3350.00,0.21,0.19,120000,120000
-8566,9505,4000.00,3700.00,11400.00,3600.00,-1.00,0.19,120000,120000
-8712,9577,4000.00,3700.00,7300.00,3600.00,-1.00,0.19,120000,120000
-8604,9653,-4000.00,3900.00,-5400.00,3800.00,1.00,0.19,120000,120000
-8417,9729,-4000.00,3900.00,-9350.00,3800.00,1.00,0.19,120000,120000
-8464,9805,2450.00,3900.00,2350.00,3800.00,0.21,0.19,120000,120000
-8672,9883,4000.00,4000.00,10400.00,3900.00,-1.00,0.19,120000,120000
-8810,9965,4000.00,4000.00,6900.00,4100.00,-1.00,-0.19,120000,120000
-8700,10033,-4000.00,3500.00,-5500.00,3400.00,1.00,0.19,120000,120000
-8513,10069,-4000.00,1900.00,-9350.00,1800.00,1.00,0.19,120000,120000
-8599,10111,4000.00,2200.00,4300.00,2100.00,-0.62,0.19,120000,120000
-8786,10158,4000.00,2450.00,9350.00,2350.00,-1.00,0.19,120000,120000
-8775,10207,-450.00,2550.00,-550.00,2450.00,0.21,0.19,120000,120000
-8630,10265,-4000.00,3000.00,-7250.00,2900.00,1.00,0.19,120000,120000
-8602,10326,-1300.00,3150.00,-1400.00,3050.00,0.21,0.19,120000,120000
-8745,10397,4000.00,3650.00,7150.00,3550.00,-1.00,0.19,120000,120000
-8819,10467,3800.00,3600.00,3700.00,3500.00,0.21,0.19,120000,120000
-8734,10542,-4000.00,3850.00,-4250.00,3750.00,0.52,0.19,120000,120000
-8730,10629,-100.00,4000.00,-200.00,4350.00,0.21,-0.67,120000,120000
-8829,10653,4000.00,1300.00,4950.00,1200.00,-1.00,0.19,120000,120000
-8860,10560,1650.00,-4000.00,1550.00,-4650.00,0.21,1.00,120000,120000
-8747,10551,-4000.00,-350.00,-5650.00,-450.00,1.00,0.19,120000,120000
-8735,10714,-500.00,4000.00,-600.00,8150.00,0.21,-1.00,120000,120000
-8894,10809,4000.00,4000.00,7950.00,4750.00,-1.00,-1.00,120000,120000
-8995,10665,4000.00,-4000.00,5050.00,-7200.00,-1.00,1.00,120000,120000
-8913,10491,-4000.00,-4000.00,-4100.00,-8700.00,0.21,1.00,120000,120000
-8746,10551,-4000.00,3100.00,-8350.00,3000.00,1.00,0.19,120000,120000
-8696,10783,-2400.00,4000.00,-2500.00,11600.00,0.21,-1.00,120000,120000
-8819,10939,4000.00,4000.00,6150.00,7800.00,-1.00,-1.00,120000,120000
-8894,10854,3850.00,-4000.00,3750.00,-4250.00,0.21,0.48,120000,120000
-8832,10711,-3000.00,-4000.00,-3100.00,-7150.00,0.21,1.00,120000,120000
-8808,10761,-1100.00,2600.00,-1200.00,2500.00,0.21,0.19,120000,120000
-8808,10988,100.00,4000.00,0.00,11350.00,0.21,-1.00,120000,120000
-8817,11141,550.00,4000.00,450.00,7650.00,0.21,-1.00,120000,120000
-8817,11034,100.00,-4000.00,0.00,-5350.00,0.21,1.00,120000,120000
-8817,10849,100.00,-4000.00,0.00,-9250.00,0.21,1.00,120000,120000
-8816,10945,50.00,4000.00,-50.00,4800.00,0.21,-1.00,120000,120000
-8817,11133,150.00,4000.00,50.00,9400.00,0.21,-1.00,120000,120000
-8816,11078,50.00,-2650.00,-50.00,-2750.00,0.21,0.19,120000,120000
-8819,10877,250.00,-4000.00,150.00,-10050.00,0.21,1.00,120000,120000
-8824,10807,350.00,-3400.00,250.00,-3500.00,0.21,0.19,120000,120000
-8828,10923,300.00,4000.00,200.00,5800.00,0.21,-1.00,120000,120000
-8846,10984,1000.00,3150.00,900.00,3050.00,0.21,0.19,120000,120000
-8886,10890,2100.00,-4000.00,2000.00,-4700.00,0.21,1.00,120000,120000
-8932,10893,2400.00,250.00,2300.00,150.00,0.21,0.19,120000,120000
-8984,11074,2700.00,4000.00,2600.00,9050.00,0.21,-1.00,120000,120000
-9045,11186,3150.00,4000.00,3050.00,5600.00,0.21,-1.00,120000,120000
-9114,11052,3550.00,-4000.00,3450.00,-6700.00,0.21,1.00,120000,120000
-9186,10851,3700.00,-4000.00,3600.00,-10050.00,0.21,1.00,120000,120000
-9262,10923,3900.00,3700.00,3800.00,3600.00,0.21,0.19,120000,120000
-9338,11161,3900.00,4000.00,3800.00,11900.00,0.21,-1.00,120000,120000
-9420,11330,4000.00,4000.00,4100.00,8450.00,-0.21,-1.00,120000,120000
-9489,11253,3550.00,-3750.00,3450.00,-3850.00,0.21,0.19,120000,120000
-9525,11045,1900.00,-4000.00,1800.00,-10400.00,0.21,1.00,120000,120000
-9570,10975,2350.00,-3400.00,2250.00,-3500.00,0.21,0.19,120000,120000
-9625,11112,2850.00,4000.00,2750.00,6850.00,0.21,-1.00,120000,120000
-9686,11191,3150.00,4000.00,3050.00,3950.00,0.21,0.10,120000,120000
-9756,11097,3600.00,-4000.00,3500.00,-4700.00,0.21,1.00,120000,120000
-9833,11068,3950.00,-1350.00,3850.00,-1450.00,0.21,0.19,120000,120000
-9914,11211,4000.00,4000.00,4050.00,7150.00,-0.10,-1.00,120000,120000
-9996,11294,4000.00,4000.00,4100.00,4150.00,-0.21,-0.29,120000,120000
-10050,11179,2800.00,-4000.00,2700.00,-5750.00,0.21,1.00,120000,120000
-10078,11110,1500.00,-3350.00,1400.00,-3450.00,0.21,0.19,120000,120000
-10096,11246,1000.00,4000.00,900.00,6800.00,0.21,-1.00,120000,120000
-10124,11327,1500.00,4000.00,1400.00,4050.00,0.21,-0.10,120000,120000
-10166,11234,2200.00,-4000.00,2100.00,-4650.00,0.21,1.00,120000,120000
-10223,11200,2950.00,-1600.00,2850.00,-1700.00,0.21,0.19,120000,120000
-10287,11356,3300.00,4000.00,3200.00,7800.00,0.21,-1.00,120000,120000
-10356,11443,3550.00,4000.00,3450.00,4350.00,0.21,-0.67,120000,120000
-10427,11307,3650.00,-4000.00,3550.00,-6800.00,0.21,1.00,120000,120000
-10509,11145,4000.00,-4000.00,4100.00,-8100.00,-0.21,1.00,120000,120000
-10570,11255,3150.00,4000.00,3050.00,5500.00,0.21,-1.00,120000,120000
-10610,11451,2100.00,4000.00,2000.00,9800.00,0.21,-1.00,120000,120000
-10642,11389,1700.00,-3000.00,1600.00,-3100.00,0.21,0.19,120000,120000
-10697,11189,2850.00,-4000.00,2750.00,-10000.00,0.21,1.00,120000,120000
-10756,11123,3050.00,-3200.00,2950.00,-3300.00,0.21,0.19,120000,120000
-10819,11267,3250.00,4000.00,3150.00,7200.00,0.21,-1.00,120000,120000
-10890,11352,3650.00,4000.00,3550.00,4250.00,0.21,-0.48,120000,120000
-10964,11226,3800.00,-4000.00,3700.00,-6300.00,0.21,1.00,120000,120000
-11050,11113,4000.00,-4000.00,4300.00,-5650.00,-0.62,1.00,120000,120000
-11094,11265,2300.00,4000.00,2200.00,7600.00,0.21,-1.00,120000,120000
-11052,11478,-2000.00,4000.00,-2100.00,10650.00,0.21,-1.00,120000,120000
-11058,11420,400.00,-2800.00,300.00,-2900.00,0.21,0.19,120000,120000
-11088,11223,1600.00,-4000.00,1500.00,-9850.00,0.21,1.00,120000,120000
-11122,11159,1800.00,-3100.00,1700.00,-3200.00,0.21,0.19,120000,120000
-11166,11292,2300.00,4000.00,2200.00,6650.00,0.21,-1.00,120000,120000
-11221,11375,2850.00,4000.00,2750.00,4150.00,0.21,-0.29,120000,120000
-11277,11261,2900.00,-4000.00,2800.00,-5700.00,0.21,1.00,120000,120000
-11350,11189,3750.00,-3500.00,3650.00,-3600.00,0.21,0.19,120000,120000
-11421,11315,3650.00,4000.00,3550.00,6300.00,0.21,-1.00,120000,120000
-11498,11381,3950.00,3400.00,3850.00,3300.00,0.21,0.19,120000,120000
-11579,11293,4000.00,-4000.00,4050.00,-4400.00,-0.10,0.76,120000,120000
-11668,11291,4000.00,0.00,4450.00,-100.00,-0.93,0.19,120000,120000
-11664,11424,-100.00,4000.00,-200.00,6650.00,0.21,-1.00,120000,120000
-11504,11493,-4000.00,3550.00,-8000.00,3450.00,1.00,0.19,120000,120000
-11478,11371,-1200.00,-4000.00,-1300.00,-6100.00,0.21,1.00,120000,120000
-11606,11346,4000.00,-1150.00,6400.00,-1250.00,-1.00,0.19,120000,120000
-11672,11489,3400.00,4000.00,3300.00,7150.00,0.21,-1.00,120000,120000
-11580,11572,-4000.00,4000.00,-4600.00,4150.00,1.00,-0.29,120000,120000
-11586,11459,400.00,-4000.00,300.00,-5650.00,0.21,1.00,120000,120000
-11736,11395,4000.00,-3100.00,7500.00,-3200.00,-1.00,0.19,120000,120000
-11823,11528,4000.00,4000.00,4350.00,6650.00,-0.72,-1.00,120000,120000
-11699,11605,-4000.00,3950.00,-6200.00,3850.00,1.00,0.19,120000,120000
-11588,11541,-4000.00,-3100.00,-5550.00,-3200.00,1.00,0.19,120000,120000
-11697,11505,4000.00,-1700.00,5450.00,-1800.00,-1.00,0.19,120000,120000
-11882,11521,4000.00,900.00,9250.00,800.00,-1.00,0.19,120000,120000
-11805,11553,-3750.00,1700.00,-3850.00,1600.00,0.21,0.19,120000,120000
-11585,11585,-4000.00,1700.00,-11000.00,1600.00,1.00,0.19,120000,120000
-11506,11640,-3850.00,2850.00,-3950.00,2750.00,0.21,0.19,120000,120000
-11622,11693,4000.00,2750.00,5800.00,2650.00,-1.00,0.19,120000,120000
-11676,11754,2800.00,3150.00,2700.00,3050.00,0.21,0.19,120000,120000
-11586,11817,-4000.00,3250.00,-4500.00,3150.00,1.00,0.19,120000,120000
-11608,11886,1200.00,3550.00,1100.00,3450.00,0.21,0.19,120000,120000
-11766,11957,4000.00,3650.00,7900.00,3550.00,-1.00,0.19,120000,120000
-11868,12035,4000.00,4000.00,5100.00,3900.00,-1.00,0.19,120000,120000
-11744,12111,-4000.00,3900.00,-6200.00,3800.00,1.00,0.19,120000,120000
-11548,12195,-4000.00,4000.00,-9800.00,4200.00,1.00,-0.38,120000,120000
-11588,12247,2100.00,2700.00,2000.00,2600.00,0.21,0.19,120000,120000
-11779,12265,4000.00,1000.00,9550.00,900.00,-1.00,0.19,120000,120000
-11898,12286,4000.00,1150.00,5950.00,1050.00,-1.00,0.19,120000,120000
-11760,12298,-4000.00,700.00,-6900.00,600.00,1.00,0.19,120000,120000
-11559,12325,-4000.00,1450.00,-10050.00,1350.00,1.00,0.19,120000,120000
-11592,12355,1750.00,1600.00,1650.00,1500.00,0.21,0.19,120000,120000
-11787,12384,4000.00,1550.00,9750.00,1450.00,-1.00,0.19,120000,120000
-11909,12435,4000.00,2650.00,6100.00,2550.00,-1.00,0.19,120000,120000
-11780,12485,-4000.00,2600.00,-6450.00,2500.00,1.00,0.19,120000,120000
-11581,12541,-4000.00,2900.00,-9950.00,2800.00,1.00,0.19,120000,120000
-11634,12591,2750.00,2600.00,2650.00,2500.00,0.21,0.19,120000,120000
-11846,12653,4000.00,3200.00,10600.00,3100.00,-1.00,0.19,120000,120000
-11993,12727,4000.00,3800.00,7350.00,3700.00,-1.00,0.19,120000,120000
-11906,12795,-4000.00,3500.00,-4350.00,3400.00,0.72,0.19,120000,120000
-11724,12873,-4000.00,4000.00,-9100.00,3900.00,1.00,0.19,120000,120000
-11774,12952,2600.00,4000.00,2500.00,3950.00,0.21,0.10,120000,120000
-11986,13021,4000.00,3550.00,10600.00,3450.00,-1.00,0.19,120000,120000
-12126,13083,4000.00,3200.00,7000.00,3100.00,-1.00,0.19,120000,120000
-12029,13151,-4000.00,3500.00,-4850.00,3400.00,1.00,0.19,120000,120000
-11856,13221,-4000.00,3600.00,-8650.00,3500.00,1.00,0.19,120000,120000
-11930,13293,3800.00,3700.00,3700.00,3600.00,0.21,0.19,120000,120000
-12163,13364,4000.00,3650.00,11650.00,3550.00,-1.00,0.19,120000,120000
-12320,13439,4000.00,3850.00,7850.00,3750.00,-1.00,0.19,120000,120000
-12229,13521,-4000.00,4000.00,-4550.00,4100.00,1.00,-0.19,120000,120000
-12056,13588,-4000.00,3450.00,-8650.00,3350.00,1.00,0.19,120000,120000
-12136,13625,4000.00,1950.00,4000.00,1850.00,0.00,0.19,120000,120000
-12362,13662,4000.00,1950.00,11300.00,1850.00,-1.00,0.19,120000,120000
-12488,13702,4000.00,2100.00,6300.00,2000.00,-1.00,0.19,120000,120000
-12360,13755,-4000.00,2750.00,-6400.00,2650.00,1.00,0.19,120000,120000
-12166,13811,-4000.00,2900.00,-9700.00,2800.00,1.00,0.19,120000,120000
-12214,13869,2500.00,3000.00,2400.00,2900.00,0.21,0.19,120000,120000
-12422,13928,4000.00,3050.00,10400.00,2950.00,-1.00,0.19,120000,120000
-12553,13993,4000.00,3350.00,6550.00,3250.00,-1.00,0.19,120000,120000
-12436,14059,-4000.00,3400.00,-5850.00,3300.00,1.00,0.19,120000,120000
-12250,14130,-4000.00,3650.00,-9300.00,3550.00,1.00,0.19,120000,120000
-12314,14204,3300.00,3800.00,3200.00,3700.00,0.21,0.19,120000,120000
-12540,14277,4000.00,3750.00,11300.00,3650.00,-1.00,0.19,120000,120000
-12690,14353,4000.00,3900.00,7500.00,3800.00,-1.00,0.19,120000,120000
-12580,14433,-4000.00,4000.00,-5500.00,4000.00,1.00,0.00,120000,120000
-12384,14513,-4000.00,4000.00,-9800.00,4000.00,1.00,0.00,120000,120000
-12454,14577,3600.00,3300.00,3500.00,3200.00,0.21,0.19,120000,120000
-12686,14632,4000.00,2850.00,11600.00,2750.00,-1.00,0.19,120000,120000
-12845,14697,4000.00,3350.00,7950.00,3250.00,-1.00,0.19,120000,120000
-12731,14761,-4000.00,3300.00,-5700.00,3200.00,1.00,0.19,120000,120000
-12538,14832,-4000.00,3650.00,-9650.00,3550.00,1.00,0.19,120000,120000
-12590,14900,2700.00,3500.00,2600.00,3400.00,0.21,0.19,120000,120000
-12806,14970,4000.00,3600.00,10800.00,3500.00,-1.00,0.19,120000,120000
-12950,15046,4000.00,3900.00,7200.00,3800.00,-1.00,0.19,120000,120000
-12830,15125,-4000.00,4000.00,-6000.00,3950.00,1.00,0.10,120000,120000
-12639,15201,-4000.00,3900.00,-9550.00,3800.00,1.00,0.19,120000,120000
-12716,15264,3950.00,3250.00,3850.00,3150.00,0.21,0.19,120000,120000
-12953,15333,4000.00,3550.00,11850.00,3450.00,-1.00,0.19,120000,120000
-13110,15401,4000.00,3500.00,7850.00,3400.00,-1.00,0.19,120000,120000
-12972,15487,-4000.00,4000.00,-6900.00,4300.00,1.00,-0.57,120000,120000
-12747,15531,-4000.00,2300.00,-11250.00,2200.00,1.00,0.19,120000,120000
-12766,15521,1050.00,-400.00,950.00,-500.00,0.21,0.19,120000,120000
-12954,15539,4000.00,1000.00,9400.00,900.00,-1.00,0.19,120000,120000
-13074,15564,4000.00,1350.00,6000.00,1250.00,-1.00,0.19,120000,120000
-12963,15596,-4000.00,1700.00,-5550.00,1600.00,1.00,0.19,120000,120000
-12782,15633,-4000.00,1950.00,-9050.00,1850.00,1.00,0.19,120000,120000
-12857,15677,3850.00,2300.00,3750.00,2200.00,0.21,0.19,120000,120000
-13090,15721,4000.00,2300.00,11650.00,2200.00,-1.00,0.19,120000,120000
-13252,15776,4000.00,2850.00,8100.00,2750.00,-1.00,0.19,120000,120000
-13162,15833,-4000.00,2950.00,-4500.00,2850.00,1.00,0.19,120000,120000
-12999,15895,-4000.00,3200.00,-8150.00,3100.00,1.00,0.19,120000,120000
-13060,15955,3150.00,3100.00,3050.00,3000.00,0.21,0.19,120000,120000
-13286,16021,4000.00,3400.00,11300.00,3300.00,-1.00,0.19,120000,120000
-13426,16087,4000.00,3400.00,7000.00,3300.00,-1.00,0.19,120000,120000
-13313,16159,-4000.00,3700.00,-5650.00,3600.00,1.00,0.19,120000,120000
-13118,16232,-4000.00,3750.00,-9750.00,3650.00,1.00,0.19,120000,120000
-13185,16304,3450.00,3700.00,3350.00,3600.00,0.21,0.19,120000,120000
-13422,16381,4000.00,3950.00,11850.00,3850.00,-1.00,0.19,120000,120000
-13578,16464,4000.00,4000.00,7800.00,4150.00,-1.00,-0.29,120000,120000
-13478,16524,-4000.00,3100.00,-5000.00,3000.00,1.00,0.19,120000,120000
-13307,16545,-4000.00,1150.00,-8550.00,1050.00,1.00,0.19,120000,120000
-13376,16548,3550.00,250.00,3450.00,150.00,0.21,0.19,120000,120000
-13602,16546,4000.00,0.00,11300.00,-100.00,-1.00,0.19,120000,120000
-13754,16545,4000.00,50.00,7600.00,-50.00,-1.00,0.19,120000,120000
-13646,16542,-4000.00,-50.00,-5400.00,-150.00,1.00,0.19,120000,120000
-13454,16541,-4000.00,50.00,-9600.00,-50.00,1.00,0.19,120000,120000
-13514,16541,3100.00,100.00,3000.00,0.00,0.21,0.19,120000,120000
-13731,16543,4000.00,200.00,10850.00,100.00,-1.00,0.19,120000,120000
-13874,16543,4000.00,100.00,7150.00,0.00,-1.00,0.19,120000,120000
-13784,16541,-4000.00,0.00,-4500.00,-100.00,1.00,0.19,120000,120000
-13610,16541,-4000.00,100.00,-8700.00,0.00,1.00,0.19,120000,120000
-13666,16541,2900.00,100.00,2800.00,0.00,0.21,0.19,120000,120000
-13878,16543,4000.00,200.00,10600.00,100.00,-1.00,0.19,120000,120000
-14018,16544,4000.00,150.00,7000.00,50.00,-1.00,0.19,120000,120000
-13902,16542,-4000.00,0.00,-5800.00,-100.00,1.00,0.19,120000,120000
-13702,16542,-4000.00,100.00,-10000.00,0.00,1.00,0.19,120000,120000
-13765,16541,3250.00,50.00,3150.00,-50.00,0.21,0.19,120000,120000
-13988,16543,4000.00,200.00,11150.00,100.00,-1.00,0.19,120000,120000
-14141,16544,4000.00,150.00,7650.00,50.00,-1.00,0.19,120000,120000
-14046,16542,-4000.00,0.00,-4750.00,-100.00,1.00,0.19,120000,120000
-13871,16541,-4000.00,50.00,-8750.00,-50.00,1.00,0.19,120000,120000
-13924,16541,2750.00,100.00,2650.00,0.00,0.21,0.19,120000,120000
-14144,16544,4000.00,250.00,11000.00,150.00,-1.00,0.19,120000,120000
-14287,16545,4000.00,150.00,7150.00,50.00,-1.00,0.19,120000,120000
-14170,16542,-4000.00,-50.00,-5850.00,-150.00,1.00,0.19,120000,120000
-13970,16542,-4000.00,100.00,-10000.00,0.00,1.00,0.19,120000,120000
-14023,16541,2750.00,50.00,2650.00,-50.00,0.21,0.19,120000,120000
-14239,16543,4000.00,200.00,10800.00,100.00,-1.00,0.19,120000,120000
-14388,16544,4000.00,150.00,7450.00,50.00,-1.00,0.19,120000,120000
-14301,16542,-4000.00,0.00,-4350.00,-100.00,0.72,0.19,120000,120000
-14130,16543,-4000.00,150.00,-8550.00,50.00,1.00,0.19,120000,120000
-14167,16543,1950.00,100.00,1850.00,0.00,0.21,0.19,120000,120000
-14362,16543,4000.00,100.00,9750.00,0.00,-1.00,0.19,120000,120000
-14489,16544,4000.00,150.00,6350.00,50.00,-1.00,0.19,120000,120000
-14357,16542,-4000.00,0.00,-6600.00,-100.00,1.00,0.19,120000,120000
-14154,16542,-4000.00,100.00,-10150.00,0.00,1.00,0.19,120000,120000
-14198,16542,2300.00,100.00,2200.00,0.00,0.21,0.19,120000,120000
-14410,16543,4000.00,150.00,10600.00,50.00,-1.00,0.19,120000,120000
-14546,16544,4000.00,150.00,6800.00,50.00,-1.00,0.19,120000,120000
-14428,16542,-4000.00,0.00,-5900.00,-100.00,1.00,0.19,120000,120000
-14232,16542,-4000.00,100.00,-9800.00,0.00,1.00,0.19,120000,120000
-14291,16541,3050.00,50.00,2950.00,-50.00,0.21,0.19,120000,120000
-14520,16543,4000.00,200.00,11450.00,100.00,-1.00,0.19,120000,120000
-14674,16544,4000.00,150.00,7700.00,50.00,-1.00,0.19,120000,120000
-14584,16542,-4000.00,0.00,-4500.00,-100.00,1.00,0.19,120000,120000
-14418,16542,-4000.00,100.00,-8300.00,0.00,1.00,0.19,120000,120000
-14489,16541,3650.00,50.00,3550.00,-50.00,0.21,0.19,120000,120000
-14726,16544,4000.00,250.00,11850.00,150.00,-1.00,0.19,120000,120000
-14884,16544,4000.00,100.00,7900.00,0.00,-1.00,0.19,120000,120000
-14786,16542,-4000.00,0.00,-4900.00,-100.00,1.00,0.19,120000,120000
-14612,16541,-4000.00,50.00,-8700.00,-50.00,1.00,0.19,120000,120000
-14676,16541,3300.00,100.00,3200.00,0.00,0.21,0.19,120000,120000
-14904,16544,4000.00,250.00,11400.00,150.00,-1.00,0.19,120000,120000
-15054,16544,4000.00,100.00,7500.00,0.00,-1.00,0.19,120000,120000
-14966,16541,-4000.00,-50.00,-4400.00,-150.00,0.83,0.19,120000,120000
-14782,16542,-4000.00,150.00,-9200.00,50.00,1.00,0.19,120000,120000
-14877,16542,4000.00,100.00,4750.00,0.00,-1.00,0.19,120000,120000
-15055,16541,4000.00,50.00,8900.00,-50.00,-1.00,0.19,120000,120000
-15002,16541,-2550.00,100.00,-2650.00,0.00,0.21,0.19,120000,120000
-14822,16544,-4000.00,250.00,-9000.00,150.00,1.00,0.19,120000,120000
-14766,16545,-2700.00,150.00,-2800.00,50.00,0.21,0.19,120000,120000
-14878,16544,4000.00,50.00,5600.00,-50.00,-1.00,0.19,120000,120000
-14934,16545,2900.00,150.00,2800.00,50.00,0.21,0.19,120000,120000
-14846,16544,-4000.00,50.00,-4400.00,-50.00,0.83,0.19,120000,120000
-14863,16545,950.00,150.00,850.00,50.00,0.21,0.19,120000,120000
-14993,16546,4000.00,150.00,6500.00,50.00,-1.00,0.19,120000,120000
-15071,16545,4000.00,50.00,3900.00,-50.00,0.21,0.19,120000,120000
-15000,16545,-3450.00,100.00,-3550.00,0.00,0.21,0.19,120000,120000
-14987,16548,-550.00,250.00,-650.00,150.00,0.21,0.19,120000,120000
-15018,16549,1650.00,150.00,1550.00,50.00,0.21,0.19,120000,120000
-15063,16549,2350.00,100.00,2250.00,0.00,0.21,0.19,120000,120000
-15122,16551,3050.00,200.00,2950.00,100.00,0.21,0.19,120000,120000
-15194,16551,3700.00,100.00,3600.00,0.00,0.21,0.19,120000,120000
-15275,16552,4000.00,150.00,4050.00,50.00,-0.10,0.19,120000,120000
-15360,16553,4000.00,150.00,4250.00,50.00,-0.52,0.19,120000,120000
-15394,16555,1800.00,200.00,1700.00,100.00,0.21,0.19,120000,120000
-15378,16555,-700.00,100.00,-800.00,0.00,0.21,0.19,120000,120000
-15406,16557,1500.00,200.00,1400.00,100.00,0.21,0.19,120000,120000
-15442,16558,1900.00,150.00,1800.00,50.00,0.21,0.19,120000,120000
-15494,16564,2700.00,400.00,2600.00,300.00,0.21,0.19,120000,120000
-15558,16585,3300.00,1150.00,3200.00,1050.00,0.21,0.19,120000,120000
-15632,16629,3800.00,2300.00,3700.00,2200.00,0.21,0.19,120000,120000
-15710,16681,4000.00,2700.00,3900.00,2600.00,0.21,0.19,120000,120000
-15800,16742,4000.00,3150.00,4500.00,3050.00,-1.00,0.19,120000,120000
-15826,16813,1400.00,3650.00,1300.00,3550.00,0.21,0.19,120000,120000
-15720,16882,-4000.00,3550.00,-5300.00,3450.00,1.00,0.19,120000,120000
-15727,16962,450.00,4000.00,350.00,4000.00,0.21,0.00,120000,120000
-15897,17035,4000.00,3750.00,8500.00,3650.00,-1.00,0.19,120000,120000
-16003,17101,4000.00,3400.00,5300.00,3300.00,-1.00,0.19,120000,120000
-15880,17167,-4000.00,3400.00,-6150.00,3300.00,1.00,0.19,120000,120000
-15691,17233,-4000.00,3400.00,-9450.00,3300.00,1.00,0.19,120000,120000
-15740,17303,2550.00,3600.00,2450.00,3500.00,0.21,0.19,120000,120000
-15951,17373,4000.00,3600.00,10550.00,3500.00,-1.00,0.19,120000,120000
-16086,17447,4000.00,3800.00,6750.00,3700.00,-1.00,0.19,120000,120000
-15962,17528,-4000.00,4000.00,-6200.00,4050.00,1.00,-0.10,120000,120000
-15756,17604,-4000.00,3900.00,-10300.00,3800.00,1.00,0.19,120000,120000
-15806,17668,2600.00,3300.00,2500.00,3200.00,0.21,0.19,120000,120000
-16029,17740,4000.00,3700.00,11150.00,3600.00,-1.00,0.19,120000,120000
-16172,17816,4000.00,3900.00,7150.00,3800.00,-1.00,0.19,120000,120000
-16051,17888,-4000.00,3700.00,-6050.00,3600.00,1.00,0.19,120000,120000
-15850,17964,-4000.00,3900.00,-10050.00,3800.00,1.00,0.19,120000,120000
-15916,18037,3400.00,3750.00,3300.00,3650.00,0.21,0.19,120000,120000
-16142,18114,4000.00,3950.00,11300.00,3850.00,-1.00,0.19,120000,120000
-16300,18195,4000.00,4000.00,7900.00,4050.00,-1.00,-0.10,120000,120000
-16187,18273,-4000.00,4000.00,-5650.00,3900.00,1.00,0.19,120000,120000
-15996,18339,-4000.00,3400.00,-9550.00,3300.00,1.00,0.19,120000,120000
-16058,18408,3200.00,3550.00,3100.00,3450.00,0.21,0.19,120000,120000
-16284,18474,4000.00,3400.00,11300.00,3300.00,-1.00,0.19,120000,120000
-16438,18549,4000.00,3850.00,7700.00,3750.00,-1.00,0.19,120000,120000
-16326,18621,-4000.00,3700.00,-5600.00,3600.00,1.00,0.19,120000,120000
-16145,18696,-4000.00,3850.00,-9050.00,3750.00,1.00,0.19,120000,120000
-16198,18772,2750.00,3900.00,2650.00,3800.00,0.21,0.19,120000,120000
-16410,18846,4000.00,3800.00,10600.00,3700.00,-1.00,0.19,120000,120000
-16553,18929,4000.00,4000.00,7150.00,4150.00,-1.00,-0.29,120000,120000
-16442,18987,-4000.00,3000.00,-5550.00,2900.00,1.00,0.19,120000,120000
-16256,19008,-4000.00,1150.00,-9300.00,1050.00,1.00,0.19,120000,120000
-16328,19018,3700.00,600.00,3600.00,500.00,0.21,0.19,120000,120000
-16554,19025,4000.00,450.00,11300.00,350.00,-1.00,0.19,120000,120000
-16702,19042,4000.00,950.00,7400.00,850.00,-1.00,0.19,120000,120000
-16595,19080,-4000.00,2000.00,-5350.00,1900.00,1.00,0.19,120000,120000
-16415,19115,-4000.00,1850.00,-9000.00,1750.00,1.00,0.19,120000,120000
-16473,19159,3000.00,2300.00,2900.00,2200.00,0.21,0.19,120000,120000
-16686,19205,4000.00,2400.00,10650.00,2300.00,-1.00,0.19,120000,120000
-16828,19255,4000.00,2600.00,7100.00,2500.00,-1.00,0.19,120000,120000
-16710,19310,-4000.00,2850.00,-5900.00,2750.00,1.00,0.19,120000,120000
-16511,19375,-4000.00,3350.00,-9950.00,3250.00,1.00,0.19,120000,120000
-16571,19435,3100.00,3100.00,3000.00,3000.00,0.21,0.19,120000,120000
-16794,19499,4000.00,3300.00,11150.00,3200.00,-1.00,0.19,120000,120000
-16945,19570,4000.00,3650.00,7550.00,3550.00,-1.00,0.19,120000,120000
-16838,19641,-4000.00,3650.00,-5350.00,3550.00,1.00,0.19,120000,120000
-16651,19715,-4000.00,3800.00,-9350.00,3700.00,1.00,0.19,120000,120000
-16768,19802,4000.00,4000.00,5850.00,4350.00,-1.00,-0.67,120000,120000
-16976,19833,4000.00,1650.00,10400.00,1550.00,-1.00,0.19,120000,120000
-16958,19797,-800.00,-1700.00,-900.00,-1800.00,0.21,0.19,120000,120000
-16799,19794,-4000.00,-50.00,-7950.00,-150.00,1.00,0.19,120000,120000
-16761,19817,-1800.00,1250.00,-1900.00,1150.00,0.21,0.19,120000,120000
-16887,19834,4000.00,950.00,6300.00,850.00,-1.00,0.19,120000,120000
-16952,19873,3350.00,2050.00,3250.00,1950.00,0.21,0.19,120000,120000
-16857,19912,-4000.00,2050.00,-4750.00,1950.00,1.00,0.19,120000,120000
-16876,19968,1050.00,2900.00,950.00,2800.00,0.21,0.19,120000,120000
-17036,20023,4000.00,2850.00,8000.00,2750.00,-1.00,0.19,120000,120000
-17141,20085,4000.00,3200.00,5250.00,3100.00,-1.00,0.19,120000,120000
-17004,20150,-4000.00,3350.00,-6850.00,3250.00,1.00,0.19,120000,120000
-16795,20219,-4000.00,3550.00,-10450.00,3450.00,1.00,0.19,120000,120000
-16854,20288,3050.00,3550.00,2950.00,3450.00,0.21,0.19,120000,120000
-17074,20355,4000.00,3450.00,11000.00,3350.00,-1.00,0.19,120000,120000
-17223,20428,4000.00,3750.00,7450.00,3650.00,-1.00,0.19,120000,120000
-17119,20498,-4000.00,3600.00,-5200.00,3500.00,1.00,0.19,120000,120000
-16934,20576,-4000.00,4000.00,-9250.00,3900.00,1.00,0.19,120000,120000
-16990,20651,2900.00,3850.00,2800.00,3750.00,0.21,0.19,120000,120000
-17202,20726,4000.00,3850.00,10600.00,3750.00,-1.00,0.19,120000,120000
-17341,20807,4000.00,4000.00,6950.00,4050.00,-1.00,-0.10,120000,120000
-17241,20881,-4000.00,3800.00,-5000.00,3700.00,1.00,0.19,120000,120000
-17056,20947,-4000.00,3400.00,-9250.00,3300.00,1.00,0.19,120000,120000
-17128,21016,3700.00,3550.00,3600.00,3450.00,0.21,0.19,120000,120000
-17354,21089,4000.00,3750.00,11300.00,3650.00,-1.00,0.19,120000,120000
-17509,21163,4000.00,3800.00,7750.00,3700.00,-1.00,0.19,120000,120000
-17404,21240,-4000.00,3950.00,-5250.00,3850.00,1.00,0.19,120000,120000
-17218,21318,-4000.00,4000.00,-9300.00,3900.00,1.00,0.19,120000,120000
-17278,21398,3100.00,4000.00,3000.00,4000.00,0.21,0.00,120000,120000
-17506,21472,4000.00,3800.00,11400.00,3700.00,-1.00,0.19,120000,120000
-17651,21533,4000.00,3150.00,7250.00,3050.00,-1.00,0.19,120000,120000
-17557,21595,-4000.00,3200.00,-4700.00,3100.00,1.00,0.19,120000,120000
-17379,21665,-4000.00,3600.00,-8900.00,3500.00,1.00,0.19,120000,120000
-17456,21734,3950.00,3550.00,3850.00,3450.00,0.21,0.19,120000,120000
-17698,21804,4000.00,3600.00,12100.00,3500.00,-1.00,0.19,120000,120000
-17857,21878,4000.00,3800.00,7950.00,3700.00,-1.00,0.19,120000,120000
-17786,21950,-3450.00,3700.00,-3550.00,3600.00,0.21,0.19,120000,120000
-17594,22027,-4000.00,3950.00,-9600.00,3850.00,1.00,0.19,120000,120000
-17531,22106,-3050.00,4000.00,-3150.00,3950.00,0.21,0.10,120000,120000
-17642,22185,4000.00,4000.00,5550.00,3950.00,-1.00,0.10,120000,120000
-17692,22245,2600.00,3100.00,2500.00,3000.00,0.21,0.19,120000,120000
-17598,22302,-4000.00,2950.00,-4700.00,2850.00,1.00,0.19,120000,120000
-17614,22373,900.00,3650.00,800.00,3550.00,0.21,0.19,120000,120000
-17783,22441,4000.00,3500.00,8450.00,3400.00,-1.00,0.19,120000,120000
-17885,22513,4000.00,3700.00,5100.00,3600.00,-1.00,0.19,120000,120000
-17752,22585,-4000.00,3700.00,-6650.00,3600.00,1.00,0.19,120000,120000
-17542,22662,-4000.00,3950.00,-10500.00,3850.00,1.00,0.19,120000,120000
-17577,22741,1850.00,4000.00,1750.00,3950.00,0.21,0.10,120000,120000
-17773,22813,4000.00,3700.00,9800.00,3600.00,-1.00,0.19,120000,120000
-17900,22871,4000.00,3000.00,6350.00,2900.00,-1.00,0.19,120000,120000
-17772,22937,-4000.00,3400.00,-6400.00,3300.00,1.00,0.19,120000,120000
-17571,23009,-4000.00,3700.00,-10050.00,3600.00,1.00,0.19,120000,120000
-17632,23079,3150.00,3600.00,3050.00,3500.00,0.21,0.19,120000,120000
-17852,23149,4000.00,3600.00,11000.00,3500.00,-1.00,0.19,120000,120000
-18000,23223,4000.00,3800.00,7400.00,3700.00,-1.00,0.19,120000,120000
-17910,23299,-4000.00,3900.00,-4500.00,3800.00,1.00,0.19,120000,120000
-17748,23377,-4000.00,4000.00,-8100.00,3900.00,1.00,0.19,120000,120000
-17815,23458,3450.00,4000.00,3350.00,4050.00,0.21,-0.10,120000,120000
-18048,23533,4000.00,3850.00,11650.00,3750.00,-1.00,0.19,120000,120000
-18202,23597,4000.00,3300.00,7700.00,3200.00,-1.00,0.19,120000,120000
-18106,23663,-4000.00,3400.00,-4800.00,3300.00,1.00,0.19,120000,120000
-17930,23736,-4000.00,3750.00,-8800.00,3650.00,1.00,0.19,120000,120000
-18004,23801,3800.00,3350.00,3700.00,3250.00,0.21,0.19,120000,120000
-18231,23873,4000.00,3700.00,11350.00,3600.00,-1.00,0.19,120000,120000
-18391,23950,4000.00,3950.00,8000.00,3850.00,-1.00,0.19,120000,120000
-18307,24028,-4000.00,4000.00,-4200.00,3900.00,0.41,0.19,120000,120000
-18106,24110,-4000.00,4000.00,-10050.00,4100.00,1.00,-0.19,120000,120000
-18079,24177,-1250.00,3450.00,-1350.00,3350.00,0.21,0.19,120000,120000
-18222,24222,4000.00,2350.00,7150.00,2250.00,-1.00,0.19,120000,120000
-18298,24273,3900.00,2650.00,3800.00,2550.00,0.21,0.19,120000,120000
-18208,24324,-4000.00,2650.00,-4500.00,2550.00,1.00,0.19,120000,120000
-18223,24389,850.00,3350.00,750.00,3250.00,0.21,0.19,120000,120000
-18382,24454,4000.00,3350.00,7950.00,3250.00,-1.00,0.19,120000,120000
-18479,24525,4000.00,3650.00,4850.00,3550.00,-1.00,0.19,120000,120000
-18342,24599,-4000.00,3800.00,-6850.00,3700.00,1.00,0.19,120000,120000
-18132,24677,-4000.00,4000.00,-10500.00,3900.00,1.00,0.19,120000,120000
-18221,24772,4000.00,4000.00,4450.00,4750.00,-0.93,-1.00,120000,120000
-18418,24798,4000.00,1400.00,9850.00,1300.00,-1.00,0.19,120000,120000
-18396,24720,-1000.00,-3800.00,-1100.00,-3900.00,0.21,0.19,120000,120000
-18228,24689,-4000.00,-1450.00,-8400.00,-1550.00,1.00,0.19,120000,120000
-18188,24685,-1900.00,-100.00,-2000.00,-200.00,0.21,0.19,120000,120000
-18322,24687,4000.00,200.00,6700.00,100.00,-1.00,0.19,120000,120000
-18397,24689,3850.00,200.00,3750.00,100.00,0.21,0.19,120000,120000
-18328,24692,-3350.00,250.00,-3450.00,150.00,0.21,0.19,120000,120000
-18317,24704,-450.00,700.00,-550.00,600.00,0.21,0.19,120000,120000
-18350,24742,1750.00,2000.00,1650.00,1900.00,0.21,0.19,120000,120000
-18397,24787,2450.00,2350.00,2350.00,2250.00,0.21,0.19,120000,120000
-18458,24850,3150.00,3250.00,3050.00,3150.00,0.21,0.19,120000,120000
-18531,24917,3750.00,3450.00,3650.00,3350.00,0.21,0.19,120000,120000
-18612,24997,4000.00,4000.00,4050.00,4000.00,-0.10,0.00,120000,120000
-18696,25076,4000.00,4000.00,4200.00,3950.00,-0.41,0.10,120000,120000
-18738,25139,2200.00,3250.00,2100.00,3150.00,0.21,0.19,120000,120000
-18725,25193,-550.00,2800.00,-650.00,2700.00,0.21,0.19,120000,120000
-18748,25261,1250.00,3500.00,1150.00,3400.00,0.21,0.19,120000,120000
-18766,25334,1000.00,3750.00,900.00,3650.00,0.21,0.19,120000,120000
-18809,25418,2250.00,4000.00,2150.00,4200.00,0.21,-0.38,120000,120000
-18856,25472,2450.00,2800.00,2350.00,2700.00,0.21,0.19,120000,120000
-18919,25463,3250.00,-350.00,3150.00,-450.00,0.21,0.19,120000,120000
-18994,25478,3850.00,850.00,3750.00,750.00,0.21,0.19,120000,120000
-19078,25496,4000.00,1000.00,4200.00,900.00,-0.41,0.19,120000,120000
-19133,25525,2850.00,1550.00,2750.00,1450.00,0.21,0.19,120000,120000
-19134,25566,150.00,2150.00,50.00,2050.00,0.21,0.19,120000,120000
-19150,25622,900.00,2900.00,800.00,2800.00,0.21,0.19,120000,120000
-19162,25688,700.00,3400.00,600.00,3300.00,0.21,0.19,120000,120000
-19191,25759,1550.00,3650.00,1450.00,3550.00,0.21,0.19,120000,120000
-19240,25839,2550.00,4000.00,2450.00,4000.00,0.21,0.00,120000,120000
-19298,25922,3000.00,4000.00,2900.00,4150.00,0.21,-0.29,120000,120000
-19368,25963,3600.00,2150.00,3500.00,2050.00,0.21,0.19,120000,120000
-19450,25956,4000.00,-250.00,4100.00,-350.00,-0.21,0.19,120000,120000
-19519,25965,3550.00,550.00,3450.00,450.00,0.21,0.19,120000,120000
-19556,25995,1950.00,1600.00,1850.00,1500.00,0.21,0.19,120000,120000
-19594,26042,2000.00,2450.00,1900.00,2350.00,0.21,0.19,120000,120000
-19648,26099,2800.00,2950.00,2700.00,2850.00,0.21,0.19,120000,120000
-19711,26166,3250.00,3450.00,3150.00,3350.00,0.21,0.19,120000,120000
-19790,26241,4000.00,3850.00,3950.00,3750.00,0.10,0.19,120000,120000
-19867,26322,3950.00,4000.00,3850.00,4050.00,0.21,-0.10,120000,120000
-19942,26409,3850.00,4000.00,3750.00,4350.00,0.21,-0.67,120000,120000
-20028,26427,4000.00,1000.00,4300.00,900.00,-0.62,0.19,120000,120000
-20064,26325,1900.00,-4000.00,1800.00,-5100.00,0.21,1.00,120000,120000
-20008,26342,-2700.00,950.00,-2800.00,850.00,0.21,0.19,120000,120000
-19992,26544,-700.00,4000.00,-800.00,10100.00,0.21,-1.00,120000,120000
-20015,26679,1250.00,4000.00,1150.00,6750.00,0.21,-1.00,120000,120000
-20030,26561,850.00,-4000.00,750.00,-5900.00,0.21,1.00,120000,120000
-20054,26371,1300.00,-4000.00,1200.00,-9500.00,0.21,1.00,120000,120000
-20090,26454,1900.00,4000.00,1800.00,4150.00,0.21,-0.29,120000,120000
-20121,26679,1650.00,4000.00,1550.00,11250.00,0.21,-1.00,120000,120000
-20174,26742,2750.00,3250.00,2650.00,3150.00,0.21,0.19,120000,120000
-20232,26635,3000.00,-4000.00,2900.00,-5350.00,0.21,1.00,120000,120000
-20302,26631,3600.00,-100.00,3500.00,-200.00,0.21,0.19,120000,120000
-20375,26806,3750.00,4000.00,3650.00,8750.00,0.21,-1.00,120000,120000
-20454,26921,4000.00,4000.00,3950.00,5750.00,0.10,-1.00,120000,120000
-20530,26793,3900.00,-4000.00,3800.00,-6400.00,0.21,1.00,120000,120000
-20595,26592,3350.00,-4000.00,3250.00,-10050.00,0.21,1.00,120000,120000
-20670,26662,3850.00,3600.00,3750.00,3500.00,0.21,0.19,120000,120000
-20747,26909,3950.00,4000.00,3850.00,12350.00,0.21,-1.00,120000,120000
-20828,27080,4000.00,4000.00,4050.00,8550.00,-0.10,-1.00,120000,120000
-20903,26981,3850.00,-4000.00,3750.00,-4950.00,0.21,1.00,120000,120000
-20968,26804,3350.00,-4000.00,3250.00,-8850.00,0.21,1.00,120000,120000
-21040,26869,3700.00,3350.00,3600.00,3250.00,0.21,0.19,120000,120000
-21114,27099,3800.00,4000.00,3700.00,11500.00,0.21,-1.00,120000,120000
-21198,27260,4000.00,4000.00,4200.00,8050.00,-0.41,-1.00,120000,120000
-21250,27173,2700.00,-4000.00,2600.00,-4350.00,0.21,0.67,120000,120000
-21264,27001,800.00,-4000.00,700.00,-8600.00,0.21,1.00,120000,120000
-21288,27047,1300.00,2400.00,1200.00,2300.00,0.21,0.19,120000,120000
-21295,27268,450.00,4000.00,350.00,11050.00,0.21,-1.00,120000,120000
-21323,27418,1500.00,4000.00,1400.00,7500.00,0.21,-1.00,120000,120000
-21358,27301,1850.00,-4000.00,1750.00,-5850.00,0.21,1.00,120000,120000
-21395,27104,1950.00,-4000.00,1850.00,-9850.00,0.21,1.00,120000,120000
-21446,27178,2650.00,3800.00,2550.00,3700.00,0.21,0.19,120000,120000
-21502,27411,2900.00,4000.00,2800.00,11650.00,0.21,-1.00,120000,120000
-21568,27578,3400.00,4000.00,3300.00,8350.00,0.21,-1.00,120000,120000
-21634,27487,3400.00,-4000.00,3300.00,-4550.00,0.21,1.00,120000,120000
-21724,27306,4000.00,-4000.00,4500.00,-9050.00,-1.00,1.00,120000,120000
-21739,27428,850.00,4000.00,750.00,6100.00,0.21,-1.00,120000,120000
-21648,27589,-4000.00,4000.00,-4550.00,8050.00,1.00,-1.00,120000,120000
-21636,27513,-500.00,-3700.00,-600.00,-3800.00,0.21,0.19,120000,120000
-21749,27341,4000.00,-4000.00,5650.00,-8600.00,-1.00,1.00,120000,120000
-21815,27290,3400.00,-2450.00,3300.00,-2550.00,0.21,0.19,120000,120000
-21764,27385,-2450.00,4000.00,-2550.00,4750.00,0.21,-1.00,120000,120000
-21754,27425,-400.00,2100.00,-500.00,2000.00,0.21,0.19,120000,120000
-21755,27313,150.00,-4000.00,50.00,-5600.00,0.21,1.00,120000,120000
-21765,27307,600.00,-200.00,500.00,-300.00,0.21,0.19,120000,120000
-21788,27481,1250.00,4000.00,1150.00,8700.00,0.21,-1.00,120000,120000
-21834,27593,2400.00,4000.00,2300.00,5600.00,0.21,-1.00,120000,120000
-21887,27465,2750.00,-4000.00,2650.00,-6400.00,0.21,1.00,120000,120000
-21944,27261,2950.00,-4000.00,2850.00,-10200.00,0.21,1.00,120000,120000
-22002,27325,3000.00,3300.00,2900.00,3200.00,0.21,0.19,120000,120000
-22064,27551,3200.00,4000.00,3100.00,11300.00,0.21,-1.00,120000,120000
-22141,27707,3950.00,4000.00,3850.00,7800.00,0.21,-1.00,120000,120000
-22217,27600,3900.00,-4000.00,3800.00,-5350.00,0.21,1.00,120000,120000
-22298,27412,4000.00,-4000.00,4050.00,-9400.00,-0.10,1.00,120000,120000
-22370,27481,3700.00,3550.00,3600.00,3450.00,0.21,0.19,120000,120000
-22435,27709,3350.00,4000.00,3250.00,11400.00,0.21,-1.00,120000,120000
-22515,27869,4000.00,4000.00,4000.00,8000.00,0.00,-1.00,120000,120000
-22584,27779,3550.00,-4000.00,3450.00,-4500.00,0.21,0.95,120000,120000
-22649,27610,3350.00,-4000.00,3250.00,-8450.00,0.21,1.00,120000,120000
-22722,27676,3750.00,3400.00,3650.00,3300.00,0.21,0.19,120000,120000
-22796,27897,3800.00,4000.00,3700.00,11050.00,0.21,-1.00,120000,120000
-22878,28043,4000.00,4000.00,4100.00,7300.00,-0.21,-1.00,120000,120000
-22940,27945,3200.00,-4000.00,3100.00,-4900.00,0.21,1.00,120000,120000
-22970,27765,1600.00,-4000.00,1500.00,-9000.00,0.21,1.00,120000,120000
-23009,27848,2050.00,4000.00,1950.00,4150.00,0.21,-0.29,120000,120000
-23046,28053,1950.00,4000.00,1850.00,10250.00,0.21,-1.00,120000,120000
-23099,28108,2750.00,2850.00,2650.00,2750.00,0.21,0.19,120000,120000
-23159,28021,3100.00,-4000.00,3000.00,-4350.00,0.21,0.67,120000,120000
-23230,28012,3650.00,-350.00,3550.00,-450.00,0.21,0.19,120000,120000
-23302,28124,3700.00,4000.00,3600.00,5600.00,0.21,-1.00,120000,120000
-23382,28184,4000.00,3100.00,4000.00,3000.00,0.00,0.19,120000,120000
-23460,28100,4000.00,-4000.00,3900.00,-4200.00,0.21,0.38,120000,120000
-23532,28076,3700.00,-1100.00,3600.00,-1200.00,0.21,0.19,120000,120000
-23614,28116,4000.00,2100.00,4100.00,2000.00,-0.21,0.19,120000,120000
-23678,28177,3300.00,3150.00,3200.00,3050.00,0.21,0.19,120000,120000
-23694,28248,900.00,3650.00,800.00,3550.00,0.21,0.19,120000,120000
-23725,28325,1650.00,3950.00,1550.00,3850.00,0.21,0.19,120000,120000
-23769,28411,2300.00,4000.00,2200.00,4300.00,0.21,-0.57,120000,120000
-23830,28452,3150.00,2150.00,3050.00,2050.00,0.21,0.19,120000,120000
-23897,28407,3450.00,-2150.00,3350.00,-2250.00,0.21,0.19,120000,120000
-23978,28398,4000.00,-350.00,4050.00,-450.00,-0.10,0.19,120000,120000
-24056,28417,4000.00,1050.00,3900.00,950.00,0.21,0.19,120000,120000
-24127,28431,3650.00,800.00,3550.00,700.00,0.21,0.19,120000,120000
-24212,28464,4000.00,1750.00,4250.00,1650.00,-0.52,0.19,120000,120000
-24262,28507,2600.00,2250.00,2500.00,2150.00,0.21,0.19,120000,120000
-24248,28565,-600.00,3000.00,-700.00,2900.00,0.21,0.19,120000,120000
-24277,28631,1550.00,3400.00,1450.00,3300.00,0.21,0.19,120000,120000
-24303,28701,1400.00,3600.00,1300.00,3500.00,0.21,0.19,120000,120000
-24355,28778,2700.00,3950.00,2600.00,3850.00,0.21,0.19,120000,120000
-24413,28862,3000.00,4000.00,2900.00,4200.00,0.21,-0.38,120000,120000
-24482,28911,3550.00,2550.00,3450.00,2450.00,0.21,0.19,120000,120000
-24560,28908,4000.00,-50.00,3900.00,-150.00,0.21,0.19,120000,120000
-24650,28937,4000.00,1550.00,4500.00,1450.00,-1.00,0.19,120000,120000
-24669,28971,1050.00,1800.00,950.00,1700.00,0.21,0.19,120000,120000
-24544,29019,-4000.00,2500.00,-6250.00,2400.00,1.00,0.19,120000,120000
-24535,29076,-350.00,2950.00,-450.00,2850.00,0.21,0.19,120000,120000
-24694,29137,4000.00,3150.00,7950.00,3050.00,-1.00,0.19,120000,120000
-24786,29201,4000.00,3300.00,4600.00,3200.00,-1.00,0.19,120000,120000
-24650,29273,-4000.00,3700.00,-6800.00,3600.00,1.00,0.19,120000,120000
-24437,29346,-4000.00,3750.00,-10650.00,3650.00,1.00,0.19,120000,120000
-24488,29417,2650.00,3650.00,2550.00,3550.00,0.21,0.19,120000,120000
\ No newline at end of file
+LeftSetpoint,LeftVelocity,LeftError,RightSetpoint,RightVelocity,RightError,HeadingVelocityCorrection,HeadingSetpoint,Heading
+0.00,0.00,0.00,0.00,0.00,0.00,-98.20,86.00,87.89
+0.00,0.00,0.00,0.00,0.00,0.00,-98.20,86.00,87.89
+Changing encoder PID setpoint!
+-4200.00,0.00,-4200.00,-4200.00,0.00,-4200.00,-105.38,86.00,88.03
+-9450.00,-685.71,-8764.29,-9450.00,-771.43,-8678.57,0.00,86.00,86.68
+-13950.00,-3766.67,-10183.33,-13950.00,-4066.67,-9883.33,0.00,86.00,85.10
+-18900.00,-9787.88,-9112.12,-18900.00,-9848.48,-9051.52,707.72,86.00,72.39
+-24150.00,-15114.29,-9035.71,-24150.00,-15314.29,-8835.71,590.48,86.00,74.64
+-29100.00,-20333.33,-8766.67,-29100.00,-19484.85,-9615.15,590.48,86.00,74.64
+-30000.00,-24558.82,-5441.18,-30000.00,-23264.71,-6735.29,535.15,86.00,75.71
+-30000.00,-28636.36,-1363.64,-30000.00,-27242.42,-2757.58,497.01,86.00,76.44
+-30000.00,-31312.50,1312.50,-30000.00,-29937.50,-62.50,260.93,86.00,80.98
+-30000.00,-31870.97,1870.97,-30000.00,-30774.19,774.19,-204.30,86.00,89.93
+-30000.00,-31037.04,1037.04,-30000.00,-30444.44,444.44,-204.30,86.00,89.93
+-30000.00,-29903.23,-96.77,-30000.00,-29935.48,-64.52,-230.62,86.00,90.44
+-30000.00,-29612.90,-387.10,-30000.00,-29870.97,-129.03,-117.44,86.00,88.26
+-30000.00,-29781.25,-218.75,-30000.00,-29812.50,-187.50,0.00,86.00,86.76
+-30000.00,-30187.50,187.50,-30000.00,-29812.50,-187.50,0.00,86.00,86.76
+-30000.00,-29870.97,-129.03,-30000.00,-29645.16,-354.84,-103.95,86.00,88.00
+-30000.00,-29625.00,-375.00,-30000.00,-29468.75,-531.25,0.00,86.00,86.83
+-30000.00,-29838.71,-161.29,-30000.00,-29677.42,-322.58,0.00,86.00,86.48
+-30000.00,-29838.71,-161.29,-30000.00,-29677.42,-322.58,0.00,86.00,86.94
+-30000.00,-29843.75,-156.25,-30000.00,-29906.25,-93.75,0.00,86.00,86.94
+-30000.00,-30096.77,96.77,-30000.00,-29903.23,-96.77,-73.01,86.00,87.40
+-30000.00,-30290.32,290.32,-30000.00,-29806.45,-193.55,-101.54,86.00,87.95
+-30000.00,-30096.77,96.77,-30000.00,-29967.74,-32.26,-94.20,86.00,87.81
+-30000.00,-29774.19,-225.81,-30000.00,-29806.45,-193.55,-94.20,86.00,87.81
+-30000.00,-29687.50,-312.50,-30000.00,-29843.75,-156.25,-114.45,86.00,88.20
+-30000.00,-30093.75,93.75,-30000.00,-29843.75,-156.25,-60.25,86.00,87.16
+-30000.00,-30187.50,187.50,-30000.00,-29843.75,-156.25,0.00,86.00,86.71
+-30000.00,-30258.06,258.06,-30000.00,-29774.19,-225.81,0.00,86.00,86.93
+-30000.00,-29870.97,-129.03,-30000.00,-29709.68,-290.32,0.00,86.00,86.93
+-30000.00,-29843.75,-156.25,-30000.00,-29531.25,-468.75,0.00,86.00,86.99
+-30000.00,-29843.75,-156.25,-30000.00,-29718.75,-281.25,0.00,86.00,85.66
+-30000.00,-29812.50,-187.50,-30000.00,-29875.00,-125.00,0.00,86.00,86.33
+-30000.00,-30064.52,64.52,-30000.00,-29870.97,-129.03,-85.40,86.00,87.64
+-30000.00,-30193.55,193.55,-30000.00,-29903.23,-96.77,-85.40,86.00,87.64
+-30000.00,-30343.75,343.75,-30000.00,-29968.75,-31.25,-106.50,86.00,88.05
+-30000.00,-29838.71,-161.29,-30000.00,-29967.74,-32.26,-86.19,86.00,87.66
+-30000.00,-29781.25,-218.75,-30000.00,-29937.50,-62.50,-78.01,86.00,87.50
+-30000.00,-29781.25,-218.75,-30000.00,-29906.25,-93.75,-80.36,86.00,87.55
+-30000.00,-29906.25,-93.75,-30000.00,-29843.75,-156.25,-80.36,86.00,87.55
+-30000.00,-30218.75,218.75,-30000.00,-29687.50,-312.50,-75.29,86.00,87.45
+-30000.00,-30096.77,96.77,-30000.00,-29774.19,-225.81,0.00,86.00,86.30
+-30000.00,-29838.71,-161.29,-30000.00,-29677.42,-322.58,0.00,86.00,86.50
+-30000.00,-30032.26,32.26,-30000.00,-29483.87,-516.13,0.00,86.00,86.50
+-30000.00,-29935.48,-64.52,-30000.00,-29838.71,-161.29,0.00,86.00,86.94
+-30000.00,-29812.50,-187.50,-30000.00,-29781.25,-218.75,0.00,86.00,86.89
+-30000.00,-29903.23,-96.77,-30000.00,-29870.97,-129.03,0.00,86.00,86.63
+-30000.00,-30483.87,483.87,-30000.00,-29741.94,-258.06,-72.93,86.00,87.40
+-30000.00,-30258.06,258.06,-30000.00,-29967.74,-32.26,-72.93,86.00,87.40
+-30000.00,-29937.50,-62.50,-30000.00,-30187.50,187.50,-118.13,86.00,88.27
+-30000.00,-29656.25,-343.75,-30000.00,-29937.50,-62.50,-62.22,86.00,87.20
+-30000.00,-29935.48,-64.52,-30000.00,-30129.03,129.03,0.00,86.00,86.90
+-30000.00,-30225.81,225.81,-30000.00,-29741.94,-258.06,0.00,86.00,86.90
+-30000.00,-30375.00,375.00,-30000.00,-29750.00,-250.00,-85.50,86.00,87.64
+-30000.00,-30064.52,64.52,-30000.00,-29870.97,-129.03,-84.18,86.00,87.62
+-30000.00,-29774.19,-225.81,-30000.00,-29838.71,-161.29,0.00,86.00,86.46
+-30000.00,-29870.97,-129.03,-30000.00,-29741.94,-258.06,0.00,86.00,86.67
+-30000.00,-29906.25,-93.75,-30000.00,-29781.25,-218.75,0.00,86.00,86.67
+-30000.00,-29838.71,-161.29,-30000.00,-29709.68,-290.32,-55.93,86.00,87.08
+-30000.00,-30064.52,64.52,-30000.00,-29870.97,-129.03,-80.12,86.00,87.54
+-30000.00,-30290.32,290.32,-30000.00,-30064.52,64.52,-75.31,86.00,87.45
+-30000.00,-30193.55,193.55,-30000.00,-30064.52,64.52,-97.21,86.00,87.87
+-30000.00,-29709.68,-290.32,-30000.00,-30000.00,0.00,-97.21,86.00,87.87
+-30000.00,-29875.00,-125.00,-30000.00,-29843.75,-156.25,-97.89,86.00,87.88
+-30000.00,-29968.75,-31.25,-30000.00,-29968.75,-31.25,-82.81,86.00,87.59
+-30000.00,-30258.06,258.06,-30000.00,-29741.94,-258.06,0.00,86.00,86.57
+-30000.00,-30250.00,250.00,-30000.00,-29722.22,-277.78,-102.55,86.00,87.97
+-30000.00,-30064.52,64.52,-30000.00,-29870.97,-129.03,-102.55,86.00,87.97
+-30000.00,-29838.71,-161.29,-30000.00,-29870.97,-129.03,-61.23,86.00,87.18
+-30000.00,-29709.68,-290.32,-30000.00,-29838.71,-161.29,0.00,86.00,86.82
+-30000.00,-29806.45,-193.55,-30000.00,-29774.19,-225.81,0.00,86.00,86.30
+-30000.00,-30032.26,32.26,-30000.00,-29838.71,-161.29,0.00,86.00,86.30
+-30000.00,-30258.06,258.06,-30000.00,-29806.45,-193.55,-96.61,86.00,87.86
+-30000.00,-30387.10,387.10,-30000.00,-29903.23,-96.77,-80.30,86.00,87.54
+-30000.00,-30064.52,64.52,-30000.00,-30000.00,0.00,-125.15,86.00,88.41
+-30000.00,-29774.19,-225.81,-30000.00,-29935.48,-64.52,-55.63,86.00,87.07
+-30000.00,-29870.97,-129.03,-29195.00,-30129.03,934.03,-55.63,86.00,87.07
+-26400.00,-30000.00,3600.00,-24545.00,-29935.48,5390.48,-96.43,86.00,87.85
+-21750.00,-29645.16,7895.16,-19895.00,-28516.13,8621.13,-281.31,86.00,91.41
+-17100.00,-26000.00,8900.00,-15245.00,-24580.65,9335.65,-1253.50,86.00,110.11
+-12450.00,-19451.61,7001.61,-10595.00,-17741.94,7146.94,-1253.50,86.00,110.11
+-7950.00,-14233.33,6283.33,-6095.00,-12766.67,6671.67,-841.06,86.00,102.17
+-3450.00,-10700.00,7250.00,-1595.00,-9366.67,7771.67,-414.06,86.00,93.96
+0.00,-7166.67,7166.67,0.00,-6133.33,6133.33,-810.11,86.00,101.58
+0.00,-1068.97,1068.97,0.00,-137.93,137.93,-810.11,86.00,101.58
+0.00,2758.62,-2758.62,0.00,2000.00,-2000.00,-279.77,86.00,91.38
+0.00,2448.28,-2448.28,0.00,1448.28,-1448.28,147.08,86.00,83.17
+0.00,1862.07,-1862.07,0.00,310.34,-310.34,121.99,86.00,83.65
+0.00,1068.97,-1068.97,0.00,34.48,-34.48,121.99,86.00,83.65
+0.00,1214.29,-1214.29,0.00,0.00,0.00,-172.96,86.00,89.33
+0.00,310.34,-310.34,0.00,0.00,0.00,81.99,86.00,84.42
+0.00,-642.86,642.86,0.00,0.00,0.00,81.99,86.00,84.42
+0.00,-107.14,107.14,0.00,0.00,0.00,0.00,86.00,85.28
+0.00,250.00,-250.00,0.00,0.00,0.00,0.00,86.00,85.30
+0.00,-107.14,107.14,0.00,0.00,0.00,79.59,86.00,84.47
+0.00,0.00,0.00,0.00,0.00,0.00,79.59,86.00,84.47
+0.00,0.00,0.00,0.00,0.00,0.00,69.07,86.00,84.67
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.02
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.02
+0.00,0.00,0.00,0.00,0.00,0.00,71.06,86.00,84.63
+0.00,0.00,0.00,0.00,0.00,0.00,61.76,86.00,84.81
+0.00,0.00,0.00,0.00,0.00,0.00,61.76,86.00,84.81
+0.00,0.00,0.00,0.00,0.00,0.00,55.68,86.00,84.93
+0.00,0.00,0.00,0.00,0.00,0.00,57.12,86.00,84.90
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.02
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.02
+0.00,0.00,0.00,0.00,0.00,0.00,69.50,86.00,84.66
+0.00,0.00,0.00,0.00,0.00,0.00,68.31,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,68.31,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,71.05,86.00,84.63
+0.00,0.00,0.00,0.00,0.00,0.00,74.74,86.00,84.56
+0.00,0.00,0.00,0.00,0.00,0.00,74.74,86.00,84.56
+0.00,0.00,0.00,0.00,0.00,0.00,80.50,86.00,84.45
+0.00,0.00,0.00,0.00,0.00,0.00,59.94,86.00,84.85
+0.00,0.00,0.00,0.00,0.00,0.00,75.89,86.00,84.54
+0.00,0.00,0.00,0.00,0.00,0.00,75.89,86.00,84.54
+0.00,0.00,0.00,0.00,0.00,0.00,69.06,86.00,84.67
+0.00,0.00,0.00,0.00,0.00,0.00,68.52,86.00,84.68
+0.00,0.00,0.00,0.00,0.00,0.00,68.52,86.00,84.68
+0.00,0.00,0.00,0.00,0.00,0.00,68.92,86.00,84.67
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.15
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.15
+0.00,0.00,0.00,0.00,0.00,0.00,70.30,86.00,84.65
+0.00,0.00,0.00,0.00,0.00,0.00,78.96,86.00,84.48
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.06
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.06
+0.00,0.00,0.00,0.00,0.00,0.00,56.18,86.00,84.92
+0.00,0.00,0.00,0.00,0.00,0.00,64.23,86.00,84.76
+0.00,0.00,0.00,0.00,0.00,0.00,64.23,86.00,84.76
+0.00,0.00,0.00,0.00,0.00,0.00,56.41,86.00,84.92
+0.00,0.00,0.00,0.00,0.00,0.00,53.36,86.00,84.97
+0.00,0.00,0.00,0.00,0.00,0.00,53.36,86.00,84.97
+0.00,0.00,0.00,0.00,0.00,0.00,66.83,86.00,84.71
+0.00,0.00,0.00,0.00,0.00,0.00,68.37,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,76.06,86.00,84.54
+0.00,0.00,0.00,0.00,0.00,0.00,76.06,86.00,84.54
+0.00,0.00,0.00,0.00,0.00,0.00,71.78,86.00,84.62
+0.00,0.00,0.00,0.00,0.00,0.00,52.86,86.00,84.98
+0.00,0.00,0.00,0.00,0.00,0.00,52.86,86.00,84.98
+0.00,0.00,0.00,0.00,0.00,0.00,71.69,86.00,84.62
+0.00,0.00,0.00,0.00,0.00,0.00,73.86,86.00,84.58
+0.00,0.00,0.00,0.00,0.00,0.00,65.27,86.00,84.74
+0.00,0.00,0.00,0.00,0.00,0.00,65.27,86.00,84.74
+0.00,0.00,0.00,0.00,0.00,0.00,75.43,86.00,84.55
+0.00,0.00,0.00,0.00,0.00,0.00,85.19,86.00,84.36
+0.00,0.00,0.00,0.00,0.00,0.00,85.19,86.00,84.36
+0.00,0.00,0.00,0.00,0.00,0.00,68.13,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,93.52,86.00,84.20
+0.00,0.00,0.00,0.00,0.00,0.00,93.52,86.00,84.20
+0.00,0.00,0.00,0.00,0.00,0.00,79.34,86.00,84.47
+0.00,0.00,0.00,0.00,0.00,0.00,67.36,86.00,84.70
+0.00,0.00,0.00,0.00,0.00,0.00,70.72,86.00,84.64
+0.00,0.00,0.00,0.00,0.00,0.00,70.72,86.00,84.64
+0.00,0.00,0.00,0.00,0.00,0.00,73.75,86.00,84.58
+0.00,0.00,0.00,0.00,0.00,0.00,62.40,86.00,84.80
+0.00,0.00,0.00,0.00,0.00,0.00,62.40,86.00,84.80
+0.00,0.00,0.00,0.00,0.00,0.00,88.94,86.00,84.29
+0.00,0.00,0.00,0.00,0.00,0.00,74.48,86.00,84.57
+0.00,0.00,0.00,0.00,0.00,0.00,74.48,86.00,84.57
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.08
+0.00,0.00,0.00,0.00,0.00,0.00,53.59,86.00,84.97
+0.00,0.00,0.00,0.00,0.00,0.00,68.31,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,68.31,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,75.59,86.00,84.55
+0.00,0.00,0.00,0.00,0.00,0.00,73.07,86.00,84.60
+0.00,0.00,0.00,0.00,0.00,0.00,73.07,86.00,84.60
+0.00,0.00,0.00,0.00,0.00,0.00,62.58,86.00,84.80
+0.00,0.00,0.00,0.00,0.00,0.00,55.37,86.00,84.94
+0.00,0.00,0.00,0.00,0.00,0.00,55.37,86.00,84.94
+0.00,0.00,0.00,0.00,0.00,0.00,52.76,86.00,84.99
+0.00,0.00,0.00,0.00,0.00,0.00,60.35,86.00,84.84
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.12
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.12
+0.00,0.00,0.00,0.00,0.00,0.00,61.77,86.00,84.81
+0.00,0.00,0.00,0.00,0.00,0.00,64.03,86.00,84.77
+0.00,0.00,0.00,0.00,0.00,0.00,64.03,86.00,84.77
+0.00,0.00,0.00,0.00,0.00,0.00,80.78,86.00,84.45
+0.00,0.00,0.00,0.00,0.00,0.00,69.44,86.00,84.66
+0.00,0.00,0.00,0.00,0.00,0.00,69.44,86.00,84.66
+0.00,0.00,0.00,0.00,0.00,0.00,68.19,86.00,84.69
+0.00,0.00,0.00,0.00,0.00,0.00,54.27,86.00,84.96
+0.00,0.00,0.00,0.00,0.00,0.00,72.12,86.00,84.61
+0.00,0.00,0.00,0.00,0.00,0.00,72.12,86.00,84.61
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.13
+0.00,0.00,0.00,0.00,0.00,0.00,57.46,86.00,84.90
+0.00,0.00,0.00,0.00,0.00,0.00,57.46,86.00,84.90
+0.00,0.00,0.00,0.00,0.00,0.00,70.05,86.00,84.65
+0.00,0.00,0.00,0.00,0.00,0.00,58.23,86.00,84.88
+0.00,0.00,0.00,0.00,0.00,0.00,58.23,86.00,84.88
+0.00,0.00,0.00,0.00,0.00,0.00,58.08,86.00,84.88
+0.00,0.00,0.00,0.00,0.00,0.00,72.22,86.00,84.61
+0.00,0.00,0.00,0.00,0.00,0.00,65.26,86.00,84.75
+0.00,0.00,0.00,0.00,0.00,0.00,65.26,86.00,84.75
+0.00,0.00,0.00,0.00,0.00,0.00,71.35,86.00,84.63
+0.00,0.00,0.00,0.00,0.00,0.00,70.17,86.00,84.65
+0.00,0.00,0.00,0.00,0.00,0.00,70.17,86.00,84.65
+0.00,0.00,0.00,0.00,0.00,0.00,60.80,86.00,84.83
+0.00,0.00,0.00,0.00,0.00,0.00,56.56,86.00,84.91
+0.00,0.00,0.00,0.00,0.00,0.00,56.56,86.00,84.91
+0.00,0.00,0.00,0.00,0.00,0.00,66.97,86.00,84.71
+0.00,0.00,0.00,0.00,0.00,0.00,67.70,86.00,84.70
+0.00,0.00,0.00,0.00,0.00,0.00,59.34,86.00,84.86
+0.00,0.00,0.00,0.00,0.00,0.00,59.34,86.00,84.86
+0.00,0.00,0.00,0.00,0.00,0.00,0.00,86.00,85.01
+0.00,0.00,0.00,0.00,0.00,0.00,53.18,86.00,84.98
+Changing encoder PID setpoint!
+4200.00,0.00,4200.00,4200.00,0.00,4200.00,53.18,86.00,84.98
+8550.00,620.69,7929.31,8550.00,896.55,7653.45,77.58,86.00,84.51
+12900.00,4068.97,8831.03,12900.00,4241.38,8658.62,-547.49,86.00,96.53
+18150.00,9542.86,8607.14,18150.00,9400.00,8750.00,-914.44,86.00,103.59
+22950.00,15718.75,7231.25,22950.00,14218.75,8731.25,-554.10,86.00,96.66
+27750.00,20656.25,7093.75,27750.00,17750.00,10000.00,-554.10,86.00,96.66
+30000.00,24343.75,5656.25,30000.00,22468.75,7531.25,-500.50,86.00,95.63
+30000.00,27181.82,2818.18,30000.00,26909.09,3090.91,-451.72,86.00,94.69
+30000.00,29774.19,225.81,30000.00,30258.06,-258.06,-162.53,86.00,89.13
+30000.00,30870.97,-870.97,30000.00,31258.06,-1258.06,-162.53,86.00,89.13
+30000.00,30741.94,-741.94,30000.00,30548.39,-548.39,66.45,86.00,84.72
+30000.00,30322.58,-322.58,30000.00,30000.00,0.00,62.79,86.00,84.79
+30000.00,29967.74,32.26,30000.00,29870.97,129.03,0.00,86.00,86.59
+30000.00,29903.23,96.77,30000.00,29677.42,322.58,0.00,86.00,86.85
+30000.00,29733.33,266.67,30000.00,30066.67,-66.67,0.00,86.00,86.85
+30000.00,29766.67,233.33,30000.00,29900.00,100.00,0.00,86.00,86.79
+30000.00,29516.13,483.87,30000.00,29677.42,322.58,0.00,86.00,86.79
+30000.00,29666.67,333.33,30000.00,30000.00,0.00,-74.83,86.00,87.44
+30000.00,29967.74,32.26,30000.00,30096.77,-96.77,-74.83,86.00,87.44
+30000.00,30064.52,-64.52,30000.00,30000.00,0.00,0.00,86.00,86.28
+30000.00,29935.48,64.52,30000.00,29645.16,354.84,0.00,86.00,86.92
+30000.00,29866.67,133.33,30000.00,30166.67,-166.67,-75.22,86.00,87.45
+30000.00,29774.19,225.81,30000.00,30129.03,-129.03,-75.22,86.00,87.45
+30000.00,30096.77,-96.77,30000.00,29741.94,258.06,0.00,86.00,86.70
+30000.00,30100.00,-100.00,30000.00,29733.33,266.67,-71.61,86.00,87.38
+30000.00,29838.71,161.29,30000.00,30000.00,0.00,0.00,86.00,86.76
+30000.00,29774.19,225.81,30000.00,29935.48,64.52,0.00,86.00,86.95
+30000.00,29967.74,32.26,30000.00,29967.74,32.26,0.00,86.00,86.95
+30000.00,30096.77,-96.77,30000.00,29612.90,387.10,0.00,86.00,86.87
+30000.00,29677.42,322.58,30000.00,29967.74,32.26,0.00,86.00,86.40
+30000.00,29774.19,225.81,30000.00,29967.74,32.26,-60.21,86.00,87.16
+30000.00,29903.23,96.77,30000.00,29967.74,32.26,-60.21,86.00,87.16
+30000.00,30096.77,-96.77,30000.00,29741.94,258.06,0.00,86.00,86.65
+30000.00,30200.00,-200.00,30000.00,29966.67,33.33,0.00,86.00,86.55
+30000.00,30100.00,-100.00,30000.00,30100.00,-100.00,-72.65,86.00,87.40
+30000.00,29870.97,129.03,30000.00,30225.81,-225.81,-72.65,86.00,87.40
+30000.00,29838.71,161.29,30000.00,30096.77,-96.77,0.00,86.00,86.13
+30000.00,30096.77,-96.77,30000.00,29548.39,451.61,0.00,86.00,86.75
+30000.00,29935.48,64.52,30000.00,29677.42,322.58,0.00,86.00,86.70
+30000.00,29833.33,166.67,30000.00,29966.67,33.33,-68.54,86.00,87.32
+30000.00,29933.33,66.67,30000.00,30433.33,-433.33,-68.54,86.00,87.32
+30000.00,30193.55,-193.55,30000.00,29935.48,64.52,0.00,86.00,85.76
+30000.00,30032.26,-32.26,30000.00,29677.42,322.58,0.00,86.00,85.59
+30000.00,29741.94,258.06,30000.00,29870.97,129.03,0.00,86.00,86.25
+30000.00,29870.97,129.03,30000.00,30193.55,-193.55,0.00,86.00,86.25
+30000.00,30000.00,0.00,30000.00,29838.71,161.29,0.00,86.00,85.85
+30000.00,29966.67,33.33,30000.00,29900.00,100.00,0.00,86.00,86.72
+30000.00,30096.77,-96.77,30000.00,30193.55,-193.55,0.00,86.00,86.92
+30000.00,30129.03,-129.03,30000.00,29838.71,161.29,0.00,86.00,86.92
+30000.00,29933.33,66.67,30000.00,29666.67,333.33,0.00,86.00,86.24
+30000.00,29866.67,133.33,30000.00,30133.33,-133.33,-79.67,86.00,87.53
+30000.00,29806.45,193.55,30000.00,30064.52,-64.52,0.00,86.00,86.75
+30000.00,29833.33,166.67,30000.00,30066.67,-66.67,0.00,86.00,86.75
+30000.00,30066.67,-66.67,30000.00,29833.33,166.67,0.00,86.00,85.88
+30000.00,30064.52,-64.52,30000.00,29774.19,225.81,0.00,86.00,86.01
+30000.00,30000.00,0.00,30000.00,30096.77,-96.77,0.00,86.00,86.27
+30000.00,29766.67,233.33,30000.00,30333.33,-333.33,0.00,86.00,86.12
+30000.00,29806.45,193.55,30000.00,30129.03,-129.03,0.00,86.00,86.12
+30000.00,30000.00,0.00,30000.00,30233.33,-233.33,0.00,86.00,85.53
+30000.00,29838.71,161.29,30000.00,30096.77,-96.77,0.00,86.00,85.15
+30000.00,30033.33,-33.33,30000.00,29866.67,133.33,-60.84,86.00,87.17
+30000.00,30300.00,-300.00,30000.00,29666.67,333.33,-60.84,86.00,87.17
+30000.00,30066.67,-66.67,30000.00,29733.33,266.67,0.00,86.00,86.86
+30000.00,30100.00,-100.00,30000.00,29766.67,233.33,-95.73,86.00,87.84
+30000.00,29709.68,290.32,30000.00,29774.19,225.81,0.00,86.00,86.72
+30000.00,30066.67,-66.67,30000.00,30200.00,-200.00,0.00,86.00,86.72
+30000.00,30129.03,-129.03,30000.00,30193.55,-193.55,0.00,86.00,86.70
+30000.00,30193.55,-193.55,30000.00,29935.48,64.52,0.00,86.00,86.98
+30000.00,29916.67,83.33,30000.00,29777.78,222.22,0.00,86.00,85.97
+30000.00,29800.00,200.00,30000.00,29966.67,33.33,0.00,86.00,86.37
+30000.00,29838.71,161.29,30000.00,30161.29,-161.29,0.00,86.00,86.37
+30000.00,30100.00,-100.00,30000.00,30066.67,-66.67,0.00,86.00,86.20
+30000.00,29900.00,100.00,30000.00,30000.00,0.00,0.00,86.00,86.16
+30000.00,30133.33,-133.33,30000.00,30266.67,-266.67,0.00,86.00,86.57
+30000.00,30033.33,-33.33,30000.00,30066.67,-66.67,0.00,86.00,86.57
+30000.00,29966.67,33.33,30000.00,29900.00,100.00,0.00,86.00,86.04
+30000.00,30000.00,0.00,30000.00,29838.71,161.29,0.00,86.00,86.76
+27860.00,29900.00,-2040.00,26680.00,29733.33,-3053.33,-79.95,86.00,87.54
+23360.00,29566.67,-6206.67,22180.00,29500.00,-7320.00,-79.95,86.00,87.54
+18710.00,27903.23,-9193.23,17530.00,26741.94,-9211.94,150.63,86.00,83.10
+14210.00,23400.00,-9190.00,13030.00,19433.33,-6403.33,387.39,86.00,78.55
+9710.00,17733.33,-8023.33,8530.00,14033.33,-5503.33,256.88,86.00,81.06
+5210.00,13066.67,-7856.67,4030.00,10300.00,-6270.00,256.88,86.00,81.06
+710.00,9066.67,-8356.67,0.00,7100.00,-7100.00,0.00,86.00,85.14
+0.00,2482.76,-2482.76,0.00,2275.86,-2275.86,571.02,86.00,75.02
+0.00,-1925.93,1925.93,0.00,-962.96,962.96,-526.54,86.00,96.13
+0.00,-1655.17,1655.17,0.00,-137.93,137.93,-526.54,86.00,96.13
+0.00,-551.72,551.72,0.00,0.00,0.00,-517.20,86.00,95.95
+0.00,-34.48,34.48,0.00,0.00,0.00,-667.26,86.00,98.83
+0.00,0.00,0.00,0.00,0.00,0.00,-667.26,86.00,98.83
+0.00,0.00,0.00,0.00,0.00,0.00,-654.14,86.00,98.58
+0.00,0.00,0.00,0.00,0.00,0.00,-689.17,86.00,99.25
+0.00,0.00,0.00,0.00,0.00,0.00,-675.88,86.00,99.00
+0.00,0.00,0.00,0.00,0.00,0.00,-675.88,86.00,99.00
+0.00,0.00,0.00,0.00,0.00,0.00,-662.41,86.00,98.74
+0.00,0.00,0.00,0.00,0.00,0.00,-670.17,86.00,98.89
+0.00,0.00,0.00,0.00,0.00,0.00,-670.17,86.00,98.89
+0.00,0.00,0.00,0.00,0.00,0.00,-670.09,86.00,98.89
+0.00,0.00,0.00,0.00,0.00,0.00,-649.70,86.00,98.49
+0.00,0.00,0.00,0.00,0.00,0.00,-649.70,86.00,98.49
+0.00,0.00,0.00,0.00,0.00,0.00,-658.46,86.00,98.66
+0.00,0.00,0.00,0.00,0.00,0.00,-684.96,86.00,99.17
+0.00,0.00,0.00,0.00,0.00,0.00,-701.31,86.00,99.49
+0.00,0.00,0.00,0.00,0.00,0.00,-701.31,86.00,99.49
+0.00,0.00,0.00,0.00,0.00,0.00,-661.94,86.00,98.73
+0.00,0.00,0.00,0.00,0.00,0.00,-660.23,86.00,98.70
+0.00,0.00,0.00,0.00,0.00,0.00,-660.23,86.00,98.70
+0.00,0.00,0.00,0.00,0.00,0.00,-697.65,86.00,99.42
+0.00,0.00,0.00,0.00,0.00,0.00,-662.31,86.00,98.74
+0.00,0.00,0.00,0.00,0.00,0.00,-662.31,86.00,98.74
+0.00,0.00,0.00,0.00,0.00,0.00,-680.61,86.00,99.09
+0.00,0.00,0.00,0.00,0.00,0.00,-667.61,86.00,98.84
+0.00,0.00,0.00,0.00,0.00,0.00,-656.55,86.00,98.63
+0.00,0.00,0.00,0.00,0.00,0.00,-656.55,86.00,98.63
+0.00,0.00,0.00,0.00,0.00,0.00,-667.36,86.00,98.83
+0.00,0.00,0.00,0.00,0.00,0.00,-652.73,86.00,98.55
+0.00,0.00,0.00,0.00,0.00,0.00,-652.73,86.00,98.55
+0.00,0.00,0.00,0.00,0.00,0.00,-676.32,86.00,99.01
+0.00,0.00,0.00,0.00,0.00,0.00,-665.64,86.00,98.80
+0.00,0.00,0.00,0.00,0.00,0.00,-665.64,86.00,98.80
+0.00,0.00,0.00,0.00,0.00,0.00,-641.47,86.00,98.34
+0.00,0.00,0.00,0.00,0.00,0.00,-666.99,86.00,98.83
+0.00,0.00,0.00,0.00,0.00,0.00,-669.35,86.00,98.87
+0.00,0.00,0.00,0.00,0.00,0.00,-669.35,86.00,98.87
+0.00,0.00,0.00,0.00,0.00,0.00,-664.70,86.00,98.78
+0.00,0.00,0.00,0.00,0.00,0.00,-665.24,86.00,98.79
+0.00,0.00,0.00,0.00,0.00,0.00,-665.24,86.00,98.79
+0.00,0.00,0.00,0.00,0.00,0.00,-662.90,86.00,98.75
+0.00,0.00,0.00,0.00,0.00,0.00,-635.90,86.00,98.23
+0.00,0.00,0.00,0.00,0.00,0.00,-635.90,86.00,98.23
+0.00,0.00,0.00,0.00,0.00,0.00,-629.01,86.00,98.10
+0.00,0.00,0.00,0.00,0.00,0.00,-616.29,86.00,97.85
+0.00,0.00,0.00,0.00,0.00,0.00,-587.72,86.00,97.30
+0.00,0.00,0.00,0.00,0.00,0.00,-587.72,86.00,97.30
+0.00,0.00,0.00,0.00,0.00,0.00,-633.14,86.00,98.18
+0.00,0.00,0.00,0.00,0.00,0.00,-664.02,86.00,98.77
+0.00,0.00,0.00,0.00,0.00,0.00,-664.02,86.00,98.77
+0.00,0.00,0.00,0.00,0.00,0.00,-643.33,86.00,98.37
+0.00,0.00,0.00,0.00,0.00,0.00,-665.67,86.00,98.80
+0.00,0.00,0.00,0.00,0.00,0.00,-643.49,86.00,98.38
+0.00,0.00,0.00,0.00,0.00,0.00,-643.49,86.00,98.38
+0.00,0.00,0.00,0.00,0.00,0.00,-635.15,86.00,98.21
+0.00,0.00,0.00,0.00,0.00,0.00,-628.68,86.00,98.09
+0.00,0.00,0.00,0.00,0.00,0.00,-628.68,86.00,98.09
+0.00,0.00,0.00,0.00,0.00,0.00,-640.34,86.00,98.31
+0.00,0.00,0.00,0.00,0.00,0.00,-656.65,86.00,98.63
+0.00,0.00,0.00,0.00,0.00,0.00,-656.65,86.00,98.63
+0.00,0.00,0.00,0.00,0.00,0.00,-607.44,86.00,97.68
+0.00,0.00,0.00,0.00,0.00,0.00,-602.86,86.00,97.59
+0.00,0.00,0.00,0.00,0.00,0.00,-648.08,86.00,98.46
+0.00,0.00,0.00,0.00,0.00,0.00,-648.08,86.00,98.46
+0.00,0.00,0.00,0.00,0.00,0.00,-624.76,86.00,98.01
+0.00,0.00,0.00,0.00,0.00,0.00,-642.59,86.00,98.36
+0.00,0.00,0.00,0.00,0.00,0.00,-642.59,86.00,98.36
+0.00,0.00,0.00,0.00,0.00,0.00,-641.05,86.00,98.33
+0.00,0.00,0.00,0.00,0.00,0.00,-658.02,86.00,98.65
+0.00,0.00,0.00,0.00,0.00,0.00,-658.02,86.00,98.65
+0.00,0.00,0.00,0.00,0.00,0.00,-632.97,86.00,98.17
+0.00,0.00,0.00,0.00,0.00,0.00,-608.01,86.00,97.69
+0.00,0.00,0.00,0.00,0.00,0.00,-653.31,86.00,98.56
+0.00,0.00,0.00,0.00,0.00,0.00,-653.31,86.00,98.56
+0.00,0.00,0.00,0.00,0.00,0.00,-646.50,86.00,98.43
+0.00,0.00,0.00,0.00,0.00,0.00,-622.67,86.00,97.97
+0.00,0.00,0.00,0.00,0.00,0.00,-622.67,86.00,97.97
+0.00,0.00,0.00,0.00,0.00,0.00,-614.63,86.00,97.82
+0.00,0.00,0.00,0.00,0.00,0.00,-605.76,86.00,97.65
+0.00,0.00,0.00,0.00,0.00,0.00,-605.76,86.00,97.65
+0.00,0.00,0.00,0.00,0.00,0.00,-616.21,86.00,97.85
+0.00,0.00,0.00,0.00,0.00,0.00,-613.68,86.00,97.80
+0.00,0.00,0.00,0.00,0.00,0.00,-635.73,86.00,98.23
+0.00,0.00,0.00,0.00,0.00,0.00,-635.73,86.00,98.23
+0.00,0.00,0.00,0.00,0.00,0.00,-621.09,86.00,97.94
+0.00,0.00,0.00,0.00,0.00,0.00,-637.73,86.00,98.26
+0.00,0.00,0.00,0.00,0.00,0.00,-637.73,86.00,98.26
+0.00,0.00,0.00,0.00,0.00,0.00,-641.94,86.00,98.35
+0.00,0.00,0.00,0.00,0.00,0.00,-649.27,86.00,98.49
+0.00,0.00,0.00,0.00,0.00,0.00,-649.27,86.00,98.49
+0.00,0.00,0.00,0.00,0.00,0.00,-623.67,86.00,97.99
+0.00,0.00,0.00,0.00,0.00,0.00,-629.29,86.00,98.10
+0.00,0.00,0.00,0.00,0.00,0.00,-639.36,86.00,98.30
+0.00,0.00,0.00,0.00,0.00,0.00,-639.36,86.00,98.30
+0.00,0.00,0.00,0.00,0.00,0.00,-618.36,86.00,97.89
+0.00,0.00,0.00,0.00,0.00,0.00,-636.39,86.00,98.24
+0.00,0.00,0.00,0.00,0.00,0.00,-636.39,86.00,98.24
+0.00,0.00,0.00,0.00,0.00,0.00,-627.53,86.00,98.07
+0.00,0.00,0.00,0.00,0.00,0.00,-613.00,86.00,97.79
+0.00,0.00,0.00,0.00,0.00,0.00,-613.00,86.00,97.79
+0.00,0.00,0.00,0.00,0.00,0.00,-628.23,86.00,98.08
+0.00,0.00,0.00,0.00,0.00,0.00,-608.60,86.00,97.70
+0.00,0.00,0.00,0.00,0.00,0.00,-610.41,86.00,97.74
+0.00,0.00,0.00,0.00,0.00,0.00,-610.41,86.00,97.74
+0.00,0.00,0.00,0.00,0.00,0.00,-627.82,86.00,98.07
+0.00,0.00,0.00,0.00,0.00,0.00,-608.53,86.00,97.70
+0.00,0.00,0.00,0.00,0.00,0.00,-608.53,86.00,97.70
+0.00,0.00,0.00,0.00,0.00,0.00,-624.78,86.00,98.02
+0.00,0.00,0.00,0.00,0.00,0.00,-620.07,86.00,97.92
+0.00,0.00,0.00,0.00,0.00,0.00,-620.07,86.00,97.92
+0.00,0.00,0.00,0.00,0.00,0.00,-616.00,86.00,97.85
+0.00,0.00,0.00,0.00,0.00,0.00,-619.39,86.00,97.91
+0.00,0.00,0.00,0.00,0.00,0.00,-653.83,86.00,98.57
+0.00,0.00,0.00,0.00,0.00,0.00,-653.83,86.00,98.57
+0.00,0.00,0.00,0.00,0.00,0.00,-640.59,86.00,98.32
+0.00,0.00,0.00,0.00,0.00,0.00,-657.35,86.00,98.64
+0.00,0.00,0.00,0.00,0.00,0.00,-657.35,86.00,98.64
+0.00,0.00,0.00,0.00,0.00,0.00,-651.52,86.00,98.53
+0.00,0.00,0.00,0.00,0.00,0.00,-668.59,86.00,98.86
+Changing encoder PID setpoint!
+-4200.00,0.00,-4200.00,-4200.00,0.00,-4200.00,-668.59,86.00,98.86
+-8550.00,-620.69,-7929.31,-8550.00,-965.52,-7584.48,-680.33,86.00,99.08
+-12900.00,-3551.72,-9348.28,-12900.00,-4724.14,-8175.86,311.86,86.00,80.00
+-17700.00,-8468.75,-9231.25,-17700.00,-10281.25,-7418.75,678.53,86.00,72.95
+-23400.00,-14657.89,-8742.11,-23400.00,-14710.53,-8689.47,650.45,86.00,73.49
+-28350.00,-20454.55,-7895.45,-28350.00,-18484.85,-9865.15,650.45,86.00,73.49
+-30000.00,-24060.61,-5939.39,-30000.00,-22545.45,-7454.55,632.53,86.00,73.84
+-30000.00,-27411.76,-2588.24,-30000.00,-26882.35,-3117.65,539.60,86.00,75.62
+-30000.00,-31125.00,1125.00,-30000.00,-29687.50,-312.50,402.54,86.00,78.26
+-30000.00,-32354.84,2354.84,-30000.00,-30580.65,580.65,-52.56,86.00,87.01
+-30000.00,-31516.13,1516.13,-30000.00,-30258.06,258.06,-52.56,86.00,87.01
+-30000.00,-30032.26,32.26,-30000.00,-30096.77,96.77,-203.04,86.00,89.90
+-30000.00,-29580.65,-419.35,-30000.00,-30000.00,0.00,-128.58,86.00,88.47
+-30000.00,-29781.25,-218.75,-30000.00,-29625.00,-375.00,0.00,86.00,86.43
+-30000.00,-30214.29,214.29,-30000.00,-29642.86,-357.14,0.00,86.00,86.63
+-30000.00,-30064.52,64.52,-30000.00,-29741.94,-258.06,0.00,86.00,86.63
+-30000.00,-29548.39,-451.61,-30000.00,-29580.65,-419.35,-62.06,86.00,87.19
+-30000.00,-29645.16,-354.84,-30000.00,-29645.16,-354.84,0.00,86.00,86.10
+-30000.00,-29781.25,-218.75,-30000.00,-29875.00,-125.00,0.00,86.00,86.28
+-30000.00,-29937.50,-62.50,-30000.00,-29812.50,-187.50,0.00,86.00,86.28
+-30000.00,-30250.00,250.00,-30000.00,-29687.50,-312.50,-85.88,86.00,87.65
+-30000.00,-30437.50,437.50,-30000.00,-29875.00,-125.00,-81.94,86.00,87.58
+-30000.00,-30031.25,31.25,-30000.00,-29875.00,-125.00,-61.65,86.00,87.19
+-30000.00,-29687.50,-312.50,-30000.00,-29937.50,-62.50,0.00,86.00,86.52
+-30000.00,-29718.75,-281.25,-30000.00,-29875.00,-125.00,0.00,86.00,86.52
+-30000.00,-29968.75,-31.25,-30000.00,-29843.75,-156.25,-57.26,86.00,87.10
+-30000.00,-30312.50,312.50,-30000.00,-29718.75,-281.25,-66.08,86.00,87.27
+-30000.00,-30281.25,281.25,-30000.00,-29687.50,-312.50,0.00,86.00,86.20
+-30000.00,-30000.00,0.00,-30000.00,-29709.68,-290.32,0.00,86.00,86.95
+-30000.00,-29733.33,-266.67,-30000.00,-29500.00,-500.00,0.00,86.00,86.95
+-30000.00,-29935.48,-64.52,-30000.00,-29677.42,-322.58,0.00,86.00,86.84
+-30000.00,-29781.25,-218.75,-30000.00,-29750.00,-250.00,0.00,86.00,85.77
+-30000.00,-30161.29,161.29,-30000.00,-29870.97,-129.03,0.00,86.00,86.29
+-30000.00,-30312.50,312.50,-30000.00,-29937.50,-62.50,-82.61,86.00,87.59
+-30000.00,-30225.81,225.81,-30000.00,-29935.48,-64.52,-82.61,86.00,87.59
+-30000.00,-29812.50,-187.50,-30000.00,-29875.00,-125.00,-115.43,86.00,88.22
+-30000.00,-29625.00,-375.00,-30000.00,-29781.25,-218.75,-66.34,86.00,87.28
+-30000.00,-29903.23,-96.77,-30000.00,-29903.23,-96.77,0.00,86.00,86.84
+-30000.00,-30032.26,32.26,-30000.00,-29935.48,-64.52,0.00,86.00,86.84
+-30000.00,-30129.03,129.03,-30000.00,-29903.23,-96.77,-97.97,86.00,87.88
+-30000.00,-30064.52,64.52,-30000.00,-29838.71,-161.29,-109.34,86.00,88.10
+-30000.00,-29774.19,-225.81,-30000.00,-29645.16,-354.84,0.00,86.00,86.72
+-30000.00,-29750.00,-250.00,-30000.00,-29718.75,-281.25,0.00,86.00,86.53
+-30000.00,-29781.25,-218.75,-30000.00,-29750.00,-250.00,0.00,86.00,86.53
+-30000.00,-29903.23,-96.77,-30000.00,-29677.42,-322.58,-58.20,86.00,87.12
+-30000.00,-30031.25,31.25,-30000.00,-29937.50,-62.50,0.00,86.00,86.53
+-30000.00,-30451.61,451.61,-30000.00,-29903.23,-96.77,-52.15,86.00,87.00
+-30000.00,-30218.75,218.75,-30000.00,-29968.75,-31.25,-88.33,86.00,87.70
+-30000.00,-29774.19,-225.81,-30000.00,-29870.97,-129.03,-88.33,86.00,87.70
+-30000.00,-29781.25,-218.75,-30000.00,-30093.75,93.75,-100.27,86.00,87.93
+-30000.00,-30096.77,96.77,-30000.00,-30161.29,161.29,0.00,86.00,86.90
+-30000.00,-30322.58,322.58,-30000.00,-30032.26,32.26,-87.90,86.00,87.69
+-30000.00,-30225.81,225.81,-30000.00,-29709.68,-290.32,-87.90,86.00,87.69
+-30000.00,-30032.26,32.26,-30000.00,-29645.16,-354.84,-92.55,86.00,87.78
+-30000.00,-29838.71,-161.29,-30000.00,-29774.19,-225.81,0.00,86.00,85.94
+-30000.00,-29774.19,-225.81,-30000.00,-29709.68,-290.32,0.00,86.00,86.42
+-30000.00,-29741.94,-258.06,-30000.00,-29806.45,-193.55,0.00,86.00,86.80
+-30000.00,-29937.50,-62.50,-30000.00,-29843.75,-156.25,0.00,86.00,86.80
+-30000.00,-30290.32,290.32,-30000.00,-29967.74,-32.26,-86.64,86.00,87.67
+-30000.00,-30451.61,451.61,-30000.00,-30096.77,96.77,-58.90,86.00,87.13
+-30000.00,-30156.25,156.25,-30000.00,-30187.50,187.50,-98.06,86.00,87.89
+-30000.00,-29838.71,-161.29,-30000.00,-30129.03,129.03,-98.06,86.00,87.89
+-30000.00,-29677.42,-322.58,-30000.00,-30000.00,0.00,-92.49,86.00,87.78
+-30000.00,-29903.23,-96.77,-30000.00,-30096.77,96.77,-90.75,86.00,87.75
+-30000.00,-30156.25,156.25,-30000.00,-29968.75,-31.25,-82.92,86.00,87.59
+-30000.00,-30281.25,281.25,-30000.00,-29718.75,-281.25,0.00,86.00,86.88
+-30000.00,-29967.74,-32.26,-30000.00,-29709.68,-290.32,0.00,86.00,86.88
+-30000.00,-29677.42,-322.58,-30000.00,-29677.42,-322.58,-59.75,86.00,87.15
+-30000.00,-29783.78,-216.22,-30000.00,-29729.73,-270.27,0.00,86.00,86.92
+-30000.00,-29741.94,-258.06,-30000.00,-29548.39,-451.61,0.00,86.00,86.38
+-30000.00,-30032.26,32.26,-30000.00,-29935.48,-64.52,0.00,86.00,86.26
+-30000.00,-30354.84,354.84,-30000.00,-29838.71,-161.29,0.00,86.00,86.26
+-30000.00,-30354.84,354.84,-30000.00,-29935.48,-64.52,-118.88,86.00,88.29
+-30000.00,-29935.48,-64.52,-30000.00,-30032.26,32.26,-78.05,86.00,87.50
+-30000.00,-29677.42,-322.58,-30000.00,-29935.48,-64.52,0.00,86.00,86.51
+-30000.00,-29843.75,-156.25,-28740.00,-30031.25,1291.25,0.00,86.00,85.87
+-25490.00,-30290.32,4800.32,-24090.00,-29903.23,5813.23,0.00,86.00,85.87
+-20840.00,-29645.16,8805.16,-19440.00,-28290.32,8850.32,-172.81,86.00,89.32
+-16190.00,-25612.90,9422.90,-14790.00,-24354.84,9564.84,-708.80,86.00,99.63
+-11540.00,-19064.52,7524.52,-10140.00,-17451.61,7311.61,-968.82,86.00,104.63
+-7040.00,-14066.67,7026.67,-5640.00,-12533.33,6893.33,-968.82,86.00,104.63
+-2540.00,-9766.67,7226.67,-1140.00,-9266.67,8126.67,-472.80,86.00,95.09
+0.00,-3466.67,3466.67,0.00,-5900.00,5900.00,-453.75,86.00,94.73
+0.00,896.55,-896.55,0.00,275.86,-275.86,-422.63,86.00,94.13
+0.00,1586.21,-1586.21,0.00,2448.28,-2448.28,-422.63,86.00,94.13
+0.00,862.07,-862.07,0.00,1758.62,-1758.62,284.75,86.00,80.52
+0.00,-379.31,379.31,0.00,1275.86,-1275.86,266.82,86.00,80.87
+0.00,-758.62,758.62,0.00,34.48,-34.48,220.53,86.00,81.76
+0.00,310.34,-310.34,0.00,-551.72,551.72,220.53,86.00,81.76
+0.00,-35.71,35.71,0.00,-285.71,285.71,282.59,86.00,80.57
+0.00,-34.48,34.48,0.00,137.93,-137.93,252.72,86.00,81.14
+0.00,0.00,0.00,0.00,0.00,0.00,252.72,86.00,81.14
+0.00,0.00,0.00,0.00,0.00,0.00,277.73,86.00,80.66
+0.00,0.00,0.00,0.00,0.00,0.00,294.53,86.00,80.34
+0.00,0.00,0.00,0.00,0.00,0.00,294.53,86.00,80.34
+0.00,0.00,0.00,0.00,0.00,0.00,294.29,86.00,80.34
+0.00,0.00,0.00,0.00,0.00,0.00,292.15,86.00,80.38
+0.00,0.00,0.00,0.00,0.00,0.00,277.82,86.00,80.66
+0.00,0.00,0.00,0.00,0.00,0.00,277.82,86.00,80.66
+0.00,0.00,0.00,0.00,0.00,0.00,302.92,86.00,80.17
+0.00,0.00,0.00,0.00,0.00,0.00,306.66,86.00,80.10
+0.00,0.00,0.00,0.00,0.00,0.00,306.66,86.00,80.10
+0.00,0.00,0.00,0.00,0.00,0.00,308.58,86.00,80.07
+0.00,0.00,0.00,0.00,0.00,0.00,296.20,86.00,80.30
+0.00,0.00,0.00,0.00,0.00,0.00,325.04,86.00,79.75
+0.00,0.00,0.00,0.00,0.00,0.00,325.04,86.00,79.75
+0.00,0.00,0.00,0.00,0.00,0.00,311.52,86.00,80.01
+0.00,0.00,0.00,0.00,0.00,0.00,319.25,86.00,79.86
+0.00,0.00,0.00,0.00,0.00,0.00,319.25,86.00,79.86
+0.00,0.00,0.00,0.00,0.00,0.00,325.43,86.00,79.74
+0.00,0.00,0.00,0.00,0.00,0.00,334.53,86.00,79.57
+0.00,0.00,0.00,0.00,0.00,0.00,334.53,86.00,79.57
+0.00,0.00,0.00,0.00,0.00,0.00,309.20,86.00,80.05
+0.00,0.00,0.00,0.00,0.00,0.00,313.17,86.00,79.98
+0.00,0.00,0.00,0.00,0.00,0.00,310.64,86.00,80.03
+0.00,0.00,0.00,0.00,0.00,0.00,310.64,86.00,80.03
+0.00,0.00,0.00,0.00,0.00,0.00,312.86,86.00,79.98
+0.00,0.00,0.00,0.00,0.00,0.00,297.14,86.00,80.29
+0.00,0.00,0.00,0.00,0.00,0.00,297.14,86.00,80.29
+0.00,0.00,0.00,0.00,0.00,0.00,336.51,86.00,79.53
+0.00,0.00,0.00,0.00,0.00,0.00,338.43,86.00,79.49
+0.00,0.00,0.00,0.00,0.00,0.00,338.43,86.00,79.49
+0.00,0.00,0.00,0.00,0.00,0.00,353.28,86.00,79.21
+0.00,0.00,0.00,0.00,0.00,0.00,331.10,86.00,79.63
+0.00,0.00,0.00,0.00,0.00,0.00,331.10,86.00,79.63
+0.00,0.00,0.00,0.00,0.00,0.00,314.31,86.00,79.96
+0.00,0.00,0.00,0.00,0.00,0.00,320.74,86.00,79.83
+0.00,0.00,0.00,0.00,0.00,0.00,334.10,86.00,79.58
+0.00,0.00,0.00,0.00,0.00,0.00,334.10,86.00,79.58
+0.00,0.00,0.00,0.00,0.00,0.00,316.46,86.00,79.91
+0.00,0.00,0.00,0.00,0.00,0.00,310.70,86.00,80.03
+0.00,0.00,0.00,0.00,0.00,0.00,310.70,86.00,80.03
+0.00,0.00,0.00,0.00,0.00,0.00,308.98,86.00,80.06
+0.00,0.00,0.00,0.00,0.00,0.00,304.29,86.00,80.15
+0.00,0.00,0.00,0.00,0.00,0.00,304.29,86.00,80.15
+0.00,0.00,0.00,0.00,0.00,0.00,313.84,86.00,79.96
+0.00,0.00,0.00,0.00,0.00,0.00,293.98,86.00,80.35
+0.00,0.00,0.00,0.00,0.00,0.00,314.06,86.00,79.96
+0.00,0.00,0.00,0.00,0.00,0.00,314.06,86.00,79.96
+0.00,0.00,0.00,0.00,0.00,0.00,304.17,86.00,80.15
+0.00,0.00,0.00,0.00,0.00,0.00,287.63,86.00,80.47
+0.00,0.00,0.00,0.00,0.00,0.00,287.63,86.00,80.47
+0.00,0.00,0.00,0.00,0.00,0.00,308.40,86.00,80.07
+0.00,0.00,0.00,0.00,0.00,0.00,307.85,86.00,80.08
+0.00,0.00,0.00,0.00,0.00,0.00,299.37,86.00,80.24
+0.00,0.00,0.00,0.00,0.00,0.00,299.37,86.00,80.24
+0.00,0.00,0.00,0.00,0.00,0.00,302.74,86.00,80.18
+0.00,0.00,0.00,0.00,0.00,0.00,322.91,86.00,79.79
+0.00,0.00,0.00,0.00,0.00,0.00,322.91,86.00,79.79
+0.00,0.00,0.00,0.00,0.00,0.00,309.56,86.00,80.05
+0.00,0.00,0.00,0.00,0.00,0.00,290.33,86.00,80.42
+0.00,0.00,0.00,0.00,0.00,0.00,290.33,86.00,80.42
+0.00,0.00,0.00,0.00,0.00,0.00,287.52,86.00,80.47
+0.00,0.00,0.00,0.00,0.00,0.00,318.42,86.00,79.88
+0.00,0.00,0.00,0.00,0.00,0.00,309.59,86.00,80.05
+0.00,0.00,0.00,0.00,0.00,0.00,309.59,86.00,80.05
+0.00,0.00,0.00,0.00,0.00,0.00,302.46,86.00,80.18
+0.00,0.00,0.00,0.00,0.00,0.00,280.42,86.00,80.61
+0.00,0.00,0.00,0.00,0.00,0.00,280.42,86.00,80.61
+0.00,0.00,0.00,0.00,0.00,0.00,293.67,86.00,80.35
+0.00,0.00,0.00,0.00,0.00,0.00,319.96,86.00,79.85
+0.00,0.00,0.00,0.00,0.00,0.00,319.96,86.00,79.85
+0.00,0.00,0.00,0.00,0.00,0.00,308.39,86.00,80.07
+0.00,0.00,0.00,0.00,0.00,0.00,305.32,86.00,80.13
+0.00,0.00,0.00,0.00,0.00,0.00,304.54,86.00,80.14
+0.00,0.00,0.00,0.00,0.00,0.00,304.54,86.00,80.14
+0.00,0.00,0.00,0.00,0.00,0.00,322.69,86.00,79.79
+0.00,0.00,0.00,0.00,0.00,0.00,297.32,86.00,80.28
+0.00,0.00,0.00,0.00,0.00,0.00,297.32,86.00,80.28
+0.00,0.00,0.00,0.00,0.00,0.00,299.95,86.00,80.23
+0.00,0.00,0.00,0.00,0.00,0.00,289.47,86.00,80.43
+0.00,0.00,0.00,0.00,0.00,0.00,289.47,86.00,80.43
+0.00,0.00,0.00,0.00,0.00,0.00,292.39,86.00,80.38
+0.00,0.00,0.00,0.00,0.00,0.00,291.22,86.00,80.40
+0.00,0.00,0.00,0.00,0.00,0.00,308.42,86.00,80.07
+0.00,0.00,0.00,0.00,0.00,0.00,308.42,86.00,80.07
+0.00,0.00,0.00,0.00,0.00,0.00,296.98,86.00,80.29
+0.00,0.00,0.00,0.00,0.00,0.00,299.60,86.00,80.24
+0.00,0.00,0.00,0.00,0.00,0.00,299.60,86.00,80.24
+0.00,0.00,0.00,0.00,0.00,0.00,303.34,86.00,80.17
+0.00,0.00,0.00,0.00,0.00,0.00,291.07,86.00,80.40
+0.00,0.00,0.00,0.00,0.00,0.00,291.07,86.00,80.40
+0.00,0.00,0.00,0.00,0.00,0.00,297.31,86.00,80.28
+0.00,0.00,0.00,0.00,0.00,0.00,270.28,86.00,80.80
+0.00,0.00,0.00,0.00,0.00,0.00,285.23,86.00,80.51
+0.00,0.00,0.00,0.00,0.00,0.00,285.23,86.00,80.51
+0.00,0.00,0.00,0.00,0.00,0.00,300.84,86.00,80.21
+0.00,0.00,0.00,0.00,0.00,0.00,315.09,86.00,79.94
+0.00,0.00,0.00,0.00,0.00,0.00,315.09,86.00,79.94
+0.00,0.00,0.00,0.00,0.00,0.00,326.42,86.00,79.72
+0.00,0.00,0.00,0.00,0.00,0.00,300.18,86.00,80.23
+0.00,0.00,0.00,0.00,0.00,0.00,300.18,86.00,80.23
+0.00,0.00,0.00,0.00,0.00,0.00,299.72,86.00,80.24
+0.00,0.00,0.00,0.00,0.00,0.00,316.21,86.00,79.92
+0.00,0.00,0.00,0.00,0.00,0.00,316.21,86.00,79.92
+0.00,0.00,0.00,0.00,0.00,0.00,305.35,86.00,80.13
+0.00,0.00,0.00,0.00,0.00,0.00,304.00,86.00,80.15
+0.00,0.00,0.00,0.00,0.00,0.00,292.78,86.00,80.37
+0.00,0.00,0.00,0.00,0.00,0.00,292.78,86.00,80.37
+0.00,0.00,0.00,0.00,0.00,0.00,298.45,86.00,80.26
+0.00,0.00,0.00,0.00,0.00,0.00,301.49,86.00,80.20
+0.00,0.00,0.00,0.00,0.00,0.00,301.49,86.00,80.20
+0.00,0.00,0.00,0.00,0.00,0.00,294.78,86.00,80.33
+0.00,0.00,0.00,0.00,0.00,0.00,296.26,86.00,80.30
+0.00,0.00,0.00,0.00,0.00,0.00,296.26,86.00,80.30
+Changing encoder PID setpoint!
+4200.00,0.00,4200.00,4200.00,0.00,4200.00,288.25,86.00,80.46
+8400.00,535.71,7864.29,8400.00,785.71,7614.29,108.05,86.00,83.92
+12600.00,3535.71,9064.29,12600.00,3857.14,8742.86,108.05,86.00,83.92
+17100.00,8533.33,8566.67,17100.00,9200.00,7900.00,-1443.62,86.00,113.76
+21900.00,13843.75,8056.25,21900.00,13562.50,8337.50,-646.47,86.00,98.43
+27450.00,19891.89,7558.11,27450.00,17351.35,10098.65,-383.60,86.00,93.38
+30000.00,24281.25,5718.75,30000.00,21062.50,8937.50,-495.16,86.00,95.52
+30000.00,26878.79,3121.21,30000.00,25454.55,4545.45,-368.11,86.00,93.08
+30000.00,29406.25,593.75,30000.00,30500.00,-500.00,-368.11,86.00,93.08
+30000.00,30774.19,-774.19,30000.00,31741.94,-1741.94,-98.67,86.00,87.90
+30000.00,30741.94,-741.94,30000.00,30838.71,-838.71,66.89,86.00,84.71
+30000.00,30290.32,-290.32,30000.00,30064.52,-64.52,0.00,86.00,85.16
+30000.00,29935.48,64.52,30000.00,29645.16,354.84,0.00,86.00,85.16
+30000.00,29966.67,33.33,30000.00,29433.33,566.67,0.00,86.00,86.55
+30000.00,29533.33,466.67,30000.00,30066.67,-66.67,0.00,86.00,86.94
+30000.00,29677.42,322.58,30000.00,30064.52,-64.52,-63.63,86.00,87.22
+30000.00,29967.74,32.26,30000.00,29967.74,32.26,0.00,86.00,86.91
+30000.00,29774.19,225.81,30000.00,29806.45,193.55,0.00,86.00,86.91
+30000.00,29806.45,193.55,30000.00,29774.19,225.81,0.00,86.00,86.69
+30000.00,30000.00,0.00,30000.00,29766.67,233.33,0.00,86.00,86.21
+30000.00,30033.33,-33.33,30000.00,30066.67,-66.67,0.00,86.00,86.56
+30000.00,29766.67,233.33,30000.00,30033.33,-33.33,0.00,86.00,86.56
+30000.00,29741.94,258.06,30000.00,30193.55,-193.55,0.00,86.00,86.19
+30000.00,29800.00,200.00,30000.00,30000.00,0.00,0.00,86.00,86.40
+30000.00,30000.00,0.00,30000.00,30000.00,0.00,0.00,86.00,86.95
+30000.00,29933.33,66.67,30000.00,29766.67,233.33,0.00,86.00,86.95
+30000.00,29833.33,166.67,30000.00,29766.67,233.33,0.00,86.00,86.87
+30000.00,29833.33,166.67,30000.00,30100.00,-100.00,-75.84,86.00,87.46
+30000.00,29870.97,129.03,30000.00,30000.00,0.00,-74.83,86.00,87.44
+30000.00,30064.52,-64.52,30000.00,30032.26,-32.26,-74.83,86.00,87.44
+30000.00,29741.94,258.06,30000.00,30000.00,0.00,0.00,86.00,85.36
+30000.00,29833.33,166.67,30000.00,29866.67,133.33,0.00,86.00,86.37
+30000.00,29866.67,133.33,30000.00,29966.67,33.33,0.00,86.00,86.04
+30000.00,30100.00,-100.00,30000.00,30166.67,-166.67,0.00,86.00,86.04
+30000.00,29838.71,161.29,30000.00,30193.55,-193.55,0.00,86.00,85.75
+30000.00,29870.97,129.03,30000.00,29806.45,193.55,0.00,86.00,87.00
+30000.00,30032.26,-32.26,30000.00,29935.48,64.52,0.00,86.00,86.57
+30000.00,30000.00,0.00,30000.00,30033.33,-33.33,0.00,86.00,86.57
+30000.00,29967.74,32.26,30000.00,29741.94,258.06,0.00,86.00,86.28
+30000.00,29709.68,290.32,30000.00,29838.71,161.29,0.00,86.00,86.20
+30000.00,29833.33,166.67,30000.00,30000.00,0.00,-74.30,86.00,87.43
+30000.00,30066.67,-66.67,30000.00,30066.67,-66.67,0.00,86.00,85.96
+30000.00,30064.52,-64.52,30000.00,30258.06,-258.06,0.00,86.00,85.96
+30000.00,29677.42,322.58,30000.00,30225.81,-225.81,0.00,86.00,85.81
+30000.00,29806.45,193.55,30000.00,29838.71,161.29,0.00,86.00,85.29
+30000.00,29966.67,33.33,30000.00,30066.67,-66.67,0.00,86.00,85.30
+30000.00,30064.52,-64.52,30000.00,29870.97,129.03,0.00,86.00,85.30
+30000.00,29709.68,290.32,30000.00,30000.00,0.00,0.00,86.00,85.06
+30000.00,29838.71,161.29,30000.00,30064.52,-64.52,0.00,86.00,85.78
+30000.00,30133.33,-133.33,30000.00,30000.00,0.00,0.00,86.00,86.44
+30000.00,29866.67,133.33,30000.00,30133.33,-133.33,0.00,86.00,86.44
+30000.00,29586.21,413.79,30000.00,29586.21,413.79,0.00,86.00,85.90
+30000.00,29741.94,258.06,30000.00,29838.71,161.29,0.00,86.00,86.15
+30000.00,29935.48,64.52,30000.00,30258.06,-258.06,-64.28,86.00,87.24
+30000.00,30032.26,-32.26,30000.00,29967.74,32.26,-64.28,86.00,87.24
+30000.00,30129.03,-129.03,30000.00,30129.03,-129.03,0.00,86.00,85.14
+30000.00,29966.67,33.33,30000.00,30133.33,-133.33,0.00,86.00,85.38
+30000.00,29774.19,225.81,30000.00,30064.52,-64.52,0.00,86.00,85.39
+30000.00,29741.94,258.06,30000.00,29935.48,64.52,0.00,86.00,86.18
+30000.00,30066.67,-66.67,30000.00,30200.00,-200.00,0.00,86.00,86.18
+30000.00,29966.67,33.33,30000.00,30200.00,-200.00,0.00,86.00,85.20
+30000.00,29966.67,33.33,30000.00,29900.00,100.00,0.00,86.00,85.71
+30000.00,29933.33,66.67,30000.00,30166.67,-166.67,0.00,86.00,85.62
+30000.00,30033.33,-33.33,30000.00,30000.00,0.00,0.00,86.00,85.62
+30000.00,29967.74,32.26,30000.00,29741.94,258.06,0.00,86.00,85.76
+30000.00,29866.67,133.33,30000.00,29633.33,366.67,0.00,86.00,85.67
+30000.00,29633.33,366.67,30000.00,29966.67,33.33,0.00,86.00,86.83
+30000.00,29866.67,133.33,30000.00,30066.67,-66.67,0.00,86.00,86.83
+30000.00,30096.77,-96.77,30000.00,30258.06,-258.06,0.00,86.00,85.78
+30000.00,29838.71,161.29,30000.00,29870.97,129.03,0.00,86.00,85.51
+30000.00,29774.19,225.81,30000.00,30129.03,-129.03,0.00,86.00,86.00
+30000.00,29944.44,55.56,30000.00,30055.56,-55.56,0.00,86.00,85.10
+30000.00,30100.00,-100.00,30000.00,30066.67,-66.67,0.00,86.00,85.10
+30000.00,29800.00,200.00,30000.00,30133.33,-133.33,0.00,86.00,85.70
+30000.00,29800.00,200.00,30000.00,29933.33,66.67,-56.45,86.00,87.09
+30000.00,29838.71,161.29,30000.00,30000.00,0.00,0.00,86.00,86.36
+30000.00,29966.67,33.33,30000.00,30100.00,-100.00,0.00,86.00,86.36
+30000.00,30064.52,-64.52,28380.00,29903.23,-1523.23,0.00,86.00,85.92
+25560.00,29741.94,-4181.94,23730.00,29516.13,-5786.13,0.00,86.00,86.49
+21360.00,28928.57,-7568.57,19530.00,27964.29,-8434.29,250.96,86.00,81.17
+16710.00,25354.84,-8644.84,14880.00,24129.03,-9249.03,250.96,86.00,81.17
+12060.00,19096.77,-7036.77,10230.00,17612.90,-7382.90,431.65,86.00,77.70
+7410.00,14129.03,-6719.03,5580.00,12548.39,-6968.39,204.14,86.00,82.07
+2910.00,10666.67,-7756.67,1080.00,8566.67,-7486.67,239.26,86.00,81.40
+0.00,7137.93,-7137.93,0.00,3172.41,-3172.41,239.26,86.00,81.40
+0.00,1137.93,-1137.93,0.00,-482.76,482.76,420.99,86.00,77.90
+0.00,-2068.97,2068.97,0.00,241.38,-241.38,-635.86,86.00,98.23
+0.00,-1620.69,1620.69,0.00,275.86,-275.86,-512.85,86.00,95.86
+0.00,-1275.86,1275.86,0.00,0.00,0.00,-512.85,86.00,95.86
+0.00,-620.69,620.69,0.00,0.00,0.00,-539.09,86.00,96.37
+0.00,-892.86,892.86,0.00,0.00,0.00,-243.08,86.00,90.67
+0.00,142.86,-142.86,0.00,0.00,0.00,-243.08,86.00,90.67
+0.00,68.97,-68.97,0.00,0.00,0.00,-136.52,86.00,88.63
+0.00,0.00,0.00,0.00,0.00,0.00,-128.41,86.00,88.47
+0.00,0.00,0.00,0.00,0.00,0.00,-111.77,86.00,88.15
+0.00,0.00,0.00,0.00,0.00,0.00,-111.77,86.00,88.15
+0.00,0.00,0.00,0.00,0.00,0.00,-126.20,86.00,88.43
+0.00,0.00,0.00,0.00,0.00,0.00,-127.95,86.00,88.46
+0.00,0.00,0.00,0.00,0.00,0.00,-127.95,86.00,88.46
+0.00,0.00,0.00,0.00,0.00,0.00,-118.92,86.00,88.29
+0.00,0.00,0.00,0.00,0.00,0.00,-123.96,86.00,88.38
+0.00,0.00,0.00,0.00,0.00,0.00,-123.96,86.00,88.38
+0.00,0.00,0.00,0.00,0.00,0.00,-118.56,86.00,88.28
+0.00,0.00,0.00,0.00,0.00,0.00,-123.68,86.00,88.38
+0.00,0.00,0.00,0.00,0.00,0.00,-125.11,86.00,88.41
diff --git a/pid_vis.py b/pid_vis.py
index da21df8..0490878 100644
--- a/pid_vis.py
+++ b/pid_vis.py
@@ -2,37 +2,72 @@
 import csv
 from collections import defaultdict
 from matplotlib import pyplot as plt
+import numpy as np
 
 with Path('pid_test.csv').open('r') as f:
     reader = csv.DictReader(filter(lambda line: not line.startswith('#'), f))
+
     lists = defaultdict(list)
-    header = "EncoderLeft,EncoderRight,DesiredVelocityLeft,DesiredVelocityRight,CurrentVelocityLeft,CurrentVelocityRight,LeftPower,RightPower,EncoderTargetLeft,EncoderTargetRight"
+    # header = "EncoderLeft,EncoderRight,DesiredVelocityLeft,DesiredVelocityRight,CurrentVelocityLeft,CurrentVelocityRight,LeftPower,RightPower,EncoderTargetLeft,EncoderTargetRight"
+    header = "LeftSetpoint,LeftVelocity,LeftError,RightSetpoint,RightVelocity,RightError,HeadingVelocityCorrection,HeadingSetpoint,Heading"
     keys = header.split(",")
     for row in reader:
+        if row["LeftSetpoint"].startswith("Changing"):
+            continue
         for k in keys:
             val = int(row[k]) if "." not in row[k] else float(row[k])
-            if "Left" in k:
-                val *= -1 # Correct for rotation test
+            # if "Left" in k:
+            #     val *= -1 # Correct for rotation test
             lists[k].append(val)
+    lists = {k: np.array(v) for k, v in lists.items()}
+
+# power_deadzone = [0.15] * len(lists["EncoderLeft"])
+# neg_power_deadzone = [-0.15] * len(lists["EncoderLeft"])
+
+# ax_enc.set_title("Encoder Values")
+# ax_enc.plot(
+#     lists["EncoderLeft"], "-b",
+#     lists["EncoderRight"], "-r",
+#     lists["EncoderTargetLeft"], "--c",
+#     lists["EncoderTargetRight"], "--m",
+# )
+# ax_vel.set_title("Velocity Values")
+# ax_vel.plot(
+#     lists["CurrentVelocityLeft"], "-b",
+#     lists["CurrentVelocityRight"], "-r",
+#     lists["DesiredVelocityLeft"], "--c",
+#     lists["DesiredVelocityRight"], "--m",
+# )
+# ax_pow.set_title("Motor Powers")
+# ax_pow.plot(power_deadzone, "--k", neg_power_deadzone, "--k", lists["LeftPower"], "-g", lists["RightPower"], "-y")
+# plt.show()
+
+fig = plt.figure(figsize=(15, 8))
+
+# Velocity plot (top left)
+ax_vel = plt.subplot2grid((2, 2), (0, 0))
+ax_vel.set_title("Velocity Values")
+ax_vel.plot(lists["LeftSetpoint"]-lists["HeadingVelocityCorrection"], "--m", lists["LeftVelocity"], "-r", lists["LeftError"], "--g",
+            lists["RightSetpoint"]+lists["HeadingVelocityCorrection"], "--c", lists["RightVelocity"], "-b", lists["RightError"], "--y",
+            lists["HeadingVelocityCorrection"], "-.k")
+ax_vel.legend(["Left Setpoint", "Left Velocity", "Left Error", "Right Setpoint", "Right Velocity", "Right Error", "Heading Velocity Correction"])
+
+# Heading plot (bottom left)
+ax_heading = plt.subplot2grid((2, 2), (1, 0), sharex=ax_vel)
+ax_heading.set_title("Heading")
+ax_heading.plot(lists["Heading"], "-k")
+ax_heading.plot(lists["HeadingSetpoint"], "--c")
+ax_heading.legend(["Heading", "Heading Setpoint"])
+ax_heading.set_xlabel("Time (index)")
+
+# Polar plot (right, spans both rows)
+ax_polar = plt.subplot2grid((2, 2), (0, 1), rowspan=2, projection='polar')
+ax_polar.set_title("Heading (Polar)")
+theta = np.deg2rad(lists["Heading"])
+r = np.arange(len(theta))
+ax_polar.plot(theta, r, color="purple")
+ax_polar.plot(np.deg2rad(lists["HeadingSetpoint"]), r, "--", color="cyan")
+ax_polar.set_rticks([])  # Hide radial ticks for clarity
 
-    power_deadzone = [0.15] * len(lists["EncoderLeft"])
-    neg_power_deadzone = [-0.15] * len(lists["EncoderLeft"])
-
-    fig, (ax_enc, ax_vel, ax_pow) = plt.subplots(3, 1)
-    ax_enc.set_title("Encoder Values")
-    ax_enc.plot(
-        lists["EncoderLeft"], "-b",
-        lists["EncoderRight"], "-r",
-        lists["EncoderTargetLeft"], "--c",
-        lists["EncoderTargetRight"], "--m",
-    )
-    ax_vel.set_title("Velocity Values")
-    ax_vel.plot(
-        lists["CurrentVelocityLeft"], "-b",
-        lists["CurrentVelocityRight"], "-r",
-        lists["DesiredVelocityLeft"], "--c",
-        lists["DesiredVelocityRight"], "--m",
-    )
-    ax_pow.set_title("Motor Powers")
-    ax_pow.plot(power_deadzone, "--k", neg_power_deadzone, "--k", lists["LeftPower"], "-g", lists["RightPower"], "-y")
-    plt.show()
+plt.tight_layout()
+plt.show()
diff --git a/platformio.ini b/platformio.ini
index f14403b..585c908 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -13,6 +13,15 @@ platform = espressif32
 board = lolin_s2_mini
 framework = arduino
 monitor_speed = 115200
+# build_src_flags
+build_flags =
+	-O3
+	-Wall
+	-Wextra
+	-std=gnu++17
+build_unflags =
+	-std=gnu++11
 lib_deps = 
 	bblanchon/ArduinoJson@^7.2.0
 	paulstoffregen/Encoder@^1.4.4
+monitor_raw = yes
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 279268d..4bbd239 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,84 +1,51 @@
-// Sets file name into memory (usually to avoid duplicate imports)
-#ifndef CHESSBOT_MAIN_CPP
-#define CHESSBOT_MAIN_CPP
-
-// Built-In Libraries
 #include <Arduino.h>
+#include <WiFi.h>
 
-// Custom Libraries
+#include "../env.h"
+#include "robot/robot.h"
+#include "robot/pid.h"
+#include "tests.h"
 #include "utils/config.h"
 #include "utils/logging.h"
-#include "utils/timer.h"
-#include "utils/status.h"
-#include "wifi/wireless.h"
 #include "wifi/connection.h"
-#include "robot/control.h"
-#include "robot/encoder.h"
-#include "../env.h"
-#include "robot/pidController.h"
+#include "wifi/packet.h"
 
-//alright SCREW YOU serial monitor i won't print every frame then if you wanna play that game
-const int8_t PRINT_INTERVAL = 60;
-int8_t framesUntilPrint = 60;
+uint32_t frame = 0;
+uint32_t previous_time = 0;
 
-// Setup gets run at startup
 void setup() {
-    // Serial port for debugging purposes
-    if (LOGGING_LEVEL > 0) Serial.begin(115200);
-
-    delay(STARTUP_DELAY);
-    serialLogln("Finished Delay!", 2);
-
-
-    // Any setup needed to get bot ready
-    setupBot();
-
-    // Create a WiFi network for the laptop to connect to
-    if (!RUN_OFFLINE) connectWiFI();
-
-    if (DO_DRIVE_TEST) startDriveTest();
+    #if ONLINE
+        WiFi.mode(WIFI_STA);
+        WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
+    #endif
 
-    delay(2000);
+    if (LOGGING_LEVEL > 0) {
+        Serial.begin(115200);
+    };
 
-    //start reading the light
-    if (DO_DRIVE_TICKS_TEST) driveTicks(20000, "NULL");
-
-    if (DO_HARDWARE_TEST) timerDelay(5000, &startMotorAndEncoderTest);
+    // sleepy_test(robot);
+    // hardware_test(robot);
 }
 
-// After setup gets run, loop is run over and over as fast ass possible
 void loop() {
-    // Checks if any timers have expired
-    timerStep();
-
-    if (!RUN_OFFLINE) 
-    {
-        // Checks whether bot is still connected to WiFi. Reconnect if not
-        if (getWiFiConnectionStatus() && !checkWiFiConnection()) reconnectWiFI();
-        // Checks whether bot is still connected to the server. Reconnect if not
-        if (getServerConnectionStatus() && !checkServerConnection()) reconnectServer();
-
-        // If the bot is connected to the server, check for received data, and accept it if available
-        if (getServerConnectionStatus()) acceptData();
-        // Checks whether bot is still connected to the server. Reconnect if not
-        if (getServerConnectionStatus() && !checkServerConnection()) reconnectServer();
-
-        // If the bot is connected to the server, check for received data, and accept it if available
-        if (getServerConnectionStatus()) acceptData();
-    }
-
-    // Run control loop
-    controlLoop(loopDelayMilliseconds, framesUntilPrint);
-
-    // This delay determines how often the code in loop is run
-    // (Forcefully pauses the thread for about the amount of milliseconds passed in)
-  	delay(loopDelayMilliseconds);
-    framesUntilPrint--;
-    if(framesUntilPrint < 0)
-    {
-        framesUntilPrint = PRINT_INTERVAL;
-    }
-}
-
-// This is used at the end of each file due to the name definition at the beginning
-#endif
+    delay(10); // We want to run at ~100 fps to standardize motor power <-> speed
+    uint32_t delta = micros() - previous_time;
+    previous_time = micros();
+    
+    #if ONLINE
+        connection_check_reconnect();
+        auto packet = recv_packet();
+        if (packet.has_value()) {
+            handle_packet(robot, packet.value());
+        }
+    #endif
+
+    robot.tick(frame, delta);
+
+    // center_test(robot);
+    // line_test(robot);
+    square_test(robot);
+    // circle_test(robot);
+
+    frame++;
+}
\ No newline at end of file
diff --git a/src/robot/battery.cpp b/src/robot/battery.cpp
deleted file mode 100644
index cb41b44..0000000
--- a/src/robot/battery.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef CHESSBOT_BATTERY_CPP
-#define CHESSBOT_BATTERY_CPP
-
-// Associated Header File
-#include "robot/battery.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
-#include "utils/config.h"
-
-int batteryVoltageOffset = 100;
-
-int getBatteryLevel() {
-    return analogRead(BATTERY_VOLTAGE_PIN) - batteryVoltageOffset;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/control.cpp b/src/robot/control.cpp
deleted file mode 100644
index 5631c5a..0000000
--- a/src/robot/control.cpp
+++ /dev/null
@@ -1,831 +0,0 @@
-#ifndef CHESSBOT_CONTROL_CPP
-#define CHESSBOT_CONTROL_CPP
-
-// Associated Header File
-#include "robot/control.h"
-#include "robot/trapezoidalProfile.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-#include <queue>
-
-// Custom Libraries
-#include "utils/logging.h"
-#include "utils/timer.h"
-#include "utils/config.h"
-#include "utils/status.h"
-#include "robot/motor.h"
-#include "robot/lightSensor.h"
-#include "wifi/connection.h"
-#include "robot/encoder.h"
-#include "robot/pidController.h"
-#include "robot/driveTest.h"
-#include "../../env.h"
-#include <algorithm>
-#include "utils/functions.h"
-
-//PLEASE ONLY USE CHESSBOT #4 FOR TESTING
-PIDController encoderAVelocityController(0.00008, 0.0000035, 0.000001, -1, +1); //Blue
-PIDController encoderBVelocityController(0.00008, 0.0000035, 0.000001, -1, +1); //Red
-
-//put this in manually for each bot. Dist between the two front encoders, or the two back encoders. In meters.
-const float lightDist = 0.07;
-
-//just measured, its 5.9 centimeters, or .059 meters
-const float TIRE_RADIUS = 0.059;
-//circumference equals pi * diameter. In meters
-const float TIRE_CIRCUMFERENCE = M_PI * 2 * TIRE_RADIUS;
-//multiply any ticks by this ratio to get distance in meters
-const float TICK_TO_METERS = TIRE_CIRCUMFERENCE / TICKS_PER_ROTATION;
-
-const uint8_t Top_Left_Encoder_Index = 0;
-const uint8_t Top_Right_Encoder_Index = 1;
-const uint8_t Bottom_Left_Encoder_Index = 2;
-const uint8_t Bottom_Right_Encoder_Index = 3;
-
-boolean testEncoderPID_value = false;
-
-//determines the encoder values the iteration right before
-int prevPositionA = 0;
-int prevPositionB = 0;
-
-//when moving to a different target, this is the encoder position we started from
-int startEncoderAPos = -1;
-int startEncoderBPos = -1;
-
-//the encoder distance to about halfway through the tile
-int encoderAHalfwayDist = 0;
-int encoderBHalfwayDist = 0;
-
-//when we receive a command from the website to center, set this from false to true,
-//then when the code sets it back to false, that's when we know we're done centering
-bool isCentering = false;
-
-//holds the current status representing the progress in centering. The different values
-//are defined below
-char centeringStatus = 'S';
-
-//measures if the forward encoders are aligning on the edge or the back encoders
-bool forwardAligning = true;
-
-//set this true when our robot is on an edge and is moving to the center from its given measured distance
-bool movingCenter = false;
-
-//set this true when our robot is turning to align itself on the next axis
-bool turningToNextAxis = false;
-
-//initially 0, once 2 means both axises aligned so we done
-uint8_t axisesAligned = 0;
-
-float angle = 0;
-unsigned long timeSinceTurn = 0;
-
-//the value of the encoder we're measuring in terms of its light value
-bool firstEncoderVal = false;
-bool secondEncoderVal = false;
-
-//represents which index is being referenced for the current encoders
-uint8_t firstEncoderIndex = 0;
-uint8_t secondEncoderIndex = 0;
-
-//Here are the possible values:
-    //0 = no encoder leading.
-    //1 = left encoder leading.
-    //2 = right encoder leading
-uint8_t leadingEncoder = 0;
-
-//previous encoder distance, specifically the one that is lagging behind
-float backPrevDistance = 0;
-
-//checks if the given encoders have changed tiles yet.
-bool leftEncoderChange = false;
-bool rightEncoderChange = false;
-
-//determines basically what tile each encoder is one. false is one tile, true is the opposite tile.
-//it could be false = white and true = black, or false = black and white = true, doesn't really matter
-bool onFirstTile[4] = {false, false, false, false};
-
-bool waitingForLight = false;
-
-void testEncoderPID()
-{
-    serialLogln("Changing encoder PID setpoint!", 2);
-    if (!testEncoderPID_value)
-    {
-        testEncoderPID_value = true;
-        setLeftMotorControl({POSITION, (float)TICKS_PER_ROTATION * 5});
-        setRightMotorControl({POSITION, (float)TICKS_PER_ROTATION * 5});
-    }
-    else
-    {
-        testEncoderPID_value = false;
-        setLeftMotorControl({POSITION, 0.0f});
-        setRightMotorControl({POSITION, 0.0f});
-    }
-
-    updateCritRange();
-}
-
-//crit range is basically getting distance we go until we are halfway to target. By the end of this range,
-//we're at our max speed and are sure we aren't at a low speed just cause we're speeding up
-void updateCritRange()
-{
-    //want to record the values before we move now
-    startEncoderAPos = profileA.currentPosition;
-    startEncoderBPos = profileB.currentPosition;
-    
-    profileA.criticalRange = fabs(profileA.targetPosition - startEncoderAPos) / 2;
-    profileB.criticalRange = fabs(profileB.targetPosition - startEncoderBPos) / 2;
-
-    //update it so that time since start is now equal to this. Only really care about this value after turns though
-    timeSinceTurn = millis();
-}
-
-void testTurn()
-{
-    resetSpeed();
-    
-    serialLog("Changing destination angle to ", 2);
-    serialLog(angle, 2);
-    //simple maths
-    turn(M_PI / 180 * angle, "NULL");
-    serialLog(" (", 2);
-    serialLog(getLeftMotorControl().value, 2);
-    serialLog(", ", 2);
-    serialLog(getRightMotorControl().value, 2);
-    serialLogln(")", 2);
-
-    //compute the new crit range now that target and start encoder have changed
-    updateCritRange();
-}
-
-void testCentering()
-{
-    serialLogln("Running centering routine...", 2);
-    startCentering();
-}
-
-// Sets up all the aspects needed for the bot to work
-void setupBot() {
-    serialLogln("Setting Up Bot...", 2);
-
-    pinMode(ONBOARD_LED_PIN, OUTPUT);
-    digitalWrite(ONBOARD_LED_PIN, HIGH);
-
-    setupMotors();
-    setupIR();
-    setupEncodersNew();
-    serialLogln("Bot Set Up!", 2);
-
-    encoderAVelocityController.Reset();
-    encoderBVelocityController.Reset();
-
-    if (DO_PID_TEST) {
-        testEncoderPID();
-        timerInterval(12000, &testEncoderPID);
-    }
-    
-    if (DO_TURN_TEST) {
-        angle = 30;
-        testTurn();
-        timerInterval(5000, testTurn);
-    }
-
-    if (DO_CENTERING_TEST) {
-        testCentering();
-        timerInterval(5000, &testCentering);
-    }
-}
-
-// + (0.0001 * desiredVelocityA)
-// Manages control loop (loopDelayMs is for reference)
-void controlLoop(int loopDelayMs, int8_t framesUntilPrint) {
-    if (DO_LIGHT_SENSOR_TEST)
-    {
-        //read the light values. May actually.... not want to do this if not centering? Keeping it outside the if
-        //statement in case it'll be used for something else
-        readLight(loopDelayMs);
-        if(isCentering)
-        {
-            // TODO see if targetPosition needs to be updated in the middle of this method - doesn't look like it?
-            updateCentering();
-        }
-        
-        // uint8_t status = driveUntilNewTile(onFirstTile);
-
-        //this way we don't even upload the code if it's not being used
-        #if LOGGING_LEVEL >= 4
-            //know our char will be 4 bits    
-            char vals[5];
-            //read the booleans as char
-            for(uint8_t i = 0; i < 4; i++)
-            {
-                vals[i] = onFirstTile[i] ? '1' : '0';
-            }
-            //must null terminate
-            vals[4] = '\0';
-            serialLog("Light statuses: ", 2);
-            serialLogln(vals, 2);
-        #endif
-    }
-
-    if (DO_ENCODER_TEST) encoderLoop();
-
-    if (DO_PID) {
-        double loopDelaySeconds = ((double) loopDelayMs) / 1000;
-
-        int currentPositionEncoderA = readLeftEncoder();
-        int currentPositionEncoderB = readRightEncoder();
-        double currentVelocityA = (currentPositionEncoderA - prevPositionA) / loopDelaySeconds;
-        double currentVelocityB = (currentPositionEncoderB - prevPositionB) / loopDelaySeconds;
-
-        profileA.currentPosition = currentPositionEncoderA;
-        profileA.currentVelocity = currentVelocityA;
-        profileB.currentPosition = currentPositionEncoderB;
-        profileB.currentVelocity = currentVelocityB;
-
-        double desiredVelocityA, desiredVelocityB;
-
-        if (getLeftMotorControl().mode == POSITION) {
-            desiredVelocityA = updateTrapezoidalProfile(profileA, loopDelaySeconds, framesUntilPrint);
-        } else {
-            desiredVelocityA = getLeftMotorControl().value;
-        }
-        if (getRightMotorControl().mode == POSITION) {
-            desiredVelocityB = updateTrapezoidalProfile(profileB, loopDelaySeconds, framesUntilPrint);
-        } else {
-            desiredVelocityB = getRightMotorControl().value;
-        }
-
-        prevPositionA = currentPositionEncoderA;
-        prevPositionB = currentPositionEncoderB;
-
-        double leftFeedForward = desiredVelocityA / MAX_VELOCITY_TPS;
-        double rightFeedForward = desiredVelocityB / MAX_VELOCITY_TPS;
-
-        double leftMotorPower = encoderAVelocityController.Compute(desiredVelocityA, currentVelocityA, loopDelaySeconds) + leftFeedForward;
-        double rightMotorPower = encoderBVelocityController.Compute(desiredVelocityB, currentVelocityB, loopDelaySeconds) + rightFeedForward;
-
-        if (leftMotorPower > 1) leftMotorPower = 1;
-        if (leftMotorPower < -1) leftMotorPower = -1;
-        if (rightMotorPower > 1) rightMotorPower = 1;
-        if (rightMotorPower < -1) rightMotorPower = -1;
-
-        //using macros this code isn't uploaded if not proper loging levels
-        #if LOGGING_LEVEL >= 4
-        
-        if(framesUntilPrint == 0)
-        {
-            serialLog("Current encoder A pos: ", 2);
-            serialLog(currentEncoderA, 2);
-            serialLog(", ", 2);
-            serialLog("Current encoder B pos: ", 2);
-            serialLog(currentEncoderB, 2);
-            serialLog(", ", 2);
-            serialLog("Desired encoder A speed: ", 2);
-            serialLog(desiredVelocityA, 2);
-            serialLog(", ", 2);
-            serialLog("Desired encoder B speed: ", 2);
-            serialLog(desiredVelocityB, 2);
-            serialLog(", ", 2);
-            serialLog("current encoder a speed: ", 2);
-            serialLog(currentVelocityA, 2);
-            serialLog(", ", 2);
-            serialLog("current encoder b speed: ", 2);
-            serialLog(currentVelocityB, 2);
-            serialLog(", ", 2);
-            serialLog("current left motor power: ", 2);
-            serialLog(leftMotorPower, 2);
-            serialLog(", ", 2);
-            serialLog("current right motor power: ", 2);
-            serialLog(rightMotorPower, 2);
-            serialLog(", ", 2);
-            serialLog("current encoder a target: ", 2);
-            serialLog(leftMotorControl.mode == POSITION ? leftMotorControl.value : 0, 2);
-            serialLog(", ", 2);
-            serialLog("current encoder b target: ", 2);
-            serialLog(rightMotorControl.mode == POSITION ? rightMotorControl.value : 0, 2); // TODO log results of trapezoidal profile into csv (on motor value graph)
-            serialLog(", ", 2);
-            serialLog("is robot pid at target? ", 2);
-            serialLog(isRobotPidAtTarget(), 2);
-            serialLog(", ", 2);
-            serialLogln(loopDelaySeconds, 2);
-        }
-        
-        #endif
-
-        drive(
-            leftMotorPower, // leftMotorPower,
-            rightMotorPower, // rightMotorPower,
-            "NULL"
-        );
-
-        // serialLogln(leftMotorPower, 3);
-
-        // turn(M_PI / 2, "NULL");
-    }
-}
-
-// start centering process
-void startCentering()
-{
-    if (!isCentering)
-    {
-        isCentering = true;
-        centeringStatus = 'S';
-    }
-}
-
-//this determines what our next action will be after an edge alignment, move to center, or axis turn.
-void determineNextAction()
-{
-    //if we were previously turning to the next axis, and finished:
-    if(turningToNextAxis)
-    {
-        turningToNextAxis = false;
-        axisesAligned++;
-        //done centering yippee!
-        if(axisesAligned == 2)
-        {
-            axisesAligned = 0;
-            isCentering = false;
-            centeringStatus = 'F';
-        }
-        //otherwise restart the whole process for this next axis
-        else
-        {
-            centeringStatus = 'S';
-        }
-    }
-    //if previously moving to center for an axis, and finished:
-    else if(movingCenter)
-    {
-        //now, turn to next direction
-        movingCenter = false;
-        turningToNextAxis = true;
-        centeringStatus = 'M';
-        //this way, if no axises aligned yet, angle = -90, and if one axis aligned,
-        //angle = 90, undoing the previous rotation.
-        angle = 90 * (axisesAligned * 2 - 1);
-
-        testTurn();
-    }
-    //if just aligned on a forward edge with the encoders in the front:
-    else if(forwardAligning)
-    {
-        //if going forward, store the current encoder value so we can see the full encoder length of the tiles
-        encoderAHalfwayDist = profileA.currentPosition;
-        encoderBHalfwayDist = profileB.currentPosition;
-        //swap to going back
-        forwardAligning = !forwardAligning;
-        //set that new drive
-        createDriveUntilNewTile();
-        centeringStatus = 'E';
-
-        serialLog((char*)"Current encoder A: ", 2);
-        serialLogln(profileA.currentPosition, 2);
-        serialLog((char*)"Current encoder B: ", 2);
-        serialLogln(profileB.currentPosition, 2);
-        serialLog((char*)"Target encoder A: ", 2);
-        serialLogln(leftMotorControl.mode == POSITION ? leftMotorControl.value : 0, 2);
-        serialLog((char*)"Target encoder B: ", 2);
-        serialLogln(rightMotorControl.mode == POSITION ? rightMotorControl.value : 0, 2);
-        }
-    //if just aligned on a backward edge with the encoders in the back:
-    else
-    {
-        //swap to going forward now
-        forwardAligning = !forwardAligning;
-        
-        //since we always go forward first and then backwards, the current value of encoderHalfwayDist > currentEncoder always
-        //what we're doing is currently, "encoderAHalfwayDist" just stores the value of the other edge in encoder ticks, now we're
-        //finding the difference between them. And of course divide by 2 as want half that distance
-        encoderAHalfwayDist = (encoderAHalfwayDist - profileA.currentPosition) / 2;
-        encoderBHalfwayDist = (encoderBHalfwayDist - profileB.currentPosition) / 2;
-
-        //decide that we take average of encoder A and B's distances, since ideally we want both to travel the same amount
-        int totalHalfwayDistance = (encoderAHalfwayDist + encoderBHalfwayDist) / 2;
-        driveTicks(totalHalfwayDistance, "NULL");
-        centeringStatus = 'M';
-        //now we moving to da center
-        movingCenter = true;
-    }
-}
-
-//this updates our centering depending on the current status of it
-void updateCentering()
-{
-    //what we do depend on the current status
-    switch(centeringStatus)
-    {
-        //What each status bit means:
-        //  S means we just started centering on the current axis
-        //  E means we are moving to an edge
-        //  M means the bot is just moving by itself, whether that be to correct for a turn or to just get to the halfway point
-        case 'S':
-            //create the first drive
-            createDriveUntilNewTile();
-            //now change it so we're driving forward
-            centeringStatus = 'E';
-            break;
-        case 'E':
-        {
-            //continue driving forward
-            uint8_t driveStatus = driveUntilNewTile();
-            //reminder status 3 = no leading encoders and driving finished
-            if(driveStatus == 3)
-            {
-                //meaning we can now go in opposite direction.
-                determineNextAction();
-            }
-            //reminder status 2 = one leading encoder finished first, but driving is finished
-            else if(driveStatus == 2)
-            {
-                //mean we now begin correcting
-                centeringStatus = 'M';
-            }
-            break;
-        }
-        case 'M':
-            //check if we're the moving we're doing is finished. If so, determine what we do next.
-            if(checkMoveFinished())
-            {
-                determineNextAction();
-            }
-            break;
-    }
-}
-
-//checks if we're done moving to our target when either moving half a tile's length or turning
-bool checkMoveFinished()
-{
-    //first, checks like "fabs(profileA.currentVelocity) < 2" see if we're slowing down or not.
-
-    //then, something like "fabs(currentEncoderA - encoderATarget) < criticalRangeA" is making sure
-    //the reason our speed is slow is specifically because we're slowing down and not because we're
-    //beginning to speed up.
-    //do this by seeing if the distance remaining is less than the midpoint distance from start to end, as by then we're at our max speed.
-
-    bool encoderAChecks = fabs(profileA.currentPosition - profileA.targetPosition) < profileA.criticalRange && fabs(profileA.currentVelocity) < 3;
-    bool encoderBChecks = fabs(profileB.currentPosition - profileB.targetPosition) < profileB.criticalRange  && fabs(profileB.currentVelocity) < 3;
-
-    //check if we've been stalling too long, for 8 seconds. If we're over time, that's bad, and means we should declare the movement finished.
-    bool timerCheck = millis() - timeSinceTurn > 8000;
-    
-    return ((encoderAChecks && encoderBChecks) || timerCheck);
-}
-
-//like the one above but just seeing if we can keep moving or not
-bool checkIfCanUpdateMovement()
-{
-    return fabs(profileA.currentPosition - profileA.targetPosition) < profileA.criticalRange && fabs(profileB.currentPosition - profileB.targetPosition) < profileB.criticalRange;
-}
-
-void setLeftMotorControl(ControlSetting control) {
-    leftMotorControl = control;
-    if (control.mode == POSITION)
-        profileA.targetPosition = control.value;
-    else
-        profileA.targetVelocity = control.value;
-}
-
-void setRightMotorControl(ControlSetting control) {
-    rightMotorControl = control;
-    if (control.mode == POSITION)
-        profileB.targetPosition = control.value;
-    else
-        profileB.targetVelocity = control.value;
-}
-
-ControlSetting getLeftMotorControl() {
-    return leftMotorControl;
-}
-
-ControlSetting getRightMotorControl() {
-    return rightMotorControl;
-}
-
-void drive(float tiles, std::string id) {
-    if (!getStoppedStatus()) {
-        const float TILE_SIZE_INCHES = 24;
-        float distanceInches = tiles * TILE_SIZE_INCHES;
-        float ticksPerInch = TICKS_PER_ROTATION / (WHEEL_DIAMETER_INCHES * M_PI);
-        int tickDistance = (int)(distanceInches * ticksPerInch);
-        driveTicks(tickDistance, id);
-    }    
-    
-}
-
-//drives the given amount of ticks
-void driveTicks(int tickDistance, std::string id)
-{
-    if (!getStoppedStatus()) {
-        resetSpeed();
-        setLeftMotorControl({POSITION, (float)(readLeftEncoder() + tickDistance)});
-        setRightMotorControl({POSITION, (float)(readRightEncoder() + tickDistance)});
-        updateCritRange();
-
-        if (id != "NULL") {
-            sendPacketOnPidComplete(id);
-        }
-    }
-}
-
-// Drives the wheels according to the powers set. Negative is backwards, Positive forwards
-void drive(float leftPower, float rightPower, std::string id) {
-    if (!getStoppedStatus()) {
-        // TODO: maybe move to motor.cpp?
-        float minPower = 0.16;
-        if (leftPower < minPower && leftPower > -minPower) {
-            leftPower = 0;
-        } if (rightPower < minPower && rightPower > -minPower) {
-            rightPower = 0;
-        }
-
-        setLeftPower(leftPower);
-        setRightPower(rightPower);
-
-        //we only send null as id during our test drive. The only other time this drive method is called will be
-        //when the server sends it, meaning it will have an id to send back.
-        if (id != "NULL") { sendActionSuccess(id); }
-    }
-}
-
-//turns the given amount in radians
-void turn(float angleRadians, std::string id) {
-
-    serialLogln("Turning", 3);
-    serialLogln(angleRadians, 3);
-    int offsetTicks = radiansToTicks(angleRadians);
-
-    if (getLeftMotorControl().mode == POSITION) {
-        setLeftMotorControl({POSITION, getLeftMotorControl().value - offsetTicks});
-    } else {
-        setLeftMotorControl({POSITION, (float)(readLeftEncoder() - offsetTicks)});
-    }
-    if (getRightMotorControl().mode == POSITION) {
-        setRightMotorControl({POSITION, getRightMotorControl().value + offsetTicks});
-    } else {
-        setRightMotorControl({POSITION, (float)(readRightEncoder() + offsetTicks)});
-    }
-
-    if (id != "NULL")
-    {
-        sendPacketOnPidComplete(id);
-    }
-}
-
-// Stops the bot in its tracks
-void stop() {
-    setStoppedStatus(true);
-    setLeftPower(0);
-    setRightPower(0);
-
-    serialLogln("Bot Stopped!", 2);
-}
-
-boolean isRobotPidAtTarget() {
-    if (!DO_PID)
-        return true;
-
-    boolean leftAtTarget, rightAtTarget;
-
-    if (getLeftMotorControl().mode == POSITION)
-    {
-        leftAtTarget = approxEquals(getLeftMotorControl().value, profileA.currentPosition, PID_POSITION_TOLERANCE)
-                    && approxEquals(profileA.currentVelocity, 0.0, PID_VELOCITY_TOLERANCE);
-
-        serialLog("Left Position: ", 3);
-        serialLog(profileA.currentPosition, 3);
-        serialLog(", Target: ", 3);
-        serialLog(getLeftMotorControl().value, 3);
-        serialLog(", Velocity: ", 3);
-        serialLog(profileA.currentVelocity, 3);
-        serialLog(", At Target: ", 3);
-        serialLogln(leftAtTarget, 3);
-    }
-    else
-    {
-        leftAtTarget = approxEquals(getLeftMotorControl().value, profileA.currentVelocity, PID_VELOCITY_TOLERANCE);
-    }
-    if (getRightMotorControl().mode == POSITION)
-    {
-        rightAtTarget = approxEquals(getRightMotorControl().value, profileB.currentPosition, PID_POSITION_TOLERANCE)
-                     && approxEquals(profileB.currentVelocity, 0.0, PID_VELOCITY_TOLERANCE);
-
-        serialLog("Right Position: ", 3);
-        serialLog(profileB.currentPosition, 3);
-        serialLog(", Target: ", 3);
-        serialLog(getRightMotorControl().value, 3);
-        serialLog(", Velocity: ", 3);
-        serialLog(profileB.currentVelocity, 3);
-        serialLog(", At Target: ", 3);
-        serialLogln(rightAtTarget, 3);
-    }
-    else
-    {
-        rightAtTarget = approxEquals(getRightMotorControl().value, profileB.currentVelocity, PID_VELOCITY_TOLERANCE);
-    }
-
-    return leftAtTarget && rightAtTarget;
-}
-
-void sendPacketOnPidComplete(std::string id) {
-    if (!DO_PID)
-        sendActionFail(id);
-
-    if (isRobotPidAtTarget()) {
-        sendActionSuccess(id);
-    } else {
-        // Run on next loop
-        timerDelay(1, [id](){ sendPacketOnPidComplete(id); });
-    }
-}
-
-// Reads in the light value of all light sensors
-void readLight(int loopDelayMs) {
-    //if we're still waiting to read, can't enter this statement. Kind of like a mutex lock
-    if(!waitingForLight)
-    {
-        waitingForLight = true;
-        // The Infrared Blaster must be activated first to get a clear reading
-        activateIR();
-        //this way, we can pass in a parameter to timerDelay as well, but we don't have to
-        timerDelay(loopDelayMs, std::bind(startLightReading, onFirstTile, &waitingForLight));
-    }
-}
-
-void resetSpeed()
-{
-    profileA.targetVelocity = 0;
-    profileB.targetVelocity = 0;
-}
-
-// Test motor and encoder synchronization
-void startMotorAndEncoderTest() {
-    (new MotorEncoderTest())->startMotorAndEncoderTest();
-}
-
-// Tests the motors. This turns the motors on.
-void startDriveTest() {
-    drive(0.5f, 0.5f, "NULL");
-    timerDelay(2000, &driveTestOff);
-}
-
-//updates us to the next distance we're traveling
-void updateToNextDistance()
-{
-    //this way if we're reversing, we're actually subtracting. if going forward was 0, then 0 * 2 - 1 = -1.
-    //if going forward was 1, 1 * 2 - 1 = 1.
-    float encTargetA = (float)profileA.currentPosition + 2500 * (forwardAligning * 2 - 1);
-    float encTargetB = (float)profileB.currentPosition + 2500 * (forwardAligning * 2 - 1);
-    setLeftMotorControl({POSITION, encTargetA});
-    setRightMotorControl({POSITION, encTargetB});
-#if LOGGING_LEVEL >= 3
-    serialLogln("changing direction!", 3);
-    serialLogln(encTargetA, 3);
-    serialLogln(encTargetB, 3);
-#endif
-
-    //update crit range yessir
-    updateCritRange();
-}
-
-//creates a new drive until the next tile
-void createDriveUntilNewTile()
-{
-    //set the next distance to travel
-    resetSpeed();
-    updateToNextDistance();
-    
-    //assign values here, will detect when they change
-    if(forwardAligning)
-    {
-        firstEncoderIndex = Top_Left_Encoder_Index;
-        secondEncoderIndex = Top_Right_Encoder_Index;
-    }
-    else
-    {
-        //from the robots perspective as it's going backwards, the encoder on the left is bottom right, 
-        //while the encoder on the right is bottom left, so we just swap em
-        firstEncoderIndex = Bottom_Right_Encoder_Index;
-        secondEncoderIndex = Bottom_Left_Encoder_Index;
-    }
-    //now set the actual values with the indices
-    firstEncoderVal = onFirstTile[firstEncoderIndex];
-    secondEncoderVal = onFirstTile[secondEncoderIndex];
-}
-
-//drives until the first two encoders are considered to hit a new value.
-//Returns a status bit, with the corresponding values:
-    // 1 - drive is going
-    // 2 - just at this moment, we have reached our destination and are going to begin reversing now.
-    // 3 - just at this moment, we've reached our destination, but DON'T need to reverse.
-uint8_t driveUntilNewTile() 
-{
-    //if we do finish moving, update to a new distance. I think we have to move at intervals for it to work
-    if(checkIfCanUpdateMovement())
-    {
-        updateToNextDistance();
-    }
-    
-    //if we already changed it, don't change it back again
-    leftEncoderChange = leftEncoderChange || (onFirstTile[firstEncoderIndex] != firstEncoderVal);
-    rightEncoderChange = rightEncoderChange || (onFirstTile[secondEncoderIndex] != secondEncoderVal);
-
-    //if either has changed
-    if(leftEncoderChange || rightEncoderChange)
-    {
-        //when both cross, we done.
-        if(leftEncoderChange && rightEncoderChange)
-        {
-            //if there had been a leading encoder, do the following:
-            uint8_t status = 0; 
-            if(leadingEncoder != 0)
-            {
-                setLeftMotorControl({POSITION, (float)profileA.currentPosition});
-                setRightMotorControl({POSITION, (float)profileB.currentPosition});
-                
-                //first get the encoder that we're comparing to get the distance. If leading encoder is 1 and we were moving forward, or if
-                //leading encoder is 2 but we were moving backward, then the back encoder is b, otherwise it's A
-                double encoderChosen = (leadingEncoder == 1 && forwardAligning || leadingEncoder == 2 && !forwardAligning) ? profileB.currentPosition : profileA.currentPosition;
-                //now, the distance we traveled is the difference between the encoders
-                double backEncoderDist = fabs(encoderChosen - backPrevDistance);
-#if LOGGING_LEVEL >= 3
-                serialLog("Begin encoder is: ", 2);
-                serialLogln(backPrevDistance, 2);
-                serialLog("End encoder is: ", 2);
-                serialLogln((leadingEncoder == 1) ? profileA.currentPosition : profileB.currentPosition, 2);
-#endif
-                //now convert difference in ticks to meter value
-                backEncoderDist *= TICK_TO_METERS;
-#if LOGGING_LEVEL >= 3
-                serialLog(" Encoder in front is gonna be: ", 2);
-                serialLogln(leadingEncoder, 2);
-                serialLog("Total distance encoder was behind is: ", 2);
-                serialLogln(backEncoderDist, 2);
-#endif
-                //reminder: angle = arctan(x/y), where x = backEncoderDist (like distance to our edge), and y = distance between two light sensors (put in manually)
-
-                //idk why, but when dividing by 2 that gets us the true angle. The BackEncoderDist and lightDist seems to be correct vales, but the value we
-                //end up getting from it is double what it should be. Maybe there's an issue with how I did it.
-                float radAngle = atan(backEncoderDist / lightDist) / 2;
-
-                //as a reminder, corresponding degrees = (pi/180) * x radians
-                float degreesAngle = 180 / M_PI * radAngle;
-
-                serialLog("Angle is going to be: ", 2);
-                serialLog(degreesAngle, 2);
-                serialLogln(" degrees.", 2);
-                
-                //we know positive angle = turn left, negative angle means turn right.
-                //encoder 1 forward means turn left, encoder 2 forward means turn right.
-                //so 2 = -1, 1 = 1. If you plug those numbers in, that's what you get.
-
-                int8_t degreesDirection = 3 - 2 * leadingEncoder;
-                serialLog("Sign is going to be: ", 2);
-                serialLogln(degreesDirection, 2);
-
-                //now update the angle, and turn
-                angle = degreesAngle * degreesDirection;
-                testTurn();
-                status = 2;
-            }
-            else
-            {
-                status = 3;
-
-                //with the other one we set the crit range after calling the turn function.
-                //Here we don't, so we gotta do that
-                updateCritRange();
-            }
-
-            //reset all of our values
-            leadingEncoder = 0;
-            backPrevDistance = 0;
-            leftEncoderChange = false;
-            rightEncoderChange = false;
-
-            //if the encoder returned 0, no need to reverse so skip to status 3.
-            return status;
-        }
-        //otherwise, want to see which one crossed. Can do this by seeing if we've updated backPrevDistance yet or not.
-        else if(backPrevDistance == 0)
-        {
-            //if left encoder changed, that's the leading encoder, otherwise 2nd is the leading encoder obviously.
-            leadingEncoder = leftEncoderChange ? 1 : 2;
-            //this makes sense as if the left encoder first crossed but we're going backwards, that's encoder A that's behind. Meanwhile if the right
-            //encoder crossed first and we're going forwards, its encoder A that's behind too. In all other cases, its encoder B
-            backPrevDistance = (leftEncoderChange && !forwardAligning || rightEncoderChange && forwardAligning) ? profileA.currentPosition : profileB.currentPosition;            
-        }
-    }
-    return 1;
-}
-
-// Tests the motors. This turns the motors off.
-void driveTestOff() {
-    stop();
-    timerDelay(2000, &startDriveTest);
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/driveTest.cpp b/src/robot/driveTest.cpp
deleted file mode 100644
index 35f6115..0000000
--- a/src/robot/driveTest.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-#ifndef CHESSBOT_DRIVETEST_CPP
-#define CHESSBOT_DRIVETEST_CPP
-
-#include "robot/driveTest.h"
-#include "Arduino.h"
-#include "robot/motor.h"
-#include "robot/encoder.h"
-#include "utils/timer.h"
-#include "utils/logging.h"
-
-void MotorEncoderTest::init()
-{
-    serialLogln("Starting Motor & Encoder Test", 2);
-
-    this->testSuccessful = true;
-
-    this->startEncoderA = readLeftEncoder();
-    this->_startEncoderB = readRightEncoder();
-
-    setLeftPower(1);
-    setRightPower(0);
-    auto fp = std::bind(&MotorEncoderTest::testLeftMotor, this);
-    timerDelay(1000, fp);
-}
-
-void MotorEncoderTest::testLeftMotor()
-{
-    int encoderAValue = readLeftEncoder();
-    if (encoderAValue > (this->startEncoderA + this->ENCODER_TOLERANCE))
-    {
-        serialLogln("Encoder A (lbl RightEncoder) is aligned with left motor (lbl M1). (Positive motor = positive encoder = CW from Motor POV)", 2);
-    }
-    else if (encoderAValue < (this->startEncoderA - this->ENCODER_TOLERANCE))
-    {
-        serialLogln("[WARN] Encoder A (lbl RightEncoder) is inverted with left motor (lbl M1)!", 2);
-        this->testSuccessful = false;
-    }
-    else
-    {
-        serialLogln("[ERR] Encoder A (lbl RightEncoder) is not associated with left motor (lbl M1)!", 2);
-        this->testSuccessful = false;
-    }
-    setLeftPower(0);
-    setRightPower(0);
-    auto fp = std::bind(&MotorEncoderTest::testWait, this);
-    timerDelay(2000, fp);
-}
-
-void MotorEncoderTest::testWait()
-{
-    this->startEncoderB = readRightEncoder();
-
-    setLeftPower(0);
-    setRightPower(1);
-    auto fp = std::bind(&MotorEncoderTest::testRightMotor, this);
-    timerDelay(1000, fp);
-}
-
-void MotorEncoderTest::testRightMotor()
-{
-    int encoderBValue = readRightEncoder();
-    if (encoderBValue > (this->startEncoderB + this->ENCODER_TOLERANCE))
-    {
-        serialLogln("Encoder B (lbl LeftEncoder) is aligned with right motor (lbl M2). (Positive motor = positive encoder = CCW from Motor POV)", 2);
-    }
-    else if (encoderBValue < (this->startEncoderB - this->ENCODER_TOLERANCE))
-    {
-        serialLogln("[WARN] Encoder B (lbl LeftEncoder) is inverted with right motor (lbl M2)!", 2);
-        this->testSuccessful = false;
-    }
-    else
-    {
-        serialLogln("[ERR] Encoder B (lbl LeftEncoder) is not associated with right motor (lbl M2)!", 2);
-        this->testSuccessful = false;
-    }
-    setRightPower(0);
-    if (this->testSuccessful)
-    {
-        serialLogln("Test successful!", 2);
-        serialLogln("To go forward, both left and right should be set positive.", 2);
-        auto fp = std::bind(&MotorEncoderTest::testMotorDeadzones, this);
-        timerDelay(2000, fp);
-    }
-    else
-    {
-        int encoderAValue = readLeftEncoder();
-        if ((this->startEncoderA - this->ENCODER_TOLERANCE) <= encoderAValue && encoderAValue <= (this->startEncoderA + this->ENCODER_TOLERANCE)) {
-            serialLogln("[WARN] Encoder A (lbl RightEncoder) did not significantly change over the course of the test!", 2);
-        }
-        if ((this->_startEncoderB - this->ENCODER_TOLERANCE) <= encoderBValue && encoderBValue <= (this->_startEncoderB + this->ENCODER_TOLERANCE))
-        {
-            serialLogln("[WARN] Encoder B (lbl LeftEncoder) did not significantly change over the course of the test!", 2);
-        }
-        serialLogln("Test failed for one or more reasons, listed above.", 2);
-        if (((this->startEncoderA - this->ENCODER_TOLERANCE) > encoderAValue || encoderAValue > (this->startEncoderA + this->ENCODER_TOLERANCE))
-         && ((this->_startEncoderB - this->ENCODER_TOLERANCE) > encoderBValue || encoderBValue > (this->_startEncoderB + this->ENCODER_TOLERANCE))) {
-            serialLogln("Encoder A and B changed but did not have the correct associations. Maybe swap the motors or the encoders?", 2);
-        }
-    }
-}
-
-void MotorEncoderTest::checkMotorDeadzone(bool leftMotor)
-{
-    serialLogln(leftMotor ? "Checking LEFT motor deadzone..." : "Checking RIGHT motor deadzone...", 2);
-    const float powerStep = 0.01f;
-    const float maxPower = 1.0f;
-    const int encoderThreshold = 200; // Minimum encoder ticks to consider as movement
-    int initialEncoder = leftMotor ? readLeftEncoder() : readRightEncoder();
-
-    for (float power = 0.0f; power <= maxPower; power += powerStep)
-    {
-        if (leftMotor)
-        {
-            setLeftPower(power);
-            setRightPower(0);
-        }
-        else
-        {
-            setLeftPower(0);
-            setRightPower(power);
-        }
-        delay(200); // Wait for motor to respond
-
-        int currentEncoder = leftMotor ? readLeftEncoder() : readRightEncoder();
-        if (abs(currentEncoder - initialEncoder) >= encoderThreshold)
-        {
-            serialLog("Deadzone crossed at power: ", 2);
-            serialLogln(power, 2);
-            break;
-        }
-    }
-    setLeftPower(0);
-    setRightPower(0);
-}
-
-void MotorEncoderTest::testMotorDeadzones()
-{
-    delay(500);
-    checkMotorDeadzone(true);  // Check left motor
-    delay(500);
-    checkMotorDeadzone(false); // Check right motor
-    serialLogln("Deadzone test complete!", 2);
-    auto fp = std::bind(&MotorEncoderTest::testDriveForward, this);
-    timerDelay(1000, fp);
-}
-
-void MotorEncoderTest::checkEncoderVelocity()
-{
-    int encA = readLeftEncoder();
-    int encB = readRightEncoder();
-    float encAVel = (encA - prevEncA) / (float) 0.02;
-    float encBVel = (encB - prevEncB) / (float) 0.02;
-    float encAAccel = (encAVel - prevEncVelA); // Am I miscalculating acceleration? Should be change in velocity over change in time, but I'm not dividing by time here
-    float encBAccel = (encBVel - prevEncVelB);
-    if (abs(encAVel) > maxEncoderVelocity) maxEncoderVelocity = abs(encAVel);
-    if (abs(encBVel) > maxEncoderVelocity) maxEncoderVelocity = abs(encBVel);
-    if (abs(encAAccel) > maxEncoderAccel) maxEncoderAccel = abs(encAAccel);
-    if (abs(encBAccel) > maxEncoderAccel) maxEncoderAccel = abs(encBAccel);
-    prevEncA = encA;
-    prevEncB = encB;
-    prevEncVelA = encAVel;
-    prevEncVelB = encBVel;
-}
-
-void MotorEncoderTest::testDriveForward()
-{
-    prevEncA = readLeftEncoder();
-    prevEncB = readRightEncoder();
-    prevEncVelA = 0;
-    prevEncVelB = 0;
-    serialLogln("Setting motors to go 'forward' for 5 seconds...", 2);
-    setLeftPower(1);
-    setRightPower(1);
-    auto fp2 = std::bind(&MotorEncoderTest::checkEncoderVelocity, this);
-    encoderCheckTimerId = timerInterval(20, fp2);
-    auto fp = std::bind(&MotorEncoderTest::testDriveDone, this);
-    timerDelay(5000, fp);
-}
-
-void MotorEncoderTest::testDriveDone()
-{
-    serialLogln("Driving done!", 2);
-    serialLog("Max encoder velocity is ", 2);
-    serialLogln(maxEncoderVelocity, 2);
-    serialLog("Max encoder acceleration is ", 2);
-    serialLogln(maxEncoderAccel, 2);
-    timerCancel(encoderCheckTimerId);
-    setLeftPower(0);
-    setRightPower(0);
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/encoder.cpp b/src/robot/encoder.cpp
deleted file mode 100644
index fa46458..0000000
--- a/src/robot/encoder.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#ifndef CHESSBOT_ENCODER_CPP
-#define CHESSBOT_ENCODER_CPP
-
-#include "Encoder.h"
-#include "robot/encoder.h"
-#include "utils/config.h"
-#include "utils/logging.h"
-
-Encoder EncoderA(ENCODER_A_PIN1, ENCODER_A_PIN2); // right
-Encoder EncoderB(ENCODER_B_PIN1, ENCODER_B_PIN2); // left
-int oldPositionA = -999;
-int oldPositionB = -999;
-
-
-void setupEncodersNew() {
-    serialLogln("Setting up encoder", 3);
-}
-
-void resetEncoders() {
-    EncoderA.write(0);
-    EncoderB.write(0);
-}
-
-void encoderLoop() {
-    int newPosition_EncA = readLeftEncoder();
-    int newPosition_EncB = readRightEncoder();
-
-    if(oldPositionA != newPosition_EncA){
-        oldPositionA = newPosition_EncA;
-        serialLog("Encoder A: ", 4);
-        serialLogln(newPosition_EncA, 4);
-    }
-
-    if(oldPositionB != newPosition_EncB){
-        oldPositionB = newPosition_EncB;
-        serialLog("Encoder B: ", 4);
-        serialLogln(newPosition_EncB, 4);
-    }
-}
-
-int readLeftEncoder() {
-    // TODO swap encoders and rename debug logs
-    return EncoderA.read();
-}
-
-int readRightEncoder() {
-    return -EncoderB.read();
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/lightSensor.cpp b/src/robot/lightSensor.cpp
deleted file mode 100644
index 980165f..0000000
--- a/src/robot/lightSensor.cpp
+++ /dev/null
@@ -1,122 +0,0 @@
-#ifndef CHESSBOT_LIGHT_SENSOR_CPP
-#define CHESSBOT_LIGHT_SENSOR_CPP
-
-// Associated Header File
-#include "robot/lightSensor.h"
-#include "../env.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
-#include "utils/logging.h"
-#include "utils/config.h"
-#include "utils/timer.h"
-
-short lightArray[4];
-float prevTickVals[4] = {-1, -1, -1, -1};
-const uint8_t DIFF_TICK = 2;
-int8_t counter = 0;
-
-const short LIMIT_TO_CHANGE = 600;
-
-// Sets the IR (Infrared) Blaster to be able to output
-void setupIR() {
-    serialLogln("Setting Up Light Sensors...", 2);
-    pinMode(RELAY_IR_LED_PIN, OUTPUT);
-    serialLogln("Light Sensors Setup!", 2);
-}
-
-// Turns on the IR Blaster
-void activateIR() {
-    digitalWrite(RELAY_IR_LED_PIN, HIGH);
-}
-
-// Turns off the IR Blaster
-void deactivateIR() {
-    digitalWrite(RELAY_IR_LED_PIN, LOW);
-}
-
-//detects the light change. returns true if change, returns false otherwise
-bool checkForLightChange(uint8_t i)
-{
-    //if difference too great and we're not at the start where prevTick is -1, declare that the encoder has changed color, and assign cooldown.
-    if(fabs(prevTickVals[i] - lightArray[i]) >= LIMIT_TO_CHANGE && (short) prevTickVals[i] != -1)
-    {
-        serialLog(prevTickVals[i], 3);
-        serialLogln((char*) "", 3);
-        serialLogln((char*) "THE DIFFERENCE IS BIG SO WE HAVE CHANGED COLOR GRAHH", 3);
-        return true;
-    }
-    return false;
-}
-
-//updates counter value for us.
-void updateCounter() {
-    counter++;
-    //on diff tick we check, so if we're now past diff tick we reset
-    if(counter == DIFF_TICK + 1)
-    {
-        counter = 0;
-    }
-}
-
-void startLightReading(bool* onFirstTile, bool* waitingForLight) {
-    //for each tick, check if its previous value is too far from what it was before. If it is, consider that the encoder has changed color.
-    readLightLevels();
-    *waitingForLight = false;
-
-    for(uint8_t i = 0; i < 4; i++)
-    {
-        //if its time to change the tick
-        if(counter == DIFF_TICK)
-        {
-            //only print once
-            if(i == 0)
-            {
-                serialLogln((char*) "Checking tick values!", 4);
-            }
-
-            if(checkForLightChange(i))
-            {
-                //invert value
-                onFirstTile[i] = !onFirstTile[i];
-            }
-            //reset the tick values
-            prevTickVals[i] = 0;
-        }
-        else
-        {
-            //do DIFF_TICK because if we change in say DIFF_TICK = 3, then 0, 1, and 2 are values we can get, so each is divided by 3.
-            prevTickVals[i] += (float) lightArray[i] / (DIFF_TICK);
-        }
-    }
-    //updating counter
-    updateCounter();
-}
-
-void readLightLevels() {
-    // Reads the values on the light sensor pins
-    lightArray[0] = analogRead(PHOTODIODE_A_PIN);
-    lightArray[1] = analogRead(PHOTODIODE_B_PIN);
-    lightArray[2] = analogRead(PHOTODIODE_C_PIN);
-    lightArray[3] = analogRead(PHOTODIODE_D_PIN);
-
-    // Deactivates the IR Blaster after reading to save battery power
-    deactivateIR();
-
-    //Logs the values for debugging purposes. This way, if the logging level doesn't permit it,
-    //we don't ever get to this code as it's not flashed, which saved on time.
-#if LOGGING_LEVEL >= 4
-    serialLog("Light Levels: ", 2);
-    serialLog(lightArray[0], 2);
-    serialLog(" ", 2);
-    serialLog(lightArray[1], 2);
-    serialLog(" ", 2);
-    serialLog(lightArray[2], 2);
-    serialLog(" ", 2);
-    serialLogln(lightArray[3], 2);
-#endif
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/lights.cpp b/src/robot/lights.cpp
new file mode 100644
index 0000000..b575559
--- /dev/null
+++ b/src/robot/lights.cpp
@@ -0,0 +1,78 @@
+#include <Arduino.h>
+
+#include "robot/lights.h"
+
+#include "../../env.h"
+#include "utils/config.h"
+#include "utils/logging.h"
+
+static short LIGHT_RAW_VALUE_CUTOFF = 5000;
+bool is_light_value_on(short value) {
+    return value > LIGHT_RAW_VALUE_CUTOFF;
+}
+
+Light::Light(gpio_num_t _pin) {
+    pin = _pin;
+}
+
+void Light::tick() {
+    bool previous_value = _value;
+    _changed_this_tick = false;
+
+    _raw_value = analogRead(pin);
+    
+    _value = is_light_value_on(_raw_value);
+    _held_value = _value || _held_value;
+    
+    if (_value != previous_value) {
+        _changed_this_tick = true;
+        _last_changed_time = millis();
+    }
+}
+
+short Light::raw_value() {
+    return _raw_value;
+}
+
+bool Light::value() {
+    return is_light_value_on(_raw_value);
+}
+
+bool Light::held_value() {
+    return _held_value;
+}
+
+void Light::reset() {
+    _held_value = false;
+}
+
+bool Light::changed_this_tick() {
+    return _changed_this_tick;
+}
+
+unsigned long Light::last_changed_time() {
+    return _last_changed_time;
+}
+
+bool IR_activated = false;
+
+// Turns on the IR Blaster
+// Does turning on and off the IR potentially take more energy than just leaving it on?
+void activateIR() {
+    if (IR_activated) {
+        return;
+    }
+
+    digitalWrite(RELAY_IR_LED_PIN, HIGH);
+    IR_activated = true;
+}
+
+// Turns off the IR Blaster
+void deactivateIR() {
+    if (!IR_activated) {
+        return;
+    }
+
+    digitalWrite(RELAY_IR_LED_PIN, LOW);
+    IR_activated = false;
+}
\ No newline at end of file
diff --git a/src/robot/motion-controller.cpp b/src/robot/motion-controller.cpp
new file mode 100644
index 0000000..247704d
--- /dev/null
+++ b/src/robot/motion-controller.cpp
@@ -0,0 +1,95 @@
+#include <Arduino.h>
+
+#include <tuple>
+#include "robot/robot.h"
+#include "wifi/connection.h"
+
+#include "utils/config.h"
+#include "utils/geometry.h"
+#include "utils/logging.h"
+
+#include "robot/motion-controller.h"
+ 
+MotionController::MotionController()
+    :   DistVelocityController(0.4, 0.2, 0.1, -5, +5, 0.0),
+        AVelocityController(.3, 0.4, 0.1, -3, +3, 0.0)
+{}
+
+MotionController::MotionPhase MotionController::phase() {
+    double dist_err = robot.position.distance_to(goal_position);
+    double angle_err = robot.rotation - goal_angle;
+
+    if (abs(dist_err) < 8 && abs(angle_err) < .01) {
+        digitalWrite(ONBOARD_LED_PIN, HIGH);
+
+        return ARRIVED;
+    } else if (abs(dist_err) < 3) {
+        digitalWrite(ONBOARD_LED_PIN, LOW);
+
+        return ALIGNING;
+    } else {
+        digitalWrite(ONBOARD_LED_PIN, LOW);
+
+        return TRAVELLING;
+    }
+}
+
+void MotionController::set_goal(Coordinate2D _goal_destination, double _goal_angle, std::optional<std::string> id) {
+    actionID = id;
+    goal_angle = _goal_angle;
+    goal_position = _goal_destination;
+}
+
+void MotionController::tick(uint32_t delta) {
+    double dist_err = robot.position.distance_to(goal_position);
+
+    _prev_phase = _phase;
+    _phase = phase();
+
+    if (_phase == ARRIVED) {
+        if (actionID.has_value()) {
+            send_success(actionID.value());
+            actionID = std::nullopt;
+        }
+
+        auto powers = std::make_tuple(0.0, 0.0);
+        robot.drive(powers);
+    } else if (_phase == ALIGNING) {
+        double angular_vel = AVelocityController.Compute(goal_angle, robot.rotation, (double) delta / 1000000);
+       auto powers = std::make_tuple(-angular_vel, angular_vel);
+       robot.drive(powers);
+    } else {
+        if (_prev_phase != TRAVELLING) {
+            DistVelocityController.Reset();
+            AVelocityController.Reset();
+        }
+
+        double temp_goal_angle;
+        if (robot.position.is_behind(robot.rotation, goal_position)) {
+            temp_goal_angle = goal_position.angle_to(robot.position);
+        } else {
+            dist_err = -dist_err;
+            temp_goal_angle = robot.position.angle_to(goal_position);
+        }
+
+        double vel = DistVelocityController.Compute(0, dist_err, (double) delta / 1000000);
+        double angular_vel = AVelocityController.Compute(temp_goal_angle, robot.rotation, (double) delta / 1000000);
+
+        // https://aleksandarhaber.com/tutorial-on-simple-position-controller-for-differential-drive-robot-with-simulation-and-animation-in-python/
+        auto powers = std::make_tuple(
+            (vel / WHEEL_RADIUS_CM) - ((TRACK_WIDTH_CM * angular_vel) / (2 * WHEEL_RADIUS_CM)),
+            (vel / WHEEL_RADIUS_CM) + ((TRACK_WIDTH_CM * angular_vel) / (2 * WHEEL_RADIUS_CM))
+        ); 
+
+        robot.drive(powers);
+    };
+}
+
+void MotionController::print_status() {
+    serial_printf(DebugLevel::DEBUG, "MotionController status: %d\n  goal_angle: %f (%fdeg)\n  goal_position: (%f, %f)", _phase, goal_angle, RAD_TO_DEG *goal_angle, goal_position.x, goal_position.y);
+}
+
+void MotionController::reset() {
+    DistVelocityController.Reset();
+    AVelocityController.Reset();
+}
diff --git a/src/robot/motor.cpp b/src/robot/motor.cpp
index 272c097..4f4484d 100644
--- a/src/robot/motor.cpp
+++ b/src/robot/motor.cpp
@@ -1,67 +1,116 @@
-#ifndef CHESSBOT_MOTOR_CPP
-#define CHESSBOT_MOTOR_CPP
+#include <Arduino.h>
 
-// Associated Header File
 #include "robot/motor.h"
 
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
-#include "utils/logging.h"
 #include "utils/config.h"
-#include "robot/pwm.h"
+#include "utils/functions.h"
+#include "utils/logging.h"
 
-// Sets up all the pins for the motors
-void setupMotors() {
-    analogWriteResolution(12);
-    serialLogln("Setting Up Motors...", 2);
-    setupPWM(MOTOR_A_PIN1);
-    setupPWM(MOTOR_A_PIN2);
-    setupPWM(MOTOR_B_PIN1);
-    setupPWM(MOTOR_B_PIN2);
-    serialLogln("Motors Setup!", 2);
+Motor::Motor(bool _inverted, int motor_pin_a, int motor_pin_b, uint8_t enc_pin_a, uint8_t enc_pin_b)
+    :   inverted(_inverted),
+        speed_controller(1.0, 0.4, 0.0, -1, +1, 0.0),
+        encoder(enc_pin_a, enc_pin_b)
+    {
+    pin_a = motor_pin_a;
+    pin_b = motor_pin_b;
+    
+    pinMode(pin_a, OUTPUT);
+    pinMode(pin_b, OUTPUT);
 }
 
-// This will set how fast and what direction left motor will spin
-// Value between [-1, 1]
-// Negative is backwards, Positive is forwards
-void setLeftPower(float power) {
-    // To spin the motor, you set one of the pins to 0, and the other to a PWM for speed.
-    // The pins you set to each determine the spin direction
-    if (power > 0) {
-        writePWM(MOTOR_A_PIN2, 0);
-        writePWM(MOTOR_A_PIN1, mapPowerToDuty(power));
-    } else {
-        writePWM(MOTOR_A_PIN1, 0);
-        writePWM(MOTOR_A_PIN2, mapPowerToDuty(-power));
+void Motor::tick(uint32_t delta) {
+    // Read from encoder and save its previous value
+    prev_raw_enc_value = raw_enc_value;
+    raw_enc_value = raw_dist();
+
+    double current_speed = _speeds.insert(tick_dist() * 1000000 / delta);
+
+    double pid_power = speed_controller.Compute(_target_speed, current_speed, (double) delta / 1000000);
+    set_power(pid_power);
+}
+
+int Motor::duty() {
+    return power_to_duty(_power);
+}
+
+// Returns the current power of the motor
+double Motor::power() {
+    if (inverted) {
+        return -_power;
     }
+    
+    return _power;
+}
 
-    // Logs the power for debugging purposes
-    serialLog("Left Power: ", 4);
-    serialLogln(power, 4);
+double Motor::speed() {
+    return _speeds.average();
 }
 
-// This will set how fast and what direction right motor will spin
-// Value between [-1, 1]
-// Negative is backwards, Positive is forwards
-void setRightPower(float power) {
-    // To spin the motor, you set one of the pins to 0, and the other to a PWM for speed.
-    // The pins you set to each determine the spin direction
-
-    // Invert power so sending (1,1) to drive results in robot moving forward
-    power *= -1;
-    if (power > 0) {
-        writePWM(MOTOR_B_PIN2, 0);
-        writePWM(MOTOR_B_PIN1, mapPowerToDuty(power));
+
+void Motor::set_power(double __power) {
+    if (inverted) {
+        __power = -__power;    
+    }
+
+    if (__power > 1.0) {
+        __power = 1.0;
+    }
+
+    _power = __power;
+    
+    analogWriteResolution(12);
+    if (_power > 0) {
+        analogWrite(pin_a, power_to_duty(_power));
+        analogWrite(pin_b, 0);
     } else {
-        writePWM(MOTOR_B_PIN1, 0);
-        writePWM(MOTOR_B_PIN2, mapPowerToDuty(-power));
+        analogWrite(pin_a, 0);
+        analogWrite(pin_b, power_to_duty(_power));
     }
+}
+
+double Motor::target_speed() {
+    return _target_speed;
+}
+
+void Motor::set_target_speed(double target) {
+    _target_speed = target;
+}
+
+void Motor::encoder_reset() {
+    encoder.readAndReset();
+}
+
+int32_t Motor::raw_dist() {
+    if (inverted) {
+        return -encoder.read();
+    }
+    
+    return encoder.read();
+}
+
+double Motor::dist() {
+    return ((double)raw_enc_value / TICKS_PER_ROTATION) * TIRE_CIRCUMFERENCE;
+}
+
+double Motor::tick_dist() {
+    int32_t dist = raw_enc_value - prev_raw_enc_value;
+    return ((double)dist / TICKS_PER_ROTATION) * TIRE_CIRCUMFERENCE;
+}
+
+double Motor::get_saved() {
+    return saved_dist;
+}
+
+double Motor::save_dist() {
+    double old = saved_dist;
+    saved_dist = dist();
 
-    // Logs the power for debugging purposes
-    serialLog("Right Power: ", 4);
-    serialLogln(power, 4);
+    return old;
 }
 
-#endif
\ No newline at end of file
+// Takes in a power between [0, 1]
+// We use this to change a double power between 0-1 to an int duty cycle between 0-4096
+int power_to_duty(double power) {
+    power = abs(power);
+    return fmap(power, 0.0, 1.0, 0.0, 4096.0);
+}
\ No newline at end of file
diff --git a/src/robot/pidController.cpp b/src/robot/pid.cpp
similarity index 72%
rename from src/robot/pidController.cpp
rename to src/robot/pid.cpp
index 2c74fba..aa761ab 100644
--- a/src/robot/pidController.cpp
+++ b/src/robot/pid.cpp
@@ -1,17 +1,25 @@
-#ifndef PID_CONTROLLER_CPP
-#define PID_CONTROLLER_CPP
-
-#include "robot/pidController.h"
-#include "utils/logging.h"
 #include <iostream>
 #include <cmath>
 #include <algorithm>
 
+#include "robot/pid.h"
+
+#include "utils/logging.h"
+
+
+PIDController::PIDController(double kp, double ki, double kd, double min, double max, double _errorTolerance)
+    : kp(kp), ki(ki), kd(kd),
+      minOutput(min), maxOutput(max),
+      prev_error(0),
+      errorTolerance(_errorTolerance),
+      integral(0)
+{}
+
 double PIDController::Compute(double setpoint, double actual_value, double dt) {
     // Calculate error
-    double error = setpoint - actual_value;
+    double error = this->getError(setpoint, actual_value);
 
-    if (abs(error) < 100) {
+    if (abs(error) < errorTolerance) {
         return 0;
     }
 
@@ -40,18 +48,9 @@ double PIDController::Compute(double setpoint, double actual_value, double dt) {
     // Clamp output
     output = std::max(minOutput, std::min(maxOutput, output));
 
-    // serialLog("PID is outputting: ", 3);
-    // serialLog(float(output), 3);
-    // serialLogln(",", 3);
-    // if (abs(output) < 0.2) {
-    //     output = 0;
-    // }
-
     return (output);
 }
 
 void PIDController::Reset(){
     integral = 0;
-}
-
-#endif
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/robot/pwm.cpp b/src/robot/pwm.cpp
deleted file mode 100644
index 768e789..0000000
--- a/src/robot/pwm.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef CHESSBOT_PWM_CPP
-#define CHESSBOT_PWM_CPP
-
-// Associated Header File
-#include "robot/pwm.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
-#include "utils/logging.h"
-#include "utils/config.h"
-#include "utils/functions.h"
-
-// Sets a pin to be able to output
-void setupPWM(int pin) {
-    pinMode(pin, OUTPUT);
-}
-
-// Writes PWM to a pin. The duty cycle is a number
-// between 0 and 255 that determines how often the signal is HIGH.
-// You can refer to this link for more information on PWM
-// https://learn.sparkfun.com/tutorials/pulse-width-modulation/all
-void writePWM(int pin, int dutyCycle) {
-    analogWrite(pin, dutyCycle);
-}
-
-// Takes in a power between [0, 1]
-// We use this to change a float power between 0-1 to an int duty cycle between 0-4096
-int mapPowerToDuty(float power) {
-    int value = fmap(power, 0.0, 1.0, 0.0, 4096);
-    serialLog("Mapped Duty: ", 4);
-    serialLogln(value, 4);
-    return value;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/robot.cpp b/src/robot/robot.cpp
new file mode 100644
index 0000000..75b82ab
--- /dev/null
+++ b/src/robot/robot.cpp
@@ -0,0 +1,282 @@
+#include <Arduino.h>
+
+#include <queue>
+#include <tuple>
+#include <math.h>
+#include <algorithm>
+#include <optional>
+
+#include "robot/robot.h"
+
+#include "../../env.h"
+#include "robot/lights.h"
+#include "robot/motor.h"
+#include "robot/pid.h"
+#include "utils/config.h"
+#include "utils/functions.h"
+#include "utils/geometry.h"
+#include "utils/logging.h"
+#include "utils/logging.h"
+#include "wifi/connection.h"
+
+Robot robot = Robot();
+
+Robot::Robot()
+    :   left(false, MOTOR_A_PIN1, MOTOR_A_PIN2, ENCODER_A_PIN1, ENCODER_A_PIN2),
+        right(true, MOTOR_B_PIN1, MOTOR_B_PIN2, ENCODER_B_PIN1, ENCODER_B_PIN2),
+
+        front_left_light(PHOTODIODE_A_PIN),
+        front_right_light(PHOTODIODE_B_PIN),
+        back_left_light(PHOTODIODE_C_PIN),
+        back_right_light(PHOTODIODE_D_PIN),
+        drive_mode(MOTION_CONTROL)
+
+
+{
+
+    // Enable ESP light
+    pinMode(RELAY_IR_LED_PIN, OUTPUT);
+    pinMode(ONBOARD_LED_PIN, OUTPUT);
+}
+
+void Robot::print_status(uint32_t delay) {
+    activateIR();
+
+    uint32_t fps = delay == 0 ? 0 : 1000000 / delay;
+    serial_printf(
+        DebugLevel::DEBUG,
+        
+        SERIAL_CLEAR
+        "FPS: %lu (%luus) Voltage: %dmV\n"
+        "WiFi status: %d Connected: %d\n"
+
+        "Position: (%fcm, %fcm)\n"
+        "Rotation: %frad (%fdeg)\n"
+        "Drive mode: %d Centering status: %d\n"
+
+        "\n"
+
+        "Motors:\n"
+        "  Left:\n"
+        "    power: %+.2f (%d duty)\n"
+        "    speed: %+.2fcm/s\n"
+        "    target speed: %+.2fcm/s\n"
+        "    distance: %+.2fcm (%d raw)\n"
+        "    saved: %+.2fcm\n"
+        "  Right:\n"
+        "    power: %+.2f (%d duty)\n"
+        "    speed: %+.2fcm/s\n"
+        "    target speed: %+.2fcm/s\n"
+        "    distance: %+.2fcm (%d raw)\n"
+        "    saved: %+.2fcm\n"
+
+        "\n"
+
+        "Lights:\n"
+        "  Front:\n"
+        "    Left: %hd (disc %d), (held %d) (changed %lu)\n"
+        "    Right: %hd (disc %d), (held %d) (changed %lu)\n"
+        "  Back:\n"
+        "    Left: %hd (disc %d), (held %d) (changed %lu)\n"
+        "    Right: %hd (disc %d), (held %d) (changed %lu)\n\n",
+
+        fps, delay, Robot::batteryLevel(),
+        WiFi.status(), client.connected(),
+
+        position.x, position.y, rotation, RAD_TO_DEG * rotation,
+        drive_mode, centeringStatus,
+
+        left.power(), left.duty(), left.speed(), left.target_speed(), left.dist(), left.raw_dist(), left.get_saved(),
+        right.power(), right.duty(), right.speed(), right.target_speed(), right.dist(), right.raw_dist(), right.get_saved(),
+
+        front_left_light.raw_value(), front_left_light.value(), front_left_light.held_value(), front_left_light.last_changed_time(),
+        front_right_light.raw_value(), front_right_light.value(), front_right_light.held_value(), front_right_light.last_changed_time(),
+
+        back_left_light.raw_value(), back_left_light.value(), back_left_light.held_value(), back_left_light.last_changed_time(),
+        back_right_light.raw_value(), back_right_light.value(), back_right_light.held_value(), back_right_light.last_changed_time()
+    );
+
+    motion_controller.print_status();
+
+}
+
+int Robot::batteryLevel() {
+    return analogRead(BATTERY_VOLTAGE_PIN) + BATTERY_VOLTAGE_OFFSET;
+}
+
+MotionController::MotionPhase Robot::motion_status() {
+    return motion_controller.phase();
+}
+
+void Robot::tick(uint32_t frame, uint32_t delta) {
+    // Pass through tick, update all sensors / motors
+    left.tick(delta);
+    right.tick(delta);
+
+    activateIR();
+        delay(10);
+        front_left_light.tick();
+        front_right_light.tick();
+        back_left_light.tick();
+        back_right_light.tick();
+    deactivateIR();
+
+    // Calculate new position and rotation
+    double distance_sum = right.tick_dist() + left.tick_dist();
+    double distance_offset = right.tick_dist() - left.tick_dist();
+
+    double d_angle = (distance_offset)/TRACK_WIDTH_CM;
+
+    // != on a double is fine here, because the robot may not move at all.
+    if(d_angle != 0){
+        double temp = (TRACK_WIDTH_CM * distance_sum)/(2*(distance_offset));
+        Coordinate2D delta(sin(rotation + d_angle) - sin(rotation), cos(rotation) - cos(rotation + d_angle));
+        position = position.transform(delta.scale(temp));
+    } else {
+        double totalDist = distance_sum / 2;
+        Coordinate2D delta(cos(rotation + d_angle), sin(rotation + d_angle));
+        position = position.transform(delta.scale(totalDist));
+    }
+
+    rotation = rotation + d_angle;
+
+    center_tick(delta);
+
+    if (drive_mode == DriveType::MANUAL) {
+        // Don't do anything, motors are being manually controlled somewhere else
+    } else if (drive_mode == DriveType::STOPPED) {
+        auto motor_speeds = std::make_tuple(0.0, 0.0);
+        drive(motor_speeds);
+    } else if (drive_mode == DriveType::MOTION_CONTROL) {
+        motion_controller.tick(delta);
+    }
+    
+    if (frame % 32 == 0) {
+        print_status(delta);
+    }
+}
+
+#define CENTER_MOTOR_SPEED .5
+#define LIGHT_DISTANCE 17 // In CM
+#define BACKUP_DIST 20.0
+void Robot::center_tick(uint32_t delay) {
+    if (centeringStatus == NOT_CENTERING) {
+        return;
+    }
+
+    if (centeringStatus == STARTED) {
+        drive_mode = DriveType::MANUAL;
+
+        auto motor_speeds = std::make_tuple(CENTER_MOTOR_SPEED, CENTER_MOTOR_SPEED);
+        drive(motor_speeds);
+
+        if (front_left_light.held_value() || front_right_light.held_value()) {
+            left.save_dist();
+            right.save_dist();
+
+            centeringStatus = ALIGNING_EDGE_1;
+        }
+    }
+
+    if (centeringStatus == ALIGNING_EDGE_1) {
+        drive_mode = DriveType::MANUAL;
+
+        auto motor_speeds = std::make_tuple(CENTER_MOTOR_SPEED, CENTER_MOTOR_SPEED);
+        drive(motor_speeds);
+        double delta_dist;
+
+        if (front_left_light.held_value() && front_right_light.held_value()) {
+            if (front_left_light.last_changed_time() > front_right_light.last_changed_time()) {
+                delta_dist = left.dist() - left.save_dist();
+                rotation = atan(delta_dist / LIGHT_DISTANCE);
+            } else {
+                delta_dist = right.dist() - right.save_dist();
+                rotation = -atan(delta_dist / LIGHT_DISTANCE);
+            }
+
+            serial_printf(DebugLevel::INFO, "ROTATION: %fdeg, delta_dist: %f", rotation * RAD_TO_DEG, delta_dist);
+            position.y = BACKUP_DIST;
+            rotation = rotation + (M_PI /  2);
+
+            centeringStatus = ALIGNED_EDGE_1;
+        }
+    }
+
+    if (centeringStatus == ALIGNED_EDGE_1) {
+        drive_mode = DriveType::MOTION_CONTROL;
+
+        motion_controller.set_goal(Coordinate2D(position.x, 0.0), 0, std::nullopt);
+
+        if (motion_controller.phase() == MotionController::MotionPhase::ARRIVED) {
+            front_left_light.reset();
+            front_right_light.reset();
+            
+            motion_controller.set_goal(Coordinate2D(position.x + 100.0, 0.0), 0, std::nullopt);
+            centeringStatus = CENTERED_Y_AXIS;
+        }
+    }
+
+    // Similar to the STARTING status
+    if (centeringStatus == CENTERED_Y_AXIS) {
+        drive_mode = DriveType::MOTION_CONTROL;
+
+        if (front_left_light.held_value() && front_right_light.held_value()) {
+            position.x = BACKUP_DIST;
+ 
+            motion_controller.set_goal(Coordinate2D(0.0, 0.0), M_PI / 2, centeringID);
+            centeringID = std::nullopt;
+            centeringStatus = NOT_CENTERING;
+        }
+    }
+}
+
+void Robot::center(std::optional<std::string> id) {
+    if (centeringStatus == NOT_CENTERING) {
+        centeringStatus = STARTED;
+        centeringID = id;
+
+        drive_mode = DriveType::MANUAL;
+
+        front_left_light.reset();
+        front_right_light.reset();
+        back_left_light.reset();
+        back_right_light.reset();
+    }
+}
+
+void Robot::drive(Coordinate2D goal_pos, double goal_angle) {
+    motion_controller.set_goal(goal_pos, goal_angle, std::nullopt);
+}
+
+void Robot::drive(double tiles, std::string id) {
+    const float TILE_SIZE_CM = 24 * 2.54;
+    
+    Coordinate2D offset(rotation);
+    offset = offset.scale(TILE_SIZE_CM * tiles);
+    Coordinate2D destination = position.transform(offset);
+
+    motion_controller.set_goal(destination, rotation, id);
+}
+
+// Drives the wheels according to the powers set. Negative is backwards, Positive forwards
+void Robot::drive(std::tuple<double, double>& powers) {
+    left.set_target_speed(std::get<0>(powers));
+    right.set_target_speed(std::get<1>(powers));
+}
+
+//turns the given amount in radians, CCW
+void Robot::turn(double delta, std::string id) {
+    motion_controller.set_goal(position, rotation + delta, id);
+}
+
+void Robot::start() {
+    drive_mode = DriveType::MOTION_CONTROL;
+    
+    serial_printf(DebugLevel::DEBUG, "Bot Started!\n");
+}
+
+void Robot::stop() {
+    drive_mode = DriveType::STOPPED;
+    
+    serial_printf(DebugLevel::DEBUG, "Bot Stopped!\n");
+}
\ No newline at end of file
diff --git a/src/robot/splines.cpp b/src/robot/splines.cpp
deleted file mode 100644
index 706f4d0..0000000
--- a/src/robot/splines.cpp
+++ /dev/null
@@ -1,253 +0,0 @@
-#ifndef CHESSBOT_SPLINES_CPP
-#define CHESSBOT_SPLINES_CPP
-
-#include "robot/splines.h"
-#include "utils/logging.h"
-#include "robot/control.h"
-#include "robot/trapezoidalProfile.h"
-#include "utils/timer.h"
-#include "wifi/connection.h"
-#include "utils/config.h"
-#include "utils/functions.h"
-#include <tuple>
-#include <queue>
-#include "robot/encoder.h"
-#include <tuple>
-#include <cmath>
-
-
-
-void velocityUpdateTimerFunction(std::string id)
-{
-    // serialLogln("Update Timer was called", 3);
-    if (timeSlicesToExecute.size() == 0) {
-        if (id != "NULL") { sendActionSuccess(id); }
-        return;
-    }
-
-    float desiredVelocityLeft, desiredVelocityRight;
-    std::tie(desiredVelocityLeft, desiredVelocityRight) = timeSlicesToExecute.front();
-    setLeftMotorControl({VELOCITY, desiredVelocityLeft});
-    setRightMotorControl({VELOCITY, desiredVelocityRight});
-    timeSlicesToExecute.pop();
-
-    // 1ms delay ensures the function will run in the next loop
-    timerDelay(1, [id](){ velocityUpdateTimerFunction(id); });
-}
-
-
-void danceMonkeyCubic(std::string id, Point start, Point control1, Point control2, Point end, float totalTimeMs){
-    float trackWidth = TRACK_WIDTH_INCHES;
-
-    int steps = (int)(totalTimeMs / loopDelayMilliseconds); // 20ms steps
-    float dt = totalTimeMs / 1000 / steps;
-
-    for (int i = 0; i < steps; i++) {
-        float t = (float)i / steps;
-        float tNext = (float)(i + 1) / steps;
-
-        // Cubic Bezier Point at t
-        Point p = {
-            (float)(pow(1 - t, 3) * start.x +
-                    3 * pow(1 - t, 2) * t * control1.x +
-                    3 * (1 - t) * pow(t, 2) * control2.x +
-                    pow(t, 3) * end.x),
-            (float)(pow(1 - t, 3) * start.y +
-                    3 * pow(1 - t, 2) * t * control1.y +
-                    3 * (1 - t) * pow(t, 2) * control2.y +
-                    pow(t, 3) * end.y)
-        };
-
-        // Cubic Bezier Point at tNext
-        Point pNext = {
-            (float)(pow(1 - tNext, 3) * start.x +
-                    3 * pow(1 - tNext, 2) * tNext * control1.x +
-                    3 * (1 - tNext) * pow(tNext, 2) * control2.x +
-                    pow(tNext, 3) * end.x),
-            (float)(pow(1 - tNext, 3) * start.y +
-                    3 * pow(1 - tNext, 2) * tNext * control1.y +
-                    3 * (1 - tNext) * pow(tNext, 2) * control2.y +
-                    pow(tNext, 3) * end.y)
-        };
-
-        // Estimate linear velocity
-        float dx = pNext.x - p.x;
-        float dy = pNext.y - p.y;
-        float linearVelocity = sqrt(dx * dx + dy * dy) / dt;
-
-        // Estimate angular velocity
-        float theta1 = atan2(dy, dx);
-        float theta0;
-
-        if (i > 0) {
-            float tPrev = (float)(i - 1) / steps;
-        Point pPrev = {
-                (float)(pow(1 - tPrev, 3) * start.x +
-                        3 * pow(1 - tPrev, 2) * tPrev * control1.x +
-                        3 * (1 - tPrev) * pow(tPrev, 2) * control2.x +
-                        pow(tPrev, 3) * end.x),
-                (float)(pow(1 - tPrev, 3) * start.y +
-                        3 * pow(1 - tPrev, 2) * tPrev * control1.y +
-                        3 * (1 - tPrev) * pow(tPrev, 2) * control2.y +
-                        pow(tPrev, 3) * end.y)
-            };
-
-            theta0 = atan2(p.y - pPrev.y, p.x - pPrev.x);
-        } else {
-            theta0 = theta1;
-        }
-
-        float angularVelocity = (theta1 - theta0) / dt;
-
-        // Normalize angle diff
-        while (angularVelocity > M_PI) angularVelocity -= 2 * M_PI;
-        while (angularVelocity < -M_PI) angularVelocity += 2 * M_PI;
-
-        float trackWidthTicks = trackWidth * TICKS_PER_ROTATION/(WHEEL_DIAMETER_INCHES*M_PI);
-
-        // Compute wheel velocities using differential drive model
-        float vLeft = linearVelocity - (angularVelocity * trackWidthTicks / 2);
-        float vRight = linearVelocity + (angularVelocity * trackWidthTicks / 2);
-
-        timeSlicesToExecute.push({vLeft, vRight});
-    }
-    velocityUpdateTimerFunction(id);
-}
-
-void danceMonkeyQuadratic(std::string id, Point start, Point control, Point end, float totalTimeMs) {
-    serialLogln("Quadratic Dancer was called", 3);
-    float trackWidth = TRACK_WIDTH_INCHES;
-    int steps = (int)(totalTimeMs / loopDelayMilliseconds); // 20ms steps
-    float dt = totalTimeMs / 1000 / steps;
-
-    for (int i = 0; i < steps; i++) {
-        float t = (float)i / steps;
-        float tNext = (float)(i + 1) / steps;
-
-        // Bezier point at t
-        Point p = {
-                (float)((1 - t) * (1 - t) * start.x + 2 * (1 - t) * t * control.x + t * t * end.x),
-                (float)((1 - t) * (1 - t) * start.y + 2 * (1 - t) * t * control.y + t * t * end.y)
-        };
-
-        // Bezier point at t + dt
-        Point pNext = {
-            (float)((1 - tNext) * (1 - tNext) * start.x + 2 * (1 - tNext) * tNext * control.x + tNext * tNext * end.x),
-            (float)((1 - tNext) * (1 - tNext) * start.y + 2 * (1 - tNext) * tNext * control.y + tNext * tNext * end.y)
-        };
-
-        // Velocity approximation
-        float dx = pNext.x - p.x;
-        float dy = pNext.y - p.y;
-        // dx *= 2*12*TICKS_PER_ROTATION/(WHEEL_DIAMETER_INCHES*M_PI);
-        // dy *= 2*12*TICKS_PER_ROTATION/(WHEEL_DIAMETER_INCHES*M_PI);
-        float linearVelocity = sqrt(dx * dx + dy * dy) / dt;
-
-        // Angular velocity approximation
-        float theta1 = atan2(dy, dx);
-        float theta0 = (i > 0) ? atan2(p.y - ((1 - (float)(i - 1) / steps) * (1 - (float)(i - 1) / steps) * start.y +
-            2 * (1 - (float)(i - 1) / steps) * ((float)(i - 1) / steps) * control.y +
-            ((float)(i - 1) / steps) * ((float)(i - 1) / steps) * end.y),
-            p.x - ((1 - (float)(i - 1) / steps) * (1 - (float)(i - 1) / steps) * start.x +
-            2 * (1 - (float)(i - 1) / steps) * ((float)(i - 1) / steps) * control.x +
-            ((float)(i - 1) / steps) * ((float)(i - 1) / steps) * end.x)) : theta1;
-        float angularVelocity = (theta1 - theta0) / dt;
-
-        // Normalize angle diff
-        while (angularVelocity > M_PI) angularVelocity -= 2 * M_PI;
-        while (angularVelocity < -M_PI) angularVelocity += 2 * M_PI;
-
-        float trackWidthTicks = trackWidth * TICKS_PER_ROTATION/(WHEEL_DIAMETER_INCHES*M_PI);
-
-        // Differential drive kinematics
-        float vLeft = linearVelocity - (angularVelocity * trackWidthTicks / 2);
-        float vRight = linearVelocity + (angularVelocity * trackWidthTicks / 2);
-
-
-        timeSlicesToExecute.push({vLeft, vRight});
-        // serialLog(vLeft, 3);
-        // serialLog(", ", 3);
-        // serialLogln(vRight, 3);
-    }
-    velocityUpdateTimerFunction(id);
-}
-
-
-
-int customPrevPositionA = 0, customPrevPositionB = 0;
-
-void startCustomMotionProfileTimer(int leftPositionTarget, int rightPositionTarget, double profileDuration, std::string id)
-{
-    double maxAcceleration = 6000;
-
-    double maxPositionTarget = std::max(std::abs(leftPositionTarget), std::abs(rightPositionTarget));
-    double leftAcceleration = maxAcceleration * std::abs(leftPositionTarget) / maxPositionTarget;
-    double rightAcceleration = maxAcceleration * std::abs(rightPositionTarget) / maxPositionTarget;
-
-    double leftSqrtValue = profileDuration*profileDuration - 4*std::abs(leftPositionTarget)/leftAcceleration;
-    double rightSqrtValue = profileDuration*profileDuration - 4*std::abs(rightPositionTarget)/rightAcceleration;
-
-    if (leftSqrtValue < 0 || rightSqrtValue < 0)
-    {
-        sendActionFail(id);
-        return;
-    }
-
-    double maxVelocityLeft = (profileDuration - sqrt(leftSqrtValue))*leftAcceleration/2;
-    double maxVelocityRight = (profileDuration - sqrt(rightSqrtValue))*rightAcceleration/2;
-
-    customPrevPositionA = readLeftEncoder();
-    customPrevPositionB = readRightEncoder();
-
-    MotionProfile customProfileA = {maxVelocityLeft, leftAcceleration, 0, 0, (double)leftPositionTarget, 0, fabs(leftPositionTarget - customPrevPositionA) / 2};
-    MotionProfile customProfileB = {maxVelocityRight, rightAcceleration, 0, 0, (double)rightPositionTarget, 0, fabs(rightPositionTarget - customPrevPositionB) / 2};
-    serialLogln("Custom motion profile timer...", 3);
-    serialLog("Left max vel: ", 3);
-    serialLog(maxVelocityLeft, 3);
-    serialLog(" Max accel: ", 3);
-    serialLog(leftAcceleration, 3);
-    serialLog(" Target: ", 3);
-    serialLogln(leftPositionTarget, 3);
-    serialLog("Right max vel: ", 3);
-    serialLog(maxVelocityRight, 3);
-    serialLog(" Max accel: ", 3);
-    serialLog(rightAcceleration, 3);
-    serialLog(" Target: ", 3);
-    serialLogln(rightPositionTarget, 3);
-    customMotionProfileTimerFunction(customProfileA, customProfileB, loopDelayMilliseconds / 1000.0, id);
-}
-
-void customMotionProfileTimerFunction(MotionProfile &customProfileA, MotionProfile &customProfileB, double dt, std::string id)
-{
-    if (approxEquals(customProfileA.targetPosition, customProfileA.currentPosition, PID_POSITION_TOLERANCE)
-     && approxEquals(customProfileA.currentVelocity, 0.0, PID_VELOCITY_TOLERANCE)
-     && approxEquals(customProfileB.targetPosition, customProfileB.currentPosition, PID_POSITION_TOLERANCE)
-     && approxEquals(customProfileB.currentVelocity, 0.0, PID_VELOCITY_TOLERANCE))
-    {
-        if (id != "NULL") { sendActionSuccess(id); }
-        return;
-    }
-
-    int currentPositionEncoderA = readLeftEncoder();
-    int currentPositionEncoderB = readRightEncoder();
-    double currentVelocityA = (currentPositionEncoderA - customPrevPositionA) / dt;
-    double currentVelocityB = (currentPositionEncoderB - customPrevPositionB) / dt;
-
-    customPrevPositionA = currentPositionEncoderA;
-    customPrevPositionB = currentPositionEncoderB;
-
-    customProfileA.currentPosition = currentPositionEncoderA;
-    customProfileA.currentVelocity = currentVelocityA;
-    customProfileB.currentPosition = currentPositionEncoderB;
-    customProfileB.currentVelocity = currentVelocityB;
-
-    float desiredVelocityLeft = updateTrapezoidalProfile(customProfileA, dt, 0);
-    float desiredVelocityRight = updateTrapezoidalProfile(customProfileB, dt, 0);
-
-    setLeftMotorControl({VELOCITY, desiredVelocityLeft});
-    setRightMotorControl({VELOCITY, desiredVelocityRight});
-
-    timerDelay(1, [&customProfileA, &customProfileB, dt, id](){ customMotionProfileTimerFunction(customProfileA, customProfileB, dt, id); });
-}
-
-#endif
\ No newline at end of file
diff --git a/src/robot/trapezoidalProfile.cpp b/src/robot/trapezoidalProfile.cpp
deleted file mode 100644
index dbe3e84..0000000
--- a/src/robot/trapezoidalProfile.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-#ifndef CHESSBOT_TRAPEZOIDAL_PROFILE_CPP
-#define CHESSBOT_TRAPEZOIDAL_PROFILE_CPP
-
-#include "robot/trapezoidalProfile.h"
-#include "utils/logging.h"
-#include <iostream>
-#include <cmath>
-#include <algorithm>
-#include <cstdint>
-#include "../env.h"
-
-using namespace std;
-
-double updateTrapezoidalProfile(MotionProfile &profile, double dt, int8_t framesUntilPrint) {
-    // distanceToGo = positive, you're still behind the target. || distanceToGo = negative, you're ahead.
-    double distanceToGo = profile.targetPosition - profile.currentPosition;
-
-    double changeInVelocity = 0;
-    double stoppingDistance;
-
-    //store all the absolute value versions so that we don't waste more computation calculating them when we already have before
-    double absVelocity = fabs(profile.currentVelocity);
-    double absDistanceToGo = fabs(distanceToGo);
-    double absCurrentPos = fabs(profile.currentPosition);
-    double absTargetVelocity = fabs(profile.targetVelocity);
-    double absTargetPos = fabs(profile.targetPosition);
-    
-    // Formula is v^2 / 2a
-    stoppingDistance = (profile.currentVelocity * profile.currentVelocity) / (2.0 * profile.maxAcceleration);
-    
-    // Acceleration phase. If we aren't already at max and we still have distance left to go, add a bit more velocity on.
-    if (absVelocity < profile.maxVelocity && absDistanceToGo > 0)
-        changeInVelocity = profile.maxAcceleration * dt * (distanceToGo > 0 ? 1 : -1);
-
-    // Deceleration if we’re getting close to target. See first if we stop right now, will we overshoot our target? If so, begin decelerating
-    if (absDistanceToGo <= stoppingDistance){
-        float fraction;
-        //if they're the same sign (or one is 0)
-        if(profile.targetPosition * profile.currentPosition >= 0)
-        {
-            //no matter if theyr'e either both positive or both negative, it'll be between 0 and 1. You can verify this to make sure.
-            if(absTargetPos > absCurrentPos)
-            {
-                
-                fraction = profile.currentPosition / profile.targetPosition;
-            }
-            else
-            {
-                fraction = profile.targetPosition / profile.currentPosition;
-            }
-        }
-        //either the target or start position is negative, the other is positive
-        else
-        {
-            //going to have it as target / range. So liike if going from 50 to -150, that'd be 150/200. It's not really right, but it's kinda
-            //finnicky how we should be handling it. Should check with people smarter than me to make sure
-            float range = absTargetPos + absCurrentPos;
-            fraction = profile.targetPosition / range;
-        }
-        //making it dependent on leftover distance/dt instead of maxAcceleration*dt
-        changeInVelocity = -(fabs(fraction)) * profile.maxAcceleration * dt * (profile.currentVelocity > 0 ? 1 : -1);
-
-        //want to make sure that we're not going to be decelerating so hard that we just reverse ourselves and don't reach our target. So if our
-        //change in velocity causes use to reverse direction, just set it so we will change our velocity to 0.
-        if(fabs(changeInVelocity) > absTargetVelocity)
-        {
-            changeInVelocity = profile.targetVelocity * -1;
-        }
-    }
-
-    profile.targetVelocity += changeInVelocity;
-
-    //clip off velocity if it's too high
-    if (absTargetVelocity > profile.maxVelocity)
-        profile.targetVelocity = profile.maxVelocity * (profile.targetVelocity > 0 ? 1 : -1);
-
-    //if we're getting close to our target, may as well just stop it. Note that the reason we have the min function
-    //is because sometimes critRange will be an even smaller number when we're moving very smaller distances (like when
-    //we turn for edge alignment)
-    if(absDistanceToGo < min(profile.criticalRange, 75.0))
-    {
-        profile.targetVelocity = 0;
-    }
-
-    //use macros so that if we're not even logging, we won't even upload the code
-#if LOGGING_LEVEL >= 3
-    if(framesUntilPrint == 0)
-    {
-        serialLog("Change in velocity was: ", 3);
-        serialLog(float(changeInVelocity), 3);
-        serialLog(", ", 3);
-        
-        serialLog("Motion profile is outputting: ", 3);
-        serialLog(float(profile.targetVelocity), 3);
-        serialLog(", ", 3);
-
-        serialLog("Current Position: ", 3);
-        serialLog(float(profile.currentPosition), 3);
-        serialLog(", ", 3);
-
-        serialLog("Target Position: ", 3);
-        serialLogln(float(profile.targetPosition), 3);
-
-        // serialLog("Stopping position: ", 3);
-        // serialLogln(float(stoppingDistance), 3);
-    }
-#endif
-
-    return profile.targetVelocity;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/tests.cpp b/src/tests.cpp
new file mode 100644
index 0000000..ea33d3d
--- /dev/null
+++ b/src/tests.cpp
@@ -0,0 +1,100 @@
+#include <Arduino.h>
+
+#include "tests.h"
+
+#include "robot/robot.h"
+#include "utils/geometry.h"
+#include "utils/logging.h"
+
+// Turns off the motion controller on the robot
+void sleepy_test(Robot& r) {
+    r.stop();
+}
+
+void center_test(Robot& r) {
+    unsigned long time_seconds = millis() / 1000;
+
+    if (time_seconds % 30 == 10) {
+        r.center(std::nullopt);
+    }
+}
+
+// Test the distance PID controller
+void line_test(Robot& r) {
+    unsigned long time_seconds = millis() / 1000;
+
+    Coordinate2D goal;
+    double rotation = 0;
+
+    if (time_seconds > 5) {
+        goal = Coordinate2D(100, 0);
+    }
+
+    if (time_seconds > 15) {
+        goal = Coordinate2D(0, 0);
+    }
+
+    r.drive(goal, rotation);
+}
+
+// Test the angular PID controller
+void circle_test(Robot& r) {
+    unsigned long time_seconds = millis() / 1000;
+
+    Coordinate2D goal(00, 0.0);
+    double rotation = 0;
+
+    if (time_seconds > 5) {
+        rotation = 2 * M_PI;
+    }
+
+    if (time_seconds > 10) {
+        rotation =  0;
+    }
+
+    if (time_seconds > 15) {
+        rotation = 4 * M_PI;
+    }
+
+    if (time_seconds > 20) {
+        rotation = 0;
+    }
+
+    r.drive(goal, rotation);
+}
+
+// Test all of MotionController
+void square_test(Robot& r) {
+    unsigned long time_seconds = millis() / 1000;
+
+    Coordinate2D goal;
+    double rotation;
+
+    if (time_seconds > 5) {
+        goal = Coordinate2D(0, 100);
+        rotation = M_PI / 2;
+    }
+
+    if (time_seconds > 20) {
+        goal = Coordinate2D(0, 0);
+        rotation = 0;
+    }
+
+    if (time_seconds > 35) {
+        goal = Coordinate2D(100, 0);
+    }
+    
+    if (time_seconds > 50) {
+        goal = Coordinate2D(100, 100);
+    }
+
+    if (time_seconds > 65) {
+        goal = Coordinate2D(0, 100);
+    }
+
+    if (time_seconds > 80) {
+        goal = Coordinate2D(0, 0);
+    }
+
+    r.drive(goal, rotation);
+}
\ No newline at end of file
diff --git a/src/utils/config.cpp b/src/utils/config.cpp
index 6049e39..c42080e 100644
--- a/src/utils/config.cpp
+++ b/src/utils/config.cpp
@@ -1,17 +1,12 @@
-#ifndef CHESSBOT_CONFIG_CPP
-#define CHESSBOT_CONFIG_CPP
+#include <Arduino.h>
+#include <ArduinoJson.h>
 
-// Associated Header File
 #include "utils/config.h"
 
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
 #include "utils/logging.h"
 
 // External Libraries
-#include <ArduinoJson.h>
+
 
 int loopDelayMilliseconds = 20;
 
@@ -33,19 +28,28 @@ gpio_num_t PHOTODIODE_D_PIN = GPIO_NUM_6;
 
 gpio_num_t ONBOARD_LED_PIN = GPIO_NUM_15;
 gpio_num_t BATTERY_VOLTAGE_PIN = GPIO_NUM_10;
+int BATTERY_VOLTAGE_OFFSET = 100;
 
 int TICKS_PER_ROTATION = 12000;
 float TRACK_WIDTH_INCHES = 8.29;
+float TRACK_WIDTH_CM = TRACK_WIDTH_INCHES * 2.54;
 float WHEEL_DIAMETER_INCHES = 4.75;
-float MAX_VELOCITY_TPS = 39000;
-float MAX_ACCELERATION_TPSPS = 5000;
+float WHEEL_RADIUS_CM = (WHEEL_DIAMETER_INCHES / 2) * 2.54;
+float THEORETICAL_MAX_VELOCITY_TPS = 52000;
+float THEORETICAL_MAX_ACCELERATION_TPSPS = 252000;
+float VELOCITY_LIMIT_TPS = 30000;
+float ACCELERATION_LIMIT_TPSPS = 125000;
+float MIN_MOTOR_POWER = 0.15; // Minimum motor power to elicit motor response, empirically determined
+float MIN_MOTOR_VELOCITY_TPS = 5000;
 float TILES_TO_TICKS = 2*12*TICKS_PER_ROTATION/(WHEEL_DIAMETER_INCHES*M_PI);
 
+int MAGNET_CCW_IS_POSITIVE = 1; // Set to 1 if counterclockwise rotation is positive, -1 if clockwise rotation is positive
+
 float PID_POSITION_TOLERANCE = 100;
 float PID_VELOCITY_TOLERANCE = 6000;
 
 void setConfig(JsonObject config) {
-    serialLogln("Setting Config...", 2);
+    serial_printf(DebugLevel::DEBUG, "Setting Config...\n");
 
     // The is<x>() method checks the type of the variable. If the type isn't none, then
     // we know this variable exists in the config and has a value
@@ -68,10 +72,12 @@ void setConfig(JsonObject config) {
     if (config["TICKS_PER_ROTATION"].is<int>()) TICKS_PER_ROTATION = config["TICKS_PER_ROTATION"];
     if (config["TRACK_WIDTH_INCHES"].is<float>()) TRACK_WIDTH_INCHES = config["TRACK_WIDTH_INCHES"];
     if (config["WHEEL_DIAMETER_INCHES"].is<float>()) WHEEL_DIAMETER_INCHES = config["WHEEL_DIAMETER_INCHES"];
-    if (config["MAX_VELOCITY_TPS"].is<float>()) MAX_VELOCITY_TPS = config["MAX_VELOCITY_TPS"];
-    if (config["MAX_ACCELERATION_TPSPS"].is<float>()) MAX_ACCELERATION_TPSPS = config["MAX_ACCELERATION_TPSPS"];
+    if (config["THEORETICAL_MAX_VELOCITY_TPS"].is<float>()) THEORETICAL_MAX_VELOCITY_TPS = config["THEORETICAL_MAX_VELOCITY_TPS"];
+    if (config["THEORETICAL_MAX_ACCELERATION_TPSPS"].is<float>()) THEORETICAL_MAX_ACCELERATION_TPSPS = config["THEORETICAL_MAX_ACCELERATION_TPSPS"];
+    if (config["MIN_MOTOR_POWER"].is<float>()) MIN_MOTOR_POWER = config["MIN_MOTOR_POWER"];
+    if (config["MIN_MOTOR_VELOCITY_TPS"].is<float>()) MIN_MOTOR_VELOCITY_TPS = config["MIN_MOTOR_VELOCITY_TPS"];
 
-    serialLogln("Config Set!", 2);
-}
+    if (config["MAGNET_CCW_IS_POSITIVE"].is<int>()) MAGNET_CCW_IS_POSITIVE = config["MAGNET_CCW_IS_POSITIVE"];
 
-#endif
\ No newline at end of file
+    serial_printf(DebugLevel::DEBUG, "Config Set!\n");
+}
\ No newline at end of file
diff --git a/src/utils/functions.cpp b/src/utils/functions.cpp
index 5fe57df..6443c2a 100644
--- a/src/utils/functions.cpp
+++ b/src/utils/functions.cpp
@@ -1,19 +1,13 @@
-#ifndef CHESSBOT_FUNCTIONS_CPP
-#define CHESSBOT_FUNCTIONS_CPP
+#include <Arduino.h>
 
-// Associated Header File
 #include "utils/functions.h"
 
-// Built-In Libraries
-#include "Arduino.h"
-
-// Custom Libraries
-#include "utils/logging.h"
 #include "utils/config.h"
+#include "utils/logging.h"
 
 // This custom map function takes a number, and scales it from one range to another.
 // We use this to change a number between 0-1 to a number between 0-255
-float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
+double fmap(double x, double in_min, double in_max, double out_min, double out_max) {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
@@ -25,21 +19,6 @@ bool approxEquals(double x, double y, double epsilon) {
     return std::abs(x-y) <= epsilon;
 }
 
-int radiansToTicks(double angle) {
-    double offsetInches = TRACK_WIDTH_INCHES * angle / 2;
-    int offsetTicks = (int)(offsetInches / (WHEEL_DIAMETER_INCHES * M_PI) * TICKS_PER_ROTATION);
-    
-    // Add debug logging
-    serialLog("Turn angle (rad): ", 3);
-    serialLog(angle, 3);
-    serialLog(", Offset inches: ", 3);
-    serialLog(offsetInches, 3);
-    serialLog(", Offset ticks: ", 3);
-    serialLogln(offsetTicks, 3);
-    
-    return offsetTicks;
-}
-
 // When we get the mac address from the esp, it gives it as an array of unsigned
 // 8 bit integers. The server is expecting it as a hex string separated by colons
 std::string unint8ArrayToHexString(uint8_t* oldArray, int len) {
@@ -56,8 +35,26 @@ std::string unint8ArrayToHexString(uint8_t* oldArray, int len) {
         result.push_back(hex[oldArray[i] / 16]);
         result.push_back(hex[oldArray[i] % 16]);
     }
-    serialLogln(result, 4);
+
     return result;
 }
 
-#endif
\ No newline at end of file
+RingBuf::RingBuf() {
+    std::fill(buf.begin(), buf.end(), 0);
+}
+
+double RingBuf::insert(double val) {
+    head = head % buf.size();
+    buf[head] = val;
+
+    return average();
+}
+
+double RingBuf::average() {
+    double sum = 0;
+    for (double item : buf) {
+        sum += item;
+    }
+
+    return sum / buf.size();
+}
\ No newline at end of file
diff --git a/src/utils/geometry.cpp b/src/utils/geometry.cpp
new file mode 100644
index 0000000..9e7efb5
--- /dev/null
+++ b/src/utils/geometry.cpp
@@ -0,0 +1,68 @@
+#include <math.h>
+
+#include "utils/geometry.h"
+
+Coordinate2D::Coordinate2D() {
+    x = 0.0;
+    y = 0.0;
+}
+
+Coordinate2D::Coordinate2D(double _x, double _y) {
+    x = _x;
+    y = _y;
+}
+
+Coordinate2D::Coordinate2D(double angle) {
+    x = cos(angle);
+    y = sin(angle);
+}
+
+// Coordinate2D operator+(const Coordinate2D& a, const Coordinate2D& b) { return a.transform(b); }
+Coordinate2D Coordinate2D::transform(const Coordinate2D offset) const {
+    return Coordinate2D(x + offset.x, y + offset.y);
+}
+
+Coordinate2D Coordinate2D::transform(double theta, double distance) const {
+    return Coordinate2D(x + distance * cos(theta), y + distance * sin(theta));
+}
+
+Coordinate2D Coordinate2D::rotate(double theta) const {
+    double newX = x * cos(theta) - y * sin(theta);
+    double newY = x * sin(theta) + y * cos(theta);
+    return Coordinate2D(newX, newY);
+}
+
+// Coordinate2D operator*(const Coordinate2D& a, double factor) { return a.scale(factor); }
+Coordinate2D Coordinate2D::scale(double factor) const {
+    return Coordinate2D(x * factor, y * factor);
+}
+
+double Coordinate2D::distance_to(const Coordinate2D destination) const {
+    double dx = destination.x - x;
+    double dy = destination.y - y;
+    return pow(
+        pow(dy, 2) + pow(dx, 2),
+        .5
+    );
+}
+
+double Coordinate2D::angle_to(const Coordinate2D destination) const {
+    double dx = destination.x - x;
+    double dy = destination.y - y;
+    return atan2(dy, dx);
+}
+
+// Same as destination - self
+Coordinate2D Coordinate2D::vector_to(const Coordinate2D destination) const {
+    Coordinate2D c(destination.x - x, destination.y - y);
+
+    return c;
+}
+
+double Coordinate2D::dot_product(const Coordinate2D other) const {
+    return (x * other.x) + (y * other.y);
+}
+
+bool Coordinate2D::is_behind(double radians, Coordinate2D other) const {
+    return vector_to(other).dot_product(Coordinate2D(radians)) < 0;
+}
\ No newline at end of file
diff --git a/src/utils/logging.cpp b/src/utils/logging.cpp
index 58ffb88..2025e29 100644
--- a/src/utils/logging.cpp
+++ b/src/utils/logging.cpp
@@ -1,70 +1,24 @@
-#ifndef CHESSBOT_LOGGING_CPP
-#define CHESSBOT_LOGGING_CPP
 
-// Associated Header File
-#include "utils/logging.h"
-
-// Built-In Libraries
 #include <Arduino.h>
 
-// Custom Libraries
-#include "utils/status.h"
 #include "../env.h"
+#include "utils/logging.h"
 
 // Prints a value or message through Serial. (The console)
 // ln means it sends a new newline character
 
-void serialLog(const char *message, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.print(message);
-}
-
-void serialLog(int value, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.print(value);
-}
-
-void serialLog(double value, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.print(value);
-}
-
-void serialLog(std::string value, int serialLoggingLevel)
-{
-    serialLog(value.c_str(), serialLoggingLevel);
-}
-
-void serialLogln(const char *message, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.println(message);
-}
-
-void serialLogln(int value, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.println(value);
-}
-
-void serialLogln(double value, int serialLoggingLevel)
-{
-    if (serialLoggingLevel <= LOGGING_LEVEL)
-        Serial.println(value);
-}
+void serial_printf(enum DebugLevel level, const char* fmt, ...) {
+    char buf[1024];
+    
+    if (level > LOGGING_LEVEL) {
+        return;
+    }
 
-void serialLogln(std::string value, int serialLoggingLevel)
-{
-    serialLogln(value.c_str(), serialLoggingLevel);
-}
+    va_list args;
 
-// This is used for specifically serialLogging errors
-void serialLogError(char message[], int error)
-{
-    if (LOGGING_LEVEL > 0)
-        Serial.printf(message, error);
-}
+    va_start(args, fmt);
+    vsnprintf(buf, sizeof(buf), fmt, args);
+    va_end(args);
 
-#endif
\ No newline at end of file
+    Serial.print(buf);
+}
\ No newline at end of file
diff --git a/src/utils/status.cpp b/src/utils/status.cpp
deleted file mode 100644
index 841ba9d..0000000
--- a/src/utils/status.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef CHESSBOT_STATUS_CPP
-#define CHESSBOT_STATUS_CPP
-
-// Associated Header File
-#include "utils/status.h"
-
-bool wifiConnected = false;
-bool serverConnected = false;
-bool botStopped = false;
-
-bool getWiFiConnectionStatus() {
-    return wifiConnected;
-}
-void setWiFiConnectionStatus(bool value) {
-    wifiConnected = value;
-}
-
-bool getServerConnectionStatus() {
-    return serverConnected;
-}
-void setServerConnectionStatus(bool value) {
-    serverConnected = value;
-}
-
-bool getStoppedStatus() {
-    return botStopped;
-}
-
-void setStoppedStatus(bool value) {
-    botStopped = value;
-}
-
-#endif
\ No newline at end of file
diff --git a/src/utils/timer.cpp b/src/utils/timer.cpp
deleted file mode 100644
index 348aa46..0000000
--- a/src/utils/timer.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-#ifndef CHESSBOT_TIMER_CPP
-#define CHESSBOT_TIMER_CPP
-
-// Associated Header File
-#include "utils/timer.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-#include <map>
-
-// Custom Libraries
-#include "utils/logging.h"
-
-// How to pass function from within a class (Uses a lambda that captures the class' pointer)
-// [this](){ func(); }
-
-// Stores all the timers, indexed by their id
-std::map<unsigned long, Timer> timers;
-
-// Will run the function you provide after 'delay' amount of milliseconds.
-// Accepts a delay in milliseconds, and a function as a pointer
-// Returns the id of the timer
-unsigned long timerDelay(int delay, TimerCallback func) {
-    // This uses a custom Timer struct (located in timer.h)
-    Timer newTimer;
-    newTimer.id = micros();
-    newTimer.oneOff = true;
-    newTimer.delay = delay;
-    newTimer.func = func;
-    newTimer.lastMillis = millis();
-    timers[newTimer.id] = newTimer;
-    // Returns the id for modifying
-    return newTimer.id;
-}
-
-// Will run the function you provide every 'interval' milliseconds.
-// Accepts an interval in milliseconds, and a function as a pointer
-// Returns the id of the timer
-unsigned long timerInterval(int interval, TimerCallback func) {
-    // This uses a custom Timer struct (located in timer.h)
-    Timer newTimer;
-    newTimer.id = micros();
-    newTimer.oneOff = false;
-    newTimer.delay = interval;
-    newTimer.func = func;
-    newTimer.lastMillis = millis();
-    timers[newTimer.id] = newTimer;
-    // Returns the id for modifying
-    return newTimer.id;
-}
-
-// Returns the Timer struct associated with the id
-Timer getTimer(unsigned long id) {
-    return timers[id];
-}
-
-// Resets the time on an active timer
-void timerReset(unsigned long id) {
-    timers[id].lastMillis = millis();
-}
-
-// Deletes the timer from the map
-void timerCancel(unsigned long id) {
-    timers.erase(id);
-}
-
-// Cancels all timers
-void timerCancelAll() {
-    timers.clear();
-}
-
-// Checks timers for any expired ones
-void timerStep() {
-    // Iterates through the timer map key value pairs
-    for (auto index = timers.begin(); index != timers.end();) {
-        unsigned long id = index->first;
-        if (millis() - timers[id].lastMillis >= timers[id].delay) {
-            TimerCallback func = timers[id].func;
-            if (timers[id].oneOff) {
-                // If the timer is one-off, cancel it after it expires
-                index = timers.erase(index);
-            } else {
-                // If the timer isn't one-off, refresh it after it expires
-                timerReset(id);
-                index++;
-            }
-
-            // Calls the linked function
-            func();
-        } else {
-            index++;
-        }
-    }
-}
-
-#endif
\ No newline at end of file
diff --git a/src/wifi/connection.cpp b/src/wifi/connection.cpp
index b2ced75..dd9b7ec 100644
--- a/src/wifi/connection.cpp
+++ b/src/wifi/connection.cpp
@@ -1,172 +1,99 @@
-#ifndef CHESSBOT_CONNECTION_CPP
-#define CHESSBOT_CONNECTION_CPP
+#include <Arduino.h>
+#include <ArduinoJson.h>
+#include <WiFi.h>
 
-// Associated Header File
 #include "wifi/connection.h"
 
-// Built-In Libraries
-#include "Arduino.h"
-#include "WiFi.h"
-
-// External Libraries
-#include <ArduinoJson.h>
-
-// Custom Libraries
-#include "wifi/packet.h"
+#include "../../env.h"
+#include "robot/robot.h"
 #include "utils/logging.h"
-#include "utils/timer.h"
-#include "utils/status.h"
-#include "robot/control.h"
-
-#include "../env.h"
+#include "wifi/packet.h"
 
-bool serverConnecting = false;
-bool pinging = false;
-int missedPings = 0;
-unsigned long pingTimeoutTimer;
+#if defined(ONLINE) \
+    && (!defined(WIFI_SSID) || !defined(WIFI_PASSWORD) || !defined(SERVER_IP) || !defined(SERVER_PORT))
+    #error ONLINE defined but one of (WIFI_SSID, WIFI_PASSWORD, SERVER_IP, SERVER_PORT) is not set
+#endif
 
 WiFiClient client;
+u_int32_t last_connection_try_time;
 
-// Called to connect to the server whose info is stored in env.h
-void connectServer() {
-    serialLogln("Connecting to Server...", 2);
-    if (client.connect(SERVER_IP, SERVER_PORT)) {
-        // If successful, sets the connection status and stops trying to connect to the server
-        setServerConnectionStatus(true);
-        serverConnecting = false;
-        serialLogln("Connected to Server!", 2);
-
-        // A handshake is an initial exchange of information, and a confirmation of a connection
-        if (DO_HANDSHAKE) { initiateHandshake(); }
-    } else {
-        serverConnecting = true;
-        // If unsuccessful, tries again in 5 seconds
-        serialLogln("Connection To Server Failed! Retrying...", 2);
-
-        timerDelay(HANDSHAKE_INTERVAL, &connectServer);
-    }
+inline bool connected() {
+    return WiFi.status() == WL_CONNECTED && client.connected();
 }
 
-// Completely disconnect from the server
-void disconnectServer() {
-    setServerConnectionStatus(false);
-    client.stop();
-    serialLogln("Disconnected From Server!", 2);
-}
+void connection_check_reconnect() {
+    u_int32_t delta_con_time = millis() - last_connection_try_time;
+    if (WiFi.status() != WL_CONNECTED && delta_con_time > 5000) {
+        WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
+        last_connection_try_time = millis();
+    }
 
-// If not connected to the server (whether by disconnect or by lost connection), reconnects
-void reconnectServer() {
-    if (!serverConnecting) {
-        setServerConnectionStatus(false);
-        serialLogln("Disconnected From Server! Reconnecting...", 2);
-        connectServer();
+    if (!client.connected()) {
+        if (client.connect(SERVER_IP, SERVER_PORT)) {
+            send_handshake();
+        }
     }
 }
 
-// Checks whether still connected to server
-bool checkServerConnection() {
-    return client.connected();
-}
+std::optional<JsonDocument> recv_packet() {
+    if (!connected()) {
+        return std::nullopt;
+    }
 
-// Sends an initial packet to the server. Contains the mac address of this bot
-void initiateHandshake() {
+    int index = 0;
+    char raw_packet[500];
     JsonDocument packet;
-    constructHelloPacket(packet);
-    serialLogln((char*)"Sending Handshake...", 2);
-    sendPacket(packet);
-}
+    
+    // Tries to read the whole packet in one go, might break if the underlying TCP packet is fragmented
+    while (client.available()) {
+        char data = client.read();
 
-// The buffer size is 500 characters. If there are issues right after
-// accepting a packet, the buffer size may be the culprit
-void acceptData() {
-    if (client.available()) {
-        // Allocates a buffer to hold the incoming packet
-        char rawPacket[500];
-        int len = 0;
-        bool packetDone = false;
-        while (client.available() || !packetDone) {
-            // Reads in a single character
-            char data = client.read();
-            if (data == ';') {
-                // If the delimiter character is encountered, the packet is done
-                packetDone = true;
-            } else {
-                // Adds the character to the buffer
-                rawPacket[len] = data;
-                len++;
-            }
+        if (data == ';' || index > 499) {
+            break;
+        } else {
+            raw_packet[index] = data;
+            index++;
         }
+    }
 
-        // The packet is in the form of a JSON. We use a library to handle them
-        JsonDocument packet;
-        // This turns the character buffer into a fully formed JSON object
-        deserializeJson(packet, rawPacket);
-        serialLog("Received Packet: ", 2);
-        // This takes that JSON object and prints it to Serial (the console) for debugging purposes
-        if (LOGGING_LEVEL >= 3) serializeJson(packet, Serial);
-        serialLog("\n", 2);
-
-        // Actually does something with the received packet
-        handlePacket(packet);
+    if (deserializeJson(packet, raw_packet) != DeserializationError::Ok) {
+        return std::nullopt;
     }
+
+    return std::make_optional(packet);
 }
 
 // Sends a packet to the server
-void sendPacket(JsonDocument& packet) {
-    // This takes that JSON object and sends it through the client's socket
+void send_packet(JsonDocument packet) {
     serializeJson(packet, client);
-    // Sends a delimiter character to mark the end of the packet
     client.write(';');
-    serialLogln("Sent Packet: ", 2);
-    // This takes that JSON object and prints it to Serial (the console) for debugging purposes
-    if (LOGGING_LEVEL >= 3) serializeJson(packet, Serial);
-    serialLog("\n", 2);
 }
 
-void sendActionSuccess(std::string messageId) {
-    JsonDocument packet;
-    constructSuccessPacket(packet, messageId);
-    serialLogln((char*)"Sending Action Success...", 2);
-    sendPacket(packet);
+void send_handshake() {
+    uint8_t mac[8];
+    JsonDocument packet;    
+
+    esp_efuse_mac_get_default(mac);
+    auto stringMac = unint8ArrayToHexString(mac, 6);
+
+    packet["type"] = "CLIENT_HELLO";
+    packet["macAddress"] = stringMac;
+
+    send_packet(packet);
 }
 
-void sendActionFail(std::string messageId) {
+void send_success(std::string id) {
     JsonDocument packet;
-    constructFailPacket(packet, messageId);
-    serialLogln((char*)"Sending Action Success...", 2);
-    sendPacket(packet);
-}
+    packet["type"] = "ACTION_SUCCESS";
+    packet["packetId"] = id;
 
-void pingTimeout() {
-    if (DO_PINGING) {
-        missedPings++;
-        serialLog(missedPings, 2);
-        serialLogln((char*)" missed ping!", 2);
-        if (missedPings >= PING_MAX_MISSES) {
-            serialLogln((char*)"SERVER TIMED OUT!", 2);
-            stop();
-            pinging = false;
-        } else {
-            pingTimeoutTimer = timerDelay(PING_TIMEOUT, &pingTimeout);
-        }
-    }
+    send_packet(packet);
 }
 
-void sendPingResponse() {
-    if (DO_PINGING) {
-        JsonDocument packet;
-        constructPingPacket(packet);
-        serialLogln((char*)"Sending Ping Response...", 2);
-        sendPacket(packet);
-        if (pinging) {
-            timerReset(pingTimeoutTimer);
-        } else {
-            pingTimeoutTimer = timerDelay(PING_TIMEOUT, &pingTimeout);
-            serialLogln((char*)"Started Ping Timeout Timer", 2);
-            pinging = true;
-        }
-        missedPings = 0;
-    }
-}
+void send_ping() {
+    JsonDocument packet;
+    packet["type"] = "PING_RESPONSE";
+    packet["batteryLevel"] = Robot::batteryLevel();
 
-#endif
\ No newline at end of file
+    send_packet(packet);
+}
\ No newline at end of file
diff --git a/src/wifi/packet.cpp b/src/wifi/packet.cpp
index f69a964..b05cb8c 100644
--- a/src/wifi/packet.cpp
+++ b/src/wifi/packet.cpp
@@ -1,112 +1,120 @@
-#ifndef CHESSBOT_PACKET_CPP
-#define CHESSBOT_PACKET_CPP
+#include <Arduino.h>
+#include <ArduinoJson.h>
+#include <esp_mac.h>
+#include <string>
 
-// Associated Header File
 #include "wifi/packet.h"
 
-// Built-In Libraries
-#include "Arduino.h"
-#include "esp_mac.h"
-
-// External Libraries
-#include <ArduinoJson.h>
-
-// Custom Libraries
-#include "utils/logging.h"
-#include "utils/status.h"
-#include "utils/functions.h"
+#include "robot/robot.h"
 #include "utils/config.h"
-#include "robot/control.h"
-#include "robot/splines.h"
-#include "robot/battery.h"
+#include "utils/functions.h"
+#include "utils/logging.h"
 #include "wifi/connection.h"
 
-// These are the various different supported message types that can be sent over TCP
-const char* CLIENT_HELLO = "CLIENT_HELLO";
-const char* SERVER_HELLO = "SERVER_HELLO";
-const char* PING_SEND = "PING_SEND";
-const char* PING_RESPONSE = "PING_RESPONSE";
-const char* QUERY_VAR = "QUERY_VAR";
-const char* QUERY_RESPONSE = "QUERY_RESPONSE";
-const char* SET_VAR = "SET_VAR";
-const char* TURN_BY_ANGLE = "TURN_BY_ANGLE";
-const char* DRIVE_TILES = "DRIVE_TILES";
-const char* DRIVE_TICKS = "DRIVE_TICKS";
-const char* ACTION_SUCCESS = "ACTION_SUCCESS";
-const char* ACTION_FAIL = "ACTION_FAIL";
-const char* DRIVE_TANK = "DRIVE_TANK";
-const char* ESTOP = "ESTOP";
-const char* CUBIC = "DRIVE_CUBIC_SPLINE";
-const char* QUADRATIC = "DRIVE_QUADRATIC_SPLINE";
-const char* SPIN_RADIANS = "SPIN_RADIANS";
+PacketType parse_packet_type(std::string type) {
+    if (type == "CLIENT_HELLO") {
+        return CLIENT_HELLO;
+    }
+    if (type == "SERVER_HELLO") {
+        return SERVER_HELLO;
+    }
+    if (type == "PING_SEND") {
+        return PING_SEND;
+    }
+    if (type == "PING_RESPONSE") {
+        return PING_RESPONSE;
+    }
+    if (type == "TURN_BY_ANGLE") {
+        return TURN_BY_ANGLE;
+    }
+    if (type == "DRIVE_TILES") {
+        return DRIVE_TILES;
+    }
+    if (type == "DRIVE_TICKS") {
+        return DRIVE_TICKS;
+    }
+    if (type == "ACTION_SUCCESS") {
+        return ACTION_SUCCESS;
+    }
+    if (type == "ACTION_FAIL") {
+        return ACTION_FAIL;
+    }
+    if (type == "DRIVE_TANK") {
+        return DRIVE_TANK;
+    }
+    if (type == "ESTOP") {
+        return ESTOP;
+    }
+    if (type == "SPIN_RADIANS") {
+        return SPIN_RADIANS;
+    }
+    if (type == "BS_MOVE") {
+        return BS_MOVE;
+    }
+    if (type == "CENTER_SEND") {
+        return CENTER_SEND;
+    }
+
+    return ERROR;
+}
 
 // Takes a packet a does specific things based on the type
-void handlePacket(JsonDocument packet) {
-    // Sadly a switch case can't be used due to the packet type being a string.
-    // We do this to allow the packets to be more readable when serialLogged
-    if (packet["type"] == SERVER_HELLO) {
+bool handle_packet(Robot& r, JsonDocument packet) {
+    ASSERT_FIELD(packet, "type", const char *)
+
+    PacketType type = parse_packet_type(packet["type"].as<std::string>());
+    serial_printf(DebugLevel::INFO, "Received a packet of type %d\n", type);
+
+    if (type == ERROR) {
+        return false;
+    }
+
+    if (type == SERVER_HELLO) {
         // When we initiate a handshake, the server sends a handshake back. This server handshake
         // contains any variable that should be changed in this bot's config
+        ASSERT_FIELD(packet, "config", JsonObject)
+
         setConfig(packet["config"].as<JsonObject>());
-    } else if (packet["type"] == DRIVE_TANK) {
-        // This is received when the bot is being manually controlled via the debug page
-        drive(packet["left"], packet["right"], packet["packetId"]);
-    } else if (packet["type"] == DRIVE_TICKS){
-        driveTicks(packet["tickDistance"], packet["packetId"]);
-    } else if (packet["type"] == ESTOP) {
-        stop();
-    } else if (packet["type"] == CUBIC) {
-        Point startPosition = {(float)packet["startPosition"]["x"]*TILES_TO_TICKS, (float)packet["startPosition"]["y"]*TILES_TO_TICKS};
-        Point endPosition = {(float)packet["endPosition"]["x"]*TILES_TO_TICKS, (float)packet["endPosition"]["y"]*TILES_TO_TICKS};
-        Point controlPositionA = {(float)packet["controlPositionA"]["x"]*TILES_TO_TICKS, (float)packet["controlPositionA"]["y"]*TILES_TO_TICKS};
-        Point controlPositionB = {(float)packet["controlPositionB"]["x"]*TILES_TO_TICKS, (float)packet["controlPositionB"]["y"]*TILES_TO_TICKS};
-        danceMonkeyCubic(packet["packetId"], startPosition, controlPositionA, controlPositionB, endPosition, packet["timeDeltaMs"]);
-    } else if (packet["type"] == QUADRATIC) {
-        serialLog("I have arrived!! at Quadratic", 3);
-        Point startPosition = {(float)packet["startPosition"]["x"]*TILES_TO_TICKS, (float)packet["startPosition"]["y"]*TILES_TO_TICKS};
-        Point endPosition = {(float)packet["endPosition"]["x"]*TILES_TO_TICKS, (float)packet["endPosition"]["y"]*TILES_TO_TICKS};
-        Point controlPosition = {(float)packet["controlPosition"]["x"]*TILES_TO_TICKS, (float)packet["controlPosition"]["y"]*TILES_TO_TICKS};
-        danceMonkeyQuadratic(packet["packetId"], startPosition, controlPosition, endPosition, packet["timeDeltaMs"]);
-    } else if (packet["type"] == SPIN_RADIANS) {
-        serialLog("Going to spin!", 3);
-        int offsetTicks = radiansToTicks((float)packet["radians"]);
-        startCustomMotionProfileTimer(-offsetTicks, offsetTicks, (double)packet["timeDeltaMs"]/1000, packet["packetId"]);
-    } else if (packet["type"] == TURN_BY_ANGLE) {
-        turn(packet["deltaHeadingRadians"], packet["packetId"]);
-    } else if (packet["type"] == DRIVE_TILES) {
-        drive(packet["tiles"], packet["packetId"]);
-    } else if (packet["type"] == PING_SEND) {
-        sendPingResponse();
-    }
-}
 
-// This creates the handshake packet sent to the server when this bot connects to it
-void constructHelloPacket(JsonDocument& packet) {
-    packet["type"] = CLIENT_HELLO;
-    uint8_t mac[8];
-    // Gets the mac address of this esp
-    esp_efuse_mac_get_default(mac);
-    // Converts the mac address into the form the server is expecting
-    std::string stringMac = unint8ArrayToHexString(mac, 6);
-    packet["macAddress"] = stringMac;
-}
+    } else if (type == DRIVE_TANK) {
+        // Manual control of the robot
 
-//Note that the assigning of the messageId to the packetId happens in all the methods. One way to better
-//streamline this might be to make a general method "constructPacket" that just handles that. For now I
-//thought it wouldn't be necessary. As we get more types of messages this may be needed.
-void constructSuccessPacket(JsonDocument& packet, std::string messageId) {
-    packet["type"] = ACTION_SUCCESS;
-    packet["packetId"] = messageId;
-}
+        ASSERT_FIELD(packet, "left", double)
+        ASSERT_FIELD(packet, "left", double)
+        ASSERT_FIELD(packet, "packetId", const char *)
 
-void constructFailPacket(JsonDocument& packet, std::string messageId) {
-    packet["type"] = ACTION_FAIL;
-    packet["packetId"] = messageId;
-}
+        std::tuple<double, double> power = std::make_tuple(
+            packet["left"].as<double>(), packet["right"].as<double>()
+        );
 
-void constructPingPacket(JsonDocument& packet) {
-    packet["type"] = PING_RESPONSE;
-    packet["batteryLevel"] = getBatteryLevel();
-}
+        r.drive(power);
+
+    } else if (type == ESTOP) {
+        r.stop();
+
+    } else if (type == TURN_BY_ANGLE) {
+        ASSERT_FIELD(packet, "deltaHeadingRadians", double)
+        ASSERT_FIELD(packet, "packetId", const char *)
+
+        double delta_angle = packet["deltaHeadingRadians"].as<double>();
+
+        r.turn(delta_angle, packet["packetId"].as<std::string>());
+
+    } else if (type == DRIVE_TILES) {
+        ASSERT_FIELD(packet, "tileDistance", double)
+        ASSERT_FIELD(packet, "packetId", const char *)
+
+        double tiles = packet["tileDistance"].as<double>();
+
+        r.drive(tiles, packet["packetId"].as<std::string>());
+
+    } else if (type == CENTER_SEND) {
+        ASSERT_FIELD(packet, "tileDistance", double)
+
+        r.center(packet["packetId"].as<std::string>());
+    } else if (type == PING_SEND) {
+        send_ping();
+    }
 
-#endif
\ No newline at end of file
+    return true;
+}
\ No newline at end of file
diff --git a/src/wifi/wireless.cpp b/src/wifi/wireless.cpp
deleted file mode 100644
index 8f0c212..0000000
--- a/src/wifi/wireless.cpp
+++ /dev/null
@@ -1,102 +0,0 @@
-#ifndef CHESSBOT_WIRELESS_CPP
-#define CHESSBOT_WIRELESS_CPP
-
-// Associated Header File
-#include "wifi/wireless.h"
-
-// Built-In Libraries
-#include "Arduino.h"
-#include "WiFi.h"
-
-// Custom Libraries
-#include "utils/logging.h"
-#include "utils/timer.h"
-#include "utils/status.h"
-#include "wifi/connection.h"
-#include "../env.h"
-
-bool wifiConnecting = false;
-
-// Gets the status of the WiFi connection. Called whenever connecting to WiFi fails to get more info
-const char* getWifiStatus(int status) {
-    switch(status){
-        case WL_IDLE_STATUS:
-        return "WL_IDLE_STATUS";
-        case WL_SCAN_COMPLETED:
-        return "WL_SCAN_COMPLETED";
-        case WL_NO_SSID_AVAIL:
-        return "WL_NO_SSID_AVAIL";
-        case WL_CONNECT_FAILED:
-        return "WL_CONNECT_FAILED";
-        case WL_CONNECTION_LOST:
-        return "WL_CONNECTION_LOST";
-        case WL_CONNECTED:
-        return "WL_CONNECTED";
-        case WL_DISCONNECTED:
-        return "WL_DISCONNECTED";
-    }
-    return "No Status";
-}
-
-// Checks whether the bot is connected to a WiFi network
-bool checkWiFiConnection() {
-    return WiFi.status() == WL_CONNECTED;
-}
-
-// If not connected to WiFi by now, serialLog 'why'
-// Tries to connect until success. After connected to WiFi, starts trying to connect to the server
-void confirmWiFi() {
-    if (checkWiFiConnection()) {
-        setWiFiConnectionStatus(true);
-        wifiConnecting = false;
-        serialLogln("Connected to WiFi Network!", 2);
-
-        // Connect to the server
-        connectServer();
-    } else {
-        setWiFiConnectionStatus(false);
-        wifiConnecting = true;
-        serialLog("Failed To Connect To WiFi: ", 2);
-        serialLog(getWifiStatus(WiFi.status()), 2);
-        serialLogln(". Retrying... ", 2);
-
-        // Check connection again after half a second
-        timerDelay(500, &confirmWiFi);
-    }
-}
-
-// Connects to a WiFi network with the SSID and Password set in env.h
-void connectWiFI() {
-    wifiConnecting = true;
-    serialLogln("Connecting to WiFi Network...", 2);
-    WiFi.mode(WIFI_STA);
-    WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
-    // We delay the connection test to give the ESP time to fully connect to the network
-    timerDelay(500, &confirmWiFi);
-}
-
-// Completely disconnect from WiFi
-void disconnectWiFI() {
-    setWiFiConnectionStatus(false);
-    WiFi.disconnect();
-    serialLogln("Disconnected From WiFI!", 2);
-}
-
-// If not connected to WiFi (whether by disconnect or by lost connection), reconnects
-void reconnectWiFI() {
-    if (!wifiConnecting) {
-        setWiFiConnectionStatus(false);
-        serialLogln("Disconnected From WiFI! Reconnecting...", 2);
-        connectWiFI();
-    }
-}
-
-// Creates a WiFi network with the SSID and Password set in env.h
-void createWiFi() {
-    serialLogln("Creating Access Point", 2);
-    WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
-    serialLogln("Access Point Created", 2);
-    connectServer();
-}
-
-#endif
\ No newline at end of file