diff --git a/src/v5_hal/firmware/include/nodes/TankDriveNode.h b/src/v5_hal/firmware/include/nodes/TankDriveNode.h
index 1752031..871940d 100644
--- a/src/v5_hal/firmware/include/nodes/TankDriveNode.h
+++ b/src/v5_hal/firmware/include/nodes/TankDriveNode.h
@@ -31,12 +31,8 @@ class TankDriveNode : public IDriveNode {
 
     virtual void setDriveVoltage(int x_voltage, int theta_voltage);
 
-    virtual void setDriveVoltage(int x_voltage, int y_voltage, int theta_voltage);
-
     virtual void setDriveVelocity(float x_velocity, float theta_velocity);
 
-    virtual void setDriveVelocity(float x_velocity, float y_velocity, float theta_velocity);
-
     virtual void teleopPeriodic();
 
     virtual void autonPeriodic();
@@ -52,4 +48,17 @@ class TankDriveNode : public IDriveNode {
 
     TankDriveKinematics m_kinematics;
 
+    Eigen::Vector2d controller_target_velocity;
+
+    void m_setLeftPosition(float distance, int max_velocity);
+
+    void m_setRightPosition(float distance, int max_velocity);
+
+    void m_setLeftVoltage(int voltage);
+
+    void m_setRightVoltage(int voltage);
+
+    void m_setLeftVelocity(float velocity);
+
+    void m_setRightVelocity(float velocity);
 };
diff --git a/src/v5_hal/firmware/lib b/src/v5_hal/firmware/lib
index 3495f6a..afe8187 160000
--- a/src/v5_hal/firmware/lib
+++ b/src/v5_hal/firmware/lib
@@ -1 +1 @@
-Subproject commit 3495f6ab2d77c8a506f8f83a60e98f2fcdfca71a
+Subproject commit afe8187d5013d189b03d114e3bfa3667ebc5f4b8
diff --git a/src/v5_hal/firmware/src/nodes/HolonomicDriveNode.cpp b/src/v5_hal/firmware/src/nodes/HolonomicDriveNode.cpp
index 7cb77fd..9bdb9ce 100644
--- a/src/v5_hal/firmware/src/nodes/HolonomicDriveNode.cpp
+++ b/src/v5_hal/firmware/src/nodes/HolonomicDriveNode.cpp
@@ -98,8 +98,7 @@ void HolonomicDriveNode::setDriveVoltage(int x_voltage, int theta_voltage) {
 }
 
 void HolonomicDriveNode::setDriveVoltage(int x_voltage, int y_voltage, int theta_voltage) {
-    IDriveKinematics::FourMotorPercentages motor_percentages = 
-        m_kinematics.inverseKinematics(x_voltage, y_voltage, theta_voltage, MAX_MOTOR_VOLTAGE);
+    IDriveKinematics::FourMotorPercentages motor_percentages = m_kinematics.inverseKinematics(x_voltage, y_voltage, theta_voltage, MAX_MOTOR_VOLTAGE);
 
     m_setLeftFrontVoltage(motor_percentages.left_front_percent * MAX_MOTOR_VOLTAGE);
     m_setLeftRearVoltage(motor_percentages.left_rear_percent * MAX_MOTOR_VOLTAGE);
@@ -109,7 +108,7 @@ void HolonomicDriveNode::setDriveVoltage(int x_voltage, int y_voltage, int theta
 
 void HolonomicDriveNode::setDriveVoltage(int left_x, int left_y, int right_x, int right_y) {
     IDriveKinematics::FourMotorPercentages motor_percentages = 
-        m_kinematics.tankKinematics(0, left_y, 0, right_y, MAX_MOTOR_VOLTAGE);
+        m_kinematics.simpleKinematics(0, left_y, 0, right_y, MAX_MOTOR_VOLTAGE);
 
     m_setLeftFrontVoltage(motor_percentages.left_front_percent * MAX_MOTOR_VOLTAGE);
     m_setLeftRearVoltage(motor_percentages.left_rear_percent * MAX_MOTOR_VOLTAGE);
diff --git a/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp b/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp
index 66accd4..8634d49 100644
--- a/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp
+++ b/src/v5_hal/firmware/src/nodes/TankDriveNode.cpp
@@ -9,73 +9,71 @@ TankDriveNode::TankDriveNode(NodeManager* node_manager, std::string handle_name,
     m_handle_name = handle_name.insert(0, "robot/");
 }
 
-void TankDriveNode::resetEncoders() {
-    m_motors.left_1_motor->resetEncoder();
-    m_motors.left_2_motor->resetEncoder();
-    m_motors.left_3_motor->resetEncoder();
-    m_motors.left_4_motor->resetEncoder();
-    m_motors.right_1_motor->resetEncoder();
-    m_motors.right_2_motor->resetEncoder();
-    m_motors.right_3_motor->resetEncoder();
-    m_motors.right_4_motor->resetEncoder();
+void TankDriveNode::m_setLeftPosition(float distance, int max_velocity) {
+    m_motors.left_1_motor->moveAbsolute(distance, max_velocity);
+    m_motors.left_2_motor->moveAbsolute(distance, max_velocity);
+    m_motors.left_3_motor->moveAbsolute(distance, max_velocity);
+    m_motors.left_4_motor->moveAbsolute(distance, max_velocity);
+}
+
+void TankDriveNode::m_setRightPosition(float distance, int max_velocity) {
+    m_motors.right_1_motor->moveAbsolute(distance, max_velocity);
+    m_motors.right_2_motor->moveAbsolute(distance, max_velocity);
+    m_motors.right_3_motor->moveAbsolute(distance, max_velocity);
+    m_motors.right_4_motor->moveAbsolute(distance, max_velocity);
 }
 
 void TankDriveNode::m_setLeftVoltage(int voltage) {
-    m_left_front_motor->moveVoltage(voltage);
-    m_left_rear_motor->moveVoltage(voltage);
+    m_motors.left_1_motor->moveVoltage(voltage);
+    m_motors.left_2_motor->moveVoltage(voltage);
+    m_motors.left_3_motor->moveVoltage(voltage);
+    m_motors.left_4_motor->moveVoltage(voltage);
 }
 
 void TankDriveNode::m_setRightVoltage(int voltage) {
-    m_right_front_motor->moveVoltage(voltage);
-    m_right_rear_motor->moveVoltage(voltage);
+    m_motors.right_1_motor->moveVoltage(voltage);
+    m_motors.right_2_motor->moveVoltage(voltage);
+    m_motors.right_3_motor->moveVoltage(voltage);
+    m_motors.right_4_motor->moveVoltage(voltage);
 }
 
 void TankDriveNode::m_setLeftVelocity(float velocity) {
-    m_left_front_motor->moveVelocity(velocity);
-    m_left_rear_motor->moveVelocity(velocity);
+    m_motors.left_1_motor->moveVelocity(velocity);
+    m_motors.left_2_motor->moveVelocity(velocity);
+    m_motors.left_3_motor->moveVelocity(velocity);
+    m_motors.left_4_motor->moveVelocity(velocity);
 }
 
 void TankDriveNode::m_setRightVelocity(float velocity) {
-    m_right_front_motor->moveVelocity(velocity);
-    m_right_rear_motor->moveVelocity(velocity);
-}
-
-void TankDriveNode::m_setLeftDistancePosition(double distance, int max_velocity) {
-    m_left_front_motor->moveAbsolute(distance, max_velocity);
-    m_left_rear_motor->moveAbsolute(distance, max_velocity);
-}
-
-void TankDriveNode::m_setRightDistancePosition(double distance, int max_velocity) {
-    m_right_front_motor->moveAbsolute(distance, max_velocity);
-    m_right_rear_motor->moveAbsolute(distance, max_velocity);
-}
-
-int TankDriveNode::getLeftDistancePosition() {
-    return m_left_front_motor->getPosition();
+    m_motors.right_1_motor->moveVelocity(velocity);
+    m_motors.right_2_motor->moveVelocity(velocity);
+    m_motors.right_3_motor->moveVelocity(velocity);
+    m_motors.right_4_motor->moveVelocity(velocity);
 }
 
-int TankDriveNode::getRightDistancePosition() {
-    return m_right_front_motor->getPosition();
+void TankDriveNode::resetEncoders() {
+    m_motors.left_1_motor->resetEncoder();
+    m_motors.left_2_motor->resetEncoder();
+    m_motors.left_3_motor->resetEncoder();
+    m_motors.left_4_motor->resetEncoder();
+    m_motors.right_1_motor->resetEncoder();
+    m_motors.right_2_motor->resetEncoder();
+    m_motors.right_3_motor->resetEncoder();
+    m_motors.right_4_motor->resetEncoder();
 }
 
 void TankDriveNode::initialize() {
     resetEncoders();
 }
 
-void TankDriveNode::setDriveVoltage(int left_voltage, int right_voltage) {
-    m_setLeftVoltage(left_voltage);
-    m_setRightVoltage(right_voltage);
-}
+void TankDriveNode::setDriveVoltage(int x_voltage, int theta_voltage) {
+    IDriveKinematics::FourMotorPercentages motor_percentages = m_kinematics.inverseKinematics(x_voltage, 0, theta_voltage, MAX_MOTOR_VOLTAGE);
 
-void TankDriveNode::setDriveVelocity(float left_velocity, float right_velocity) { //incoming values should be in m/s so we convert to rpm here
-    m_setLeftVelocity(left_velocity / MAX_ROBOT_SPEED * 200);
-    m_setRightVelocity(right_velocity / MAX_ROBOT_SPEED * 200);
+    
 }
 
-void TankDriveNode::setDriveDistancePID(double left_distance, double right_distance, int max_velocity) {
-    resetEncoders();
-    m_setLeftDistancePID(left_distance, max_velocity);
-    m_setRightDistancePID(right_distance, max_velocity);
+void TankDriveNode::setDriveVelocity(float x_velocity, float theta_velocity) { //incoming values should be in m/s so we convert to rpm here
+    // 
 }
 
 void TankDriveNode::teleopPeriodic() {
@@ -88,8 +86,5 @@ void TankDriveNode::autonPeriodic() {
 }
 
 TankDriveNode::~TankDriveNode() {
-    delete m_left_front_motor;
-    delete m_left_rear_motor;
-    delete m_right_front_motor;
-    delete m_right_rear_motor;
+    
 }
\ No newline at end of file