diff --git a/src/pathing/CMakeLists.txt b/src/pathing/CMakeLists.txt
index c1e2b7a..2a79b0f 100644
--- a/src/pathing/CMakeLists.txt
+++ b/src/pathing/CMakeLists.txt
@@ -1,2 +1,5 @@
-add_executable(simulator simulator.cc pathing.cc)
-target_link_libraries(simulator PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
\ No newline at end of file
+add_executable(simulator simulator.cc pathing.cc) 
+target_link_libraries(simulator PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
+
+add_library(controller controller.cc velocity_sender.cc pathing.cc)
+target_link_libraries(controller PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
diff --git a/src/pathing/controller.cc b/src/pathing/controller.cc
new file mode 100644
index 0000000..23b86b2
--- /dev/null
+++ b/src/pathing/controller.cc
@@ -0,0 +1,129 @@
+#include "src/pathing/controller.h"
+#include <frc/Timer.h>
+#include <chrono>
+#include <fstream>
+#include <nlohmann/json.hpp>
+#include <thread>
+#include "src/pathing/pathing.h"
+#include "src/pathing/velocity_sender.h"
+
+namespace pathing {
+
+Controller::Controller() : instance_(nt::NetworkTableInstance::GetDefault()) {
+
+  auto table = instance_.GetTable("Pathing");
+
+  current_pose_sub_ =
+      table->GetStructTopic<frc::Pose2d>("CurrentPose").Subscribe({});
+
+  target_pose_sub_ =
+      table->GetStructTopic<frc::Pose2d>("TargetPose").Subscribe({});
+}
+
+void Controller::Send() {
+
+  VelocitySender sender;
+
+  std::ifstream file("/root/bos/constants/navgrid.json");
+  nlohmann::json data = nlohmann::json::parse(file);
+
+  int W = data["grid"][0].size();
+  int H = data["grid"].size();
+  double nodeSize = data["nodeSizeMeters"];
+
+  std::vector<std::vector<bool>> gridData(H, std::vector<bool>(W));
+
+  for (int y = 0; y < H; y++)
+    for (int x = 0; x < W; x++)
+      gridData[y][x] = data["grid"][y][x];
+
+  cv::Mat grid = initializeGrid(gridData);
+
+  frc::Pose2d start = current_pose_sub_.Get();
+  frc::Pose2d target = target_pose_sub_.Get();
+
+  int sx = start.X().value() / nodeSize;
+  int sy = start.Y().value() / nodeSize;
+  int tx = target.X().value() / nodeSize;
+  int ty = target.Y().value() / nodeSize;
+
+  auto poses = createSpline(grid, sx, sy, tx, ty, nodeSize);
+
+  if (poses.empty()) {
+    sender.Send(0, 0);
+    return;
+  }
+
+  std::vector<double> dist(poses.size(), 0);
+
+  for (size_t i = 1; i < poses.size(); i++) {
+    double dx = poses[i].X().value() - poses[i - 1].X().value();
+    double dy = poses[i].Y().value() - poses[i - 1].Y().value();
+    dist[i] = dist[i - 1] + std::hypot(dx, dy);
+  }
+
+  double total = dist.back();
+
+  constexpr double maxV = 5.0;
+  constexpr double maxA = 3.0;
+  constexpr double lookahead = 0.5;
+  constexpr double tol = 0.1;
+
+  std::vector<double> speed(poses.size());
+
+  for (size_t i = 0; i < poses.size(); i++) {
+    double a = std::sqrt(2 * maxA * dist[i]);
+    double d = std::sqrt(2 * maxA * (total - dist[i]));
+    speed[i] = std::min({a, d, maxV});
+  }
+
+  size_t idx = 0;
+
+  while (true) {
+
+    frc::Pose2d pose = current_pose_sub_.Get();
+    double rx = pose.X().value();
+    double ry = pose.Y().value();
+
+    double goalDist = std::hypot(poses.back().X().value() - rx,
+                                 poses.back().Y().value() - ry);
+
+    if (goalDist < tol) {
+      sender.Send(0, 0);
+      break;
+    }
+
+    for (size_t i = idx; i < poses.size(); i++) {
+      double d =
+          std::hypot(poses[i].X().value() - rx, poses[i].Y().value() - ry);
+      if (d >= lookahead) {
+        idx = i;
+        break;
+      }
+    }
+
+    if (idx >= poses.size())
+      idx = poses.size() - 1;
+
+    double txp = poses[idx].X().value();
+    double typ = poses[idx].Y().value();
+
+    double dx = txp - rx;
+    double dy = typ - ry;
+    double mag = std::hypot(dx, dy);
+
+    if (mag < 1e-6) {
+      sender.Send(0, 0);
+      continue;
+    }
+
+    double vx = dx / mag * speed[idx];
+    double vy = dy / mag * speed[idx];
+
+    sender.Send(vx, vy);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+}
+
+}  // namespace pathing
\ No newline at end of file
diff --git a/src/pathing/controller.h b/src/pathing/controller.h
new file mode 100644
index 0000000..c607a12
--- /dev/null
+++ b/src/pathing/controller.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include <cstdint>
+
+namespace pathing {
+class Controller {
+ public:
+  Controller();
+  void Send();
+
+ private:
+  nt::NetworkTableInstance instance_;
+  nt::StructSubscriber<frc::Pose2d> current_pose_sub_;
+  nt::StructSubscriber<frc::Pose2d> target_pose_sub_;
+
+  int64_t kDtUs = 20'000;
+  double ax;
+  double ay;
+};
+
+}  // namespace pathing
diff --git a/src/pathing/pathing.cc b/src/pathing/pathing.cc
index 39b3111..aa43459 100644
--- a/src/pathing/pathing.cc
+++ b/src/pathing/pathing.cc
@@ -1,221 +1,136 @@
 #include "src/pathing/pathing.h"
-#include <frc/DataLogManager.h>
-#include <frc/geometry/Pose2d.h>
-#include <networktables/NetworkTableInstance.h>
-#include <networktables/StructArrayTopic.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <algorithm>
 #include <cmath>
-#include <iostream>
-#include <opencv2/core.hpp>
-#include <opencv2/imgproc.hpp>
 #include <opencv2/opencv.hpp>
-#include <ostream>
 #include <queue>
-#include <vector>
-
-const int CELL_SIZE = 20;
 
 auto initializeGrid(const std::vector<std::vector<bool>>& gridData) -> cv::Mat {
-  int GRID_H = gridData.size();
-  int GRID_W = gridData[0].size();
-  cv::Mat grid(GRID_H, GRID_W, CV_8UC1);
-  for (int y = 0; y < GRID_H; ++y) {
-    for (int x = 0; x < GRID_W; ++x) {
+
+  int H = gridData.size();
+  int W = gridData[0].size();
+
+  cv::Mat grid(H, W, CV_8UC1);
+
+  for (int y = 0; y < H; y++)
+    for (int x = 0; x < W; x++)
       grid.at<uchar>(y, x) = gridData[y][x] ? 0 : 255;
-    }
-  }
+
   return grid;
 }
 
 auto BFS(const cv::Mat& grid, std::pair<int, int> start,
          std::pair<int, int> target) -> std::vector<std::pair<int, int>> {
-  std::cout << "BFS: Grid size: " << grid.cols << "x" << grid.rows << std::endl;
-  std::cout << "BFS: Start (" << start.first << ", " << start.second
-            << ") value: " << (int)grid.at<uchar>(start.second, start.first)
-            << std::endl;
-  std::cout << "BFS: Target (" << target.first << ", " << target.second
-            << ") value: " << (int)grid.at<uchar>(target.second, target.first)
-            << std::endl;
-
-  if (grid.at<uchar>(start.second, start.first) == 0) {
-    std::cerr << "BFS: Start position is blocked!" << std::endl;
+
+  if (grid.at<uchar>(start.second, start.first) == 0)
     return {};
-  }
-  if (grid.at<uchar>(target.second, target.first) == 0) {
-    std::cerr << "BFS: Target position is blocked!" << std::endl;
+  if (grid.at<uchar>(target.second, target.first) == 0)
     return {};
-  }
 
   cv::Mat visited(grid.rows, grid.cols, CV_8UC1, cv::Scalar(0));
   cv::Mat parent(grid.rows, grid.cols, CV_32SC2, cv::Scalar(-1, -1));
 
-  std::queue<std::pair<int, int>> queue;
+  std::queue<std::pair<int, int>> q;
 
   visited.at<uchar>(start.second, start.first) = 1;
-  queue.push(start);
+  q.push(start);
 
   std::vector<std::pair<int, int>> dirs = {{0, -1},  {0, 1},  {-1, 0}, {1, 0},
                                            {-1, -1}, {-1, 1}, {1, -1}, {1, 1}};
 
-  while (!queue.empty()) {
-    auto current = queue.front();
-    queue.pop();
+  while (!q.empty()) {
+
+    auto cur = q.front();
+    q.pop();
 
-    if (current == target) {
+    if (cur == target) {
       std::vector<std::pair<int, int>> path;
-      auto node = target;
+      auto n = target;
+
       while (true) {
-        path.push_back(node);
-        cv::Vec2i p = parent.at<cv::Vec2i>(node.second, node.first);
-        if (p[0] == -1) {
+        path.push_back(n);
+        cv::Vec2i p = parent.at<cv::Vec2i>(n.second, n.first);
+        if (p[0] == -1)
           break;
-        }
-        node = {p[0], p[1]};
+        n = {p[0], p[1]};
       }
+
       std::reverse(path.begin(), path.end());
       return path;
     }
 
-    for (const std::pair<int, int>& d : dirs) {
-      int nx = current.first + d.first;
-      int ny = current.second + d.second;
+    for (auto& d : dirs) {
+
+      int nx = cur.first + d.first;
+      int ny = cur.second + d.second;
 
-      if (nx < 0 || nx >= grid.cols || ny < 0 || ny >= grid.rows) {
+      if (nx < 0 || ny < 0 || nx >= grid.cols || ny >= grid.rows)
         continue;
+      if (grid.at<uchar>(ny, nx) == 0)
+        continue;
+
+      if (d.first != 0 && d.second != 0) {
+        if (grid.at<uchar>(cur.second, nx) == 0)
+          continue;
+        if (grid.at<uchar>(ny, cur.first) == 0)
+          continue;
       }
 
-      if (grid.at<uchar>(ny, nx) == 0 || visited.at<uchar>(ny, nx) == 1) {
+      if (visited.at<uchar>(ny, nx))
         continue;
-      }
 
       visited.at<uchar>(ny, nx) = 1;
-      parent.at<cv::Vec2i>(ny, nx) = cv::Vec2i(current.first, current.second);
-      queue.push({nx, ny});
+      parent.at<cv::Vec2i>(ny, nx) = cv::Vec2i(cur.first, cur.second);
+
+      q.push({nx, ny});
     }
   }
 
   return {};
 }
 
-auto constructLinePath(cv::Mat& canvas, std::vector<std::pair<int, int>> path)
-    -> std::vector<std::pair<int, int>> {
-  std::vector<std::pair<int, int>> controlPoints;
-  for (size_t i = 0; i < path.size(); ++i) {
-    int px = path[i].first * CELL_SIZE + (CELL_SIZE / 2);
-    int py = path[i].second * CELL_SIZE + (CELL_SIZE / 2);
-
-    controlPoints.emplace_back(px, py);
-
-    if (i > 0) {
-      int prevX = path[i - 1].first * CELL_SIZE + (CELL_SIZE / 2);
-      int prevY = path[i - 1].second * CELL_SIZE + (CELL_SIZE / 2);
-      cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0},
-               2);
-    }
-  }
-  return controlPoints;
-}
+auto createSpline(cv::Mat& grid, int sx, int sy, int tx, int ty,
+                  double nodeSize) -> std::vector<frc::Pose2d> {
 
-auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
-  std::vector<double> knots;
-  int n = (int)k;
-  int d = (int)p;
+  auto path = BFS(grid, {sx, sy}, {tx, ty});
 
-  for (int i = 0; i <= d; ++i) {
-    knots.push_back(0.0);
-  }
+  if (path.empty())
+    return {};
 
-  int middle = n - d - 1;
-  for (int i = 1; i <= middle; ++i) {
-    knots.push_back((double)i / (middle + 1));
-  }
+  std::vector<std::pair<double, double>> pts;
+  pts.reserve(path.size());
 
-  for (int i = 0; i <= d; ++i) {
-    knots.push_back(1.0);
+  for (auto& p : path) {
+    pts.emplace_back(p.first * nodeSize + nodeSize / 2,
+                     p.second * nodeSize + nodeSize / 2);
   }
 
-  return knots;
-}
+  std::vector<frc::Pose2d> traj;
 
-auto basisFunction(double i, double p, double t,
-                   const std::vector<double>& knots) -> double {
-  int idx = static_cast<int>(i);
-  int deg = static_cast<int>(p);
+  double spacing = 0.05;
 
-  if (deg == 0) {
-    if (p == 0) {
-      if (knots[i] <= t && t < knots[i + 1]) {
-        return 1.0;
-      }
+  for (size_t i = 1; i < pts.size(); i++) {
 
-      if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
-        return 1.0;
-      }
-
-      if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
-        return 1.0;
-      }
-
-      return 0.0;
-    }
-  }
+    double dx = pts[i].first - pts[i - 1].first;
+    double dy = pts[i].second - pts[i - 1].second;
 
-  double weight = 0.0;
+    double len = std::hypot(dx, dy);
 
-  double denom1 = knots[idx + deg] - knots[idx];
-  if (denom1 != 0) {
-    weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
-  }
-  double denom2 = knots[idx + deg + 1] - knots[idx + 1];
-  if (denom2 != 0) {
-    weight += ((knots[idx + deg + 1] - t) / denom2) *
-              basisFunction(i + 1, p - 1, t, knots);
-  }
-  return weight;
-}
+    int steps = std::max(1, (int)(len / spacing));
 
-auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points,
-                    const std::vector<double>& knots, int p)
-    -> std::pair<double, double> {
-  double px = 0.0, py = 0.0;
-  for (size_t i = 0; i < points.size(); ++i) {
-    double weight = basisFunction((double)i, (double)p, t, knots);
-    px += points[i].first * weight;
-    py += points[i].second * weight;
-  }
-  return {px, py};
-}
+    for (int s = 0; s < steps; s++) {
 
-auto createSpline(cv::Mat& grid, int start_x, int start_y, int target_x,
-                  int target_y, double nodeSizeMeters)
-    -> std::vector<frc::Pose2d> {
-  auto path = BFS(grid, {start_x, start_y}, {target_x, target_y});
-  if (path.empty()) {
-    return {};
-  }
+      double t = (double)s / steps;
 
-  auto controlPoints = constructLinePath(grid, path);
+      double x = pts[i - 1].first + dx * t;
+      double y = pts[i - 1].second + dy * t;
 
-  if (controlPoints.size() < 4) {
-    return {};
+      traj.push_back(frc::Pose2d(units::meter_t{x}, units::meter_t{y},
+                                 units::radian_t{0}));
+    }
   }
 
-  auto knots = clampedUniformKnotVector(controlPoints.size(), 3);
-
-  std::vector<frc::Pose2d> trajectory;
-  int resolution = 200;
-  for (int i = 0; i < resolution; ++i) {
-    double t = (double)i / resolution;
-    std::pair<double, double> point =
-        getSplinePoint(t, controlPoints, knots, 3);
-    double px = point.first / CELL_SIZE;
-    double py = point.second / CELL_SIZE;
-    trajectory.push_back(frc::Pose2d(units::meter_t{px * nodeSizeMeters},
-                                     units::meter_t{py * nodeSizeMeters},
-                                     units::radian_t{0.0}));
-  }
+  traj.push_back(frc::Pose2d(units::meter_t{pts.back().first},
+                             units::meter_t{pts.back().second},
+                             units::radian_t{0}));
 
-  return trajectory;
+  return traj;
 }
\ No newline at end of file
diff --git a/src/pathing/pathing.h b/src/pathing/pathing.h
index 13a8bad..1cc384a 100644
--- a/src/pathing/pathing.h
+++ b/src/pathing/pathing.h
@@ -1,26 +1,15 @@
 #pragma once
 
 #include <frc/geometry/Pose2d.h>
+#include <units/angle.h>
+#include <units/length.h>
 #include <opencv2/opencv.hpp>
 #include <vector>
 
-auto initializeGrid(const std::vector<std::vector<bool>>& gridData) -> cv::Mat;
+cv::Mat initializeGrid(const std::vector<std::vector<bool>>& gridData);
 
 auto BFS(const cv::Mat& grid, std::pair<int, int> start,
          std::pair<int, int> target) -> std::vector<std::pair<int, int>>;
 
-auto constructLinePath(cv::Mat& canvas, std::vector<std::pair<int, int>> path)
-    -> std::vector<std::pair<int, int>>;
-
-auto clampedUniformKnotVector(double k, double p) -> std::vector<double>;
-
-auto basisFunction(double i, double p, double t,
-                   const std::vector<double>& knots) -> double;
-
-auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points,
-                    const std::vector<double>& knots, int p)
-    -> std::pair<double, double>;
-
-auto createSpline(cv::Mat& grid, int start_x, int start_y, int target_x,
-                  int target_y, double nodeSizeMeters)
-    -> std::vector<frc::Pose2d>;
+auto createSpline(cv::Mat& grid, int sx, int sy, int tx, int ty,
+                  double nodeSize) -> std::vector<frc::Pose2d>;
\ No newline at end of file
diff --git a/src/pathing/simulator.cc b/src/pathing/simulator.cc
index 45b391e..4a7a6ec 100644
--- a/src/pathing/simulator.cc
+++ b/src/pathing/simulator.cc
@@ -45,7 +45,7 @@ auto main() -> int {
     return 1;
   }
 
-  constexpr int64_t kDtUs = 20'000;
+  constexpr int64_t kDtUs = 100'000;
   constexpr double kDtSec = kDtUs / 1'000'000.0;
   constexpr double kMaxAccel = 3.0;
   constexpr double kMaxDecel = 3.0;
@@ -93,12 +93,70 @@ auto main() -> int {
   double currentY = poses[0].Y().value();
   double currentSpeed = 0.0;
 
-  for (size_t i = 0; i < poses.size(); ++i) {
-    frc::Pose2d actualPose{units::meter_t{currentX}, units::meter_t{currentY},
-                           poses[i].Rotation()};
+  constexpr double kLookaheadDist = 0.5;  // meters
+  constexpr double kGoalTolerance = 0.1;  // meters
+  constexpr int kFlushInterval = 1;       // 10 iterations
+
+  size_t currentWaypoint = 0;
+  int iterationCount = 0;
+
+  while (true) {
+    double robotX = currentX;
+    double robotY = currentY;
+
+    frc::Pose2d actualPose{
+        units::meter_t{robotX}, units::meter_t{robotY},
+        poses[std::min(currentWaypoint, poses.size() - 1)].Rotation()};
     poseLog.Append(actualPose, t);
 
-    double desiredSpeed = targetSpeed[i];
+    size_t targetIdx = currentWaypoint;
+    double minDist = INFINITY;
+
+    for (size_t i = currentWaypoint; i < poses.size(); ++i) {
+      double dx = poses[i].X().value() - robotX;
+      double dy = poses[i].Y().value() - robotY;
+      double dist = std::sqrt(dx * dx + dy * dy);
+
+      if (dist >= kLookaheadDist) {
+        targetIdx = i;
+        break;
+      }
+
+      if (dist < minDist) {
+        minDist = dist;
+        currentWaypoint = i;
+      }
+    }
+
+    // Check if we reached the goal
+    double goalDx = poses.back().X().value() - robotX;
+    double goalDy = poses.back().Y().value() - robotY;
+    double goalDist = std::sqrt(goalDx * goalDx + goalDy * goalDy);
+
+    if (goalDist < kGoalTolerance) {
+
+      accelXLog.Append(0.0, t);
+      accelYLog.Append(0.0, t);
+      accelMagLog.Append(0.0, t);
+      velXLog.Append(0.0, t);
+      velYLog.Append(0.0, t);
+      break;
+    }
+
+    if (targetIdx >= poses.size() - 1) {
+      targetIdx = poses.size() - 1;
+    }
+
+    double targetX = poses[targetIdx].X().value();
+    double targetY = poses[targetIdx].Y().value();
+    double dx = targetX - robotX;
+    double dy = targetY - robotY;
+    double distToTarget = std::sqrt(dx * dx + dy * dy);
+
+    double dirX = distToTarget > 0.001 ? dx / distToTarget : 0.0;
+    double dirY = distToTarget > 0.001 ? dy / distToTarget : 0.0;
+
+    double desiredSpeed = targetSpeed[targetIdx];
 
     double dvMag = desiredSpeed - currentSpeed;
     double accelMag = 0.0;
@@ -112,36 +170,31 @@ auto main() -> int {
     currentSpeed += accelMag * kDtSec;
     currentSpeed = std::max(0.0, currentSpeed);
 
-    if (i > 0) {
-      double dx = poses[i].X().value() - poses[i - 1].X().value();
-      double dy = poses[i].Y().value() - poses[i - 1].Y().value();
-      double segDist = std::sqrt(dx * dx + dy * dy);
+    double newVx = dirX * currentSpeed;
+    double newVy = dirY * currentSpeed;
 
-      double dirX = segDist > 0.001 ? dx / segDist : 0.0;
-      double dirY = segDist > 0.001 ? dy / segDist : 0.0;
+    double ax = (newVx - currentVx) / kDtSec;
+    double ay = (newVy - currentVy) / kDtSec;
 
-      double newVx = dirX * currentSpeed;
-      double newVy = dirY * currentSpeed;
+    currentVx = newVx;
+    currentVy = newVy;
 
-      double ax = (newVx - currentVx) / kDtSec;
-      double ay = (newVy - currentVy) / kDtSec;
+    currentX += currentVx * kDtSec;
+    currentY += currentVy * kDtSec;
 
-      currentVx = newVx;
-      currentVy = newVy;
+    double accelMagTotal = std::sqrt(ax * ax + ay * ay);
 
-      currentX += currentVx * kDtSec;
-      currentY += currentVy * kDtSec;
+    accelXLog.Append(ax, t);
+    accelYLog.Append(ay, t);
+    accelMagLog.Append(accelMagTotal, t);
+    velXLog.Append(currentVx, t);
+    velYLog.Append(currentVy, t);
 
-      double accelMagTotal = std::sqrt(ax * ax + ay * ay);
+    t += kDtUs;
 
-      accelXLog.Append(ax, t);
-      accelYLog.Append(ay, t);
-      accelMagLog.Append(accelMagTotal, t);
-      velXLog.Append(currentVx, t);
-      velYLog.Append(currentVy, t);
+    if (++iterationCount % kFlushInterval == 0) {
+      log.Flush();
     }
-
-    t += kDtUs;
   }
 
   log.Flush();
diff --git a/src/pathing/velocity_sender.cc b/src/pathing/velocity_sender.cc
new file mode 100644
index 0000000..b9850e4
--- /dev/null
+++ b/src/pathing/velocity_sender.cc
@@ -0,0 +1,30 @@
+#include "src/pathing/velocity_sender.h"
+#include <networktables/DoubleTopic.h>
+#include <networktables/StructTopic.h>
+#include <units/angle.h>
+#include "frc/DataLogManager.h"
+
+namespace pathing {
+
+constexpr auto RadianToDegree(double radian) -> double {
+  return radian * (180 / M_PI);
+}
+
+VelocitySender::VelocitySender()
+    : instance_(nt::NetworkTableInstance::GetDefault()) {
+  std::shared_ptr<nt::NetworkTable> table =
+      instance_.GetTable("Orin/OTFVelocity/");
+
+  nt::DoubleArrayTopic vel_topic = table->GetDoubleArrayTopic("Velocity");
+  vel_publisher_ = vel_topic.Publish();
+}
+
+void VelocitySender::Send(const double ax, const double bx) {
+  if (mutex_.try_lock()) {
+    std::vector<double> vel_array = {ax, bx};
+    vel_publisher_.Set(vel_array);
+  }
+
+  mutex_.unlock();
+}
+}  // namespace pathing
diff --git a/src/pathing/velocity_sender.h b/src/pathing/velocity_sender.h
new file mode 100644
index 0000000..1e06d2d
--- /dev/null
+++ b/src/pathing/velocity_sender.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/DoubleArrayTopic.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructTopic.h>
+#include "src/utils/pch.h"
+
+namespace pathing {
+class VelocitySender {
+ public:
+  VelocitySender();
+  void Send(const double ax, const double ay);
+
+ private:
+  nt::NetworkTableInstance instance_;
+
+  nt::DoubleArrayPublisher vel_publisher_;
+
+  std::mutex mutex_;
+};
+}  // namespace pathing
\ No newline at end of file