diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt
index a5ab68b..f26d0d7 100644
--- a/server_v2/CMakeLists.txt
+++ b/server_v2/CMakeLists.txt
@@ -49,7 +49,8 @@ set(SRC_FILES
         src/communication/MessageProcessor.cpp
         src/communication/MessageBuilder.cpp
         src/communication/UnixSocketServer.cpp
-        src/communication/TCPSocketServer.cpp)
+        src/communication/TCPSocketServer.cpp
+        src/GoToGoalController.cpp)
 
 set(HDR_FILES
         include/spi.hpp
@@ -67,7 +68,8 @@ set(HDR_FILES
         include/communication/MessageProcessor.hpp
         include/communication/MessageBuilder.hpp
         include/communication/UnixSocketServer.hpp
-        include/communication/TCPSocketServer.hpp)
+        include/communication/TCPSocketServer.hpp
+        include/GoToGoalController.hpp)
 
 include_directories(third_party/asio)
 
diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp
index 5e05ff8..b5a5805 100644
--- a/server_v2/include/ClosedLoopMotorController.hpp
+++ b/server_v2/include/ClosedLoopMotorController.hpp
@@ -22,7 +22,7 @@ public:
 
   void set_power(uint8_t port, double power);
 
-  void set_speed(uint8_t port, double speed_ms);
+  void set_speed(uint8_t port, double speed_rad_s);
 
   void generate_step_response_data();
 
diff --git a/server_v2/include/Encoders.hpp b/server_v2/include/Encoders.hpp
index f2b29fc..bbde700 100644
--- a/server_v2/include/Encoders.hpp
+++ b/server_v2/include/Encoders.hpp
@@ -19,7 +19,7 @@ public:
 
   std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();
 
-  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ms();
+  std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();
 
 private:
   Encoders() = default;
@@ -28,7 +28,7 @@ private:
   Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
 
   std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
-  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ms = {0};
+  std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
 };
 
 
diff --git a/server_v2/include/GoToGoalController.hpp b/server_v2/include/GoToGoalController.hpp
new file mode 100644
index 0000000..33ec68d
--- /dev/null
+++ b/server_v2/include/GoToGoalController.hpp
@@ -0,0 +1,29 @@
+//
+// Created by KonstantinViesure on 26.05.22.
+//
+
+#ifndef COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
+#define COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
+
+
+class GoToGoalController {
+public:
+  static GoToGoalController &getInstance() {
+    static GoToGoalController instance;
+    return instance;
+  }
+
+  GoToGoalController(GoToGoalController const &) = delete;
+
+  void operator=(GoToGoalController const &) = delete;
+
+  static void go_to_point(double goal_x, double goal_y, double v_ms);
+
+  static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s);
+
+private:
+  GoToGoalController() = default;
+};
+
+
+#endif //COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
diff --git a/server_v2/include/OdometryController.hpp b/server_v2/include/OdometryController.hpp
index 1be1e95..570a603 100644
--- a/server_v2/include/OdometryController.hpp
+++ b/server_v2/include/OdometryController.hpp
@@ -26,6 +26,8 @@ public:
 
   Odometry get();
 
+  [[nodiscard]] bool is_enabled() const;
+
 private:
   typedef std::chrono::steady_clock clock;
 
@@ -40,6 +42,7 @@ private:
   double last_position_left{0};
   double last_position_right{0};
   std::chrono::time_point<clock> last_run;
+
 };
 
 
diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp
index 9400f0f..11f8f6e 100644
--- a/server_v2/include/Robot.hpp
+++ b/server_v2/include/Robot.hpp
@@ -7,10 +7,10 @@
 #define ROBOT_IR_RATE_HZ 250
 
 #define ROBOT_MOTOR_COUNT 4
-#define ROBOT_MOTOR_SPEED_CONTROL_KP 222.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KI 2110.0
-#define ROBOT_MOTOR_SPEED_CONTROL_KD 2.22
-#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 0.2
+#define ROBOT_MOTOR_SPEED_CONTROL_KP 2.5
+#define ROBOT_MOTOR_SPEED_CONTROL_KI 19.0
+#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.07
+#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI
 #define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
 
 #define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
@@ -21,12 +21,15 @@
 
 #define ROBOT_ENCODER_RATE_HZ 250
 
-#define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
-#define ROBOT_WHEEL_CIRCUMFERENCE_M (ROBOT_WHEEL_CIRCUMFERENCE_MM / 1000.0)
+#define ROBOT_WHEEL_RADIUS_MM 35.5
+#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
+#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2 * ROBOT_WHEEL_RADIUS_MM * M_PI)
+#define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI)
 #define ROBOT_TICKS_PER_TURN (27.7 * 100.0)
 #define ROBOT_ARBOR_LENGTH_MM 139.0
 #define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0)
 #define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
 
+#define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1
 
 #endif //COMPLIB_SERVER_ROBOT_HPP
diff --git a/server_v2/include/mathUtils.hpp b/server_v2/include/mathUtils.hpp
index 603c6fd..9e16e4b 100644
--- a/server_v2/include/mathUtils.hpp
+++ b/server_v2/include/mathUtils.hpp
@@ -2,6 +2,7 @@
 #define COMPLIB_SERVER_MATHUTILS_HPP
 
 #include <cstdint>
+#include <cmath>
 
 namespace mathUtils {
   inline int int_from_bytes(uint8_t *data, int length) {
@@ -34,6 +35,11 @@ namespace mathUtils {
       i++;
     }
   }
+
+  template<class T>
+  inline T wrap_angle_to_pi(T angle) {
+    return atan2(sin(angle), cos(angle));
+  }
 }
 
 #endif // COMPLIB_SERVER_MATHUTILS_HPP
\ No newline at end of file
diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp
index 4e33a88..cc2ed36 100644
--- a/server_v2/src/ClosedLoopMotorController.cpp
+++ b/server_v2/src/ClosedLoopMotorController.cpp
@@ -40,7 +40,7 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
       continue;
     }
 
-    auto speeds_ms = Encoders::getInstance().get_velocities_ms();
+    auto speeds_ms = Encoders::getInstance().get_velocities_rad_s();
     for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
       if (control_modes.at(i) == ControlMode::SPEED) {
         double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i));
@@ -55,7 +55,7 @@ void ClosedLoopMotorController::generate_step_response_data() {
   console->set_pattern("%v");
 
   auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
-  auto input = 25.0;
+  auto input = 10.0;
 
   speed_targets.at(port) = input;
   pids.at(port) = PID(
@@ -70,16 +70,16 @@ void ClosedLoopMotorController::generate_step_response_data() {
   for (int i = 0; i < 500; ++i) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
-    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
     console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
   }
 
-  input = 50.0;
+  input = 20.0;
   speed_targets.at(port) = input;
   for (int i = 0; i < 500; ++i) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
-    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
     console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
   }
 
@@ -92,12 +92,12 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
   console->set_pattern("%v");
 
   auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
-  auto input = 0.25;
+  auto input = 2 * M_PI;
 
   speed_targets.at(port) = input;
   pids.at(port) = PID(
-      221.0, 2110.0, 2.21,
-      0.2, 100.0);
+      2.5, 19.0, 0.07,
+      4 * M_PI, 100.0);
   control_modes.at(port) = ControlMode::SPEED;
 
   std::this_thread::sleep_for(std::chrono::seconds(2));
@@ -107,28 +107,28 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
   for (int i = 0; i < 500; ++i) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
-    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
     console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
   }
 
-  input = 0.35;
+  input = 4 * M_PI;
   speed_targets.at(port) = input;
   for (int i = 0; i < 500; ++i) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
-    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
     console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
   }
 
-  input = 0.1;
+  input = M_PI;
   speed_targets.at(port) = input;
   for (int i = 0; i < 500; ++i) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
     double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
-    auto output = Encoders::getInstance().get_velocities_ms().at(port);
+    auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
     console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
 
-    input += 0.001;
+    input += 0.01;
     speed_targets.at(port) = input;
   }
 
diff --git a/server_v2/src/Encoders.cpp b/server_v2/src/Encoders.cpp
index 78aba7c..921d552 100644
--- a/server_v2/src/Encoders.cpp
+++ b/server_v2/src/Encoders.cpp
@@ -15,16 +15,17 @@ std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
   return cached_positions;
 }
 
-std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ms() {
+std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
   if (velocity_cache.is_expired()) {
     uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
     Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
     for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
-      auto velocity_tps = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
-      auto velocity_rps = velocity_tps / ROBOT_TICKS_PER_TURN;
-      cached_velocities_ms.at(i) = velocity_rps * ROBOT_WHEEL_CIRCUMFERENCE_M;
+      auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
+      auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN;
+      auto velocity_rad_s = velocity_rot_s * 2 * M_PI;
+      cached_velocities_rad_s.at(i) = velocity_rad_s;
     }
     velocity_cache.update();
   }
-  return cached_velocities_ms;
+  return cached_velocities_rad_s;
 }
diff --git a/server_v2/src/GoToGoalController.cpp b/server_v2/src/GoToGoalController.cpp
new file mode 100644
index 0000000..3d6aafb
--- /dev/null
+++ b/server_v2/src/GoToGoalController.cpp
@@ -0,0 +1,64 @@
+#include <spdlog/spdlog.h>
+
+#include "include/GoToGoalController.hpp"
+#include "include/Robot.hpp"
+#include "include/ClosedLoopMotorController.hpp"
+#include "include/OdometryController.hpp"
+#include "include/mathUtils.hpp"
+
+#define MS_IN_S 1000
+#define US_IN_S (1000 * MS_IN_S)
+
+// Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities
+// Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code
+void GoToGoalController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) {
+  auto vr_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
+  auto vl_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
+
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s);
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
+}
+
+void GoToGoalController::go_to_point(double goal_x, double goal_y, double v_ms) {
+  auto was_enabled = OdometryController::getInstance().is_enabled();
+  OdometryController::getInstance().enable();
+  auto start_pos = OdometryController::getInstance().get();
+
+  auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
+
+  auto abs_distance = fabs(sqrt(
+      pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2)
+  ));
+  while (abs_distance > 0.05) {
+    auto current_pos = OdometryController::getInstance().get();
+    auto alpha = mathUtils::wrap_angle_to_pi(
+        (atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2))
+    );
+
+    auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha));
+    auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha));
+    auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
+    auto vl_rad_s = vl_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
+
+    ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s);
+    ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
+
+    abs_distance = fabs(sqrt(
+        pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2)
+    ));
+    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
+                  current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
+                  goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(), atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()),
+                  current_pos.get_angular_orientation() + M_PI_2, alpha,
+                  vr_rad_s, vl_rad_s);
+
+    std::this_thread::sleep_for(sleep_duration);
+  }
+
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
+  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
+
+  if (!was_enabled) {
+    OdometryController::getInstance().disable();
+  }
+}
diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp
index aed8698..379401c 100644
--- a/server_v2/src/OdometryController.cpp
+++ b/server_v2/src/OdometryController.cpp
@@ -4,10 +4,15 @@
 #include "include/Robot.hpp"
 #include "include/Encoders.hpp"
 #include <spdlog/spdlog.h>
+#include "include/mathUtils.hpp"
 
 #define MS_IN_S 1000
 #define US_IN_S (1000 * MS_IN_S)
 
+bool OdometryController::is_enabled() const {
+  return enabled;
+}
+
 void OdometryController::enable() {
   last_run = clock::now();
   enabled = true;
@@ -15,6 +20,7 @@ void OdometryController::enable() {
 
 void OdometryController::disable() {
   enabled = false;
+  OdometryController::reset();
 }
 
 void OdometryController::reset() {
@@ -70,7 +76,7 @@ OdometryController::OdometryController() {
                           cos(current_orientation) * distance_y);
       }
       if (theta != 0) {
-        new_angular_orientation = current_odometry.get_angular_orientation() + theta;
+        new_angular_orientation = mathUtils::wrap_angle_to_pi(current_odometry.get_angular_orientation() + theta);
       }
       current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation);
 
diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp
index a7b02e9..0cbc171 100644
--- a/server_v2/src/communication/MessageProcessor.cpp
+++ b/server_v2/src/communication/MessageProcessor.cpp
@@ -20,7 +20,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
     if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) {
       return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions());
     } else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) {
-      return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_ms());
+      return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rad_s());
     }
 
     // IR Sensors
diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp
index 8c4f330..104e8d4 100644
--- a/server_v2/src/main.cpp
+++ b/server_v2/src/main.cpp
@@ -6,6 +6,7 @@
 #include "include/ClosedLoopMotorController.hpp"
 #include "include/communication/UnixSocketServer.hpp"
 #include "include/communication/TCPSocketServer.hpp"
+#include "include/GoToGoalController.hpp"
 
 using namespace std;
 
@@ -17,8 +18,8 @@ int main() {
   UnixSocketServer unixSocketServer;
   TCPSocketServer tcpSocketServer;
 
-//  ClosedLoopMotorController::getInstance().set_speed(0, -125);
-//  ClosedLoopMotorController::getInstance().set_speed(3, 125);
+//  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT);
+//  ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT);
 //  OdometryController::getInstance().enable();
 
 //  for (int i = 0; i < 10000; ++i) {
@@ -27,7 +28,18 @@ int main() {
 //    usleep(1000);
 //  }
 
-  ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
+//  OdometryController::getInstance().enable();
+//  GoToGoalController::diff_drive_inverse_kinematics(0.2, -M_PI_4);
+//  for (int i = 0; i < 800; ++i) {
+//    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+//    auto odom = OdometryController::getInstance().get();
+//    spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
+//  }
+
+  GoToGoalController::go_to_point(2, 0, 0.2);
+
+//  ClosedLoopMotorController::getInstance().generate_step_response_data();
+//  ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
   std::this_thread::sleep_for(std::chrono::hours(12));
   return 0;
 }