diff --git a/.gitignore b/.gitignore
index 159790a3..76a69cc1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,6 +9,7 @@ devel/
 /.vscode/*
 # except recommended extensions
 !/.vscode/extensions.json
+!/.vscode/settings.json
 
 # fetched external libs
 # NOTE: these may be removed in the future, tbd
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 00000000..48600c15
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,14 @@
+{
+    "clang-tidy.checks": [
+        "bugprone-*",
+        "performance-*",
+        "readability-*",
+        "modernize-*",
+        "hicpp-*",
+        "cppcoreguidelines-*",
+        "clang-analyzer-*",
+        "-readability-braces-around-statements",
+        "-hicpp-braces-around-statements",
+        "-cppcoreguidelines-non-private-member-variables-in-classes"
+    ]
+}
\ No newline at end of file
diff --git a/project.json b/project.json
index 8dd30293..0bbb2578 100644
--- a/project.json
+++ b/project.json
@@ -34,6 +34,13 @@
         "package": "ros-noetic-moveit",
         "provider": "apt"
       }
+    },
+    {
+      "name": "moveit-visual-tools",
+      "build": {
+        "package": "ros-noetic-moveit-visual-tools",
+        "provider": "apt"
+      }
     }
   ]
 }
diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt
index d3f0ce92..c9d43c92 100644
--- a/src/wr_control_drive_arm/CMakeLists.txt
+++ b/src/wr_control_drive_arm/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(wr_control_drive_arm)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
diff --git a/src/wr_control_drive_arm/config/encoder_parameters.yaml b/src/wr_control_drive_arm/config/encoder_parameters.yaml
index 9421827f..76e5cbd2 100644
--- a/src/wr_control_drive_arm/config/encoder_parameters.yaml
+++ b/src/wr_control_drive_arm/config/encoder_parameters.yaml
@@ -3,17 +3,17 @@ encoder_parameters:
   - counts_per_rotation: -2048
     offset: 736
 # forearm_roll
-  - counts_per_rotation: 2048
-    offset: 0
+  - counts_per_rotation: -2048
+    offset: 600
 # shoulder
   - counts_per_rotation: -6144
-    offset: 460
+    offset: 1450
 # turntable
   - counts_per_rotation: 2048
-    offset: 1111
+    offset: 736
 # wrist_pitch
   - counts_per_rotation: 2048
-    offset: 350
+    offset: 696
 # wrist_roll
   - counts_per_rotation: 2048
-    offset: 1023
+    offset: 1045
diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp
index 2c42679b..f425eeb6 100644
--- a/src/wr_control_drive_arm/src/AbstractJoint.cpp
+++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp
@@ -1,40 +1,41 @@
 /**
  * @file AbstractJoint.cpp
  * @author Nichols Underwood
- * @brief ablskjlfkejfs
+ * @brief Implementation of the AbstractJoint class
  * @date 2021-10-25
  */
+ 
 #include "AbstractJoint.hpp"
 
-AbstractJoint::AbstractJoint(ros::NodeHandle* n, int numMotors){
-    this->n = n;
-    this->numMotors = numMotors;
-
-    jointPositions.reserve(numMotors);
-    jointVelocites.reserve(numMotors);
-
+AbstractJoint::AbstractJoint(ros::NodeHandle &n, int numMotors){
+    this->motors.resize(numMotors);
 }
 
-int AbstractJoint::getDegreesOfFreedom(){
-    return this->numMotors;
+auto AbstractJoint::getDegreesOfFreedom() const -> unsigned int{
+    return this->motors.size();
 }
 
-ArmMotor* AbstractJoint::getMotor(int motorIndex){
-    return this->motors[motorIndex];
+auto AbstractJoint::getMotor(int motorIndex) const -> const std::unique_ptr<ArmMotor>&{
+    return this->motors.at(motorIndex).motor;
 }
 
 void AbstractJoint::configSetpoint(int degreeIndex, double position, double velocity){
 
-    this->jointPositions[degreeIndex] = position;
-    this->jointVelocites[degreeIndex] = velocity;
+    this->motors.at(degreeIndex).position = position;
+    this->motors.at(degreeIndex).velocity = velocity;
 }
 
 
-bool AbstractJoint::exectute(){
-    vector<double> motorPositions;
-    this->getMotorPositions(jointPositions, motorPositions);
-    for(int i = 0; i < this->numMotors; i++){
-        if (!this->motors[i]->runToTarget(motorPositions[i], 0)) return false;
+auto AbstractJoint::exectute() -> bool{
+    for(auto &motorHandle : motors){
+        if (!motorHandle.motor->runToTarget(motorHandle.position, motorHandle.velocity)); 
+            return false;
     }
     return true;
+}
+
+void AbstractJoint::stopJoint(){
+    for(auto &motorHandle : motors){
+        motorHandle.motor->setPower(0.F);
+    }
 }
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp
index 978345e9..ca4d22ac 100644
--- a/src/wr_control_drive_arm/src/AbstractJoint.hpp
+++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp
@@ -1,3 +1,10 @@
+/**
+ * @file AbstractJoint.hpp
+ * @author Nichols Underwood
+ * @brief Header file of the AbstractJoint class
+ * @date 2021-10-25
+ */
+
 #ifndef ABSTRACT_JOINT_GUARD
 #define ABSTRACT_JOINT_GUARD
 
@@ -5,39 +12,35 @@
 using std::vector;
 
 class AbstractJoint {
+public:
+    struct MotorHandler{
+        std::unique_ptr<ArmMotor> motor;
+        double position;
+        double velocity;
+        std::string jointTopicName;
+        std::string motorTopicName;
+        bool newVelocity;
+    };
 
+    AbstractJoint(ros::NodeHandle &n, int numMotors);
 
-    protected:
-        ros::NodeHandle* n;
-        unsigned int numMotors;
-        vector<ArmMotor*> motors;
-
-        vector<double> jointPositions;   
-        vector<double> jointVelocites;
-
-        vector<std::string> jointTopicsNames;  
-        vector<std::string> motorTopicsNames;  
-        vector<bool> newVelocitiesVector;
-
-    public:
-
-        AbstractJoint(ros::NodeHandle* n, int numMotors);
-        ~AbstractJoint(){};
+    // never used, need to be defined for compiler v-table
+    virtual auto getMotorPositions(const vector<double> &jointPositions) -> vector<double> = 0;
+    virtual auto getMotorVelocities(const vector<double> &joinVelocities) -> vector<double> = 0;
+    virtual auto getJointPositions(const vector<double> &motorPositions) -> vector<double> = 0;
 
-        // never used, need to be defined for compiler v-table
-        virtual void getMotorPositions(vector<double> &jointPositions, vector<double> &target) = 0;
-        virtual void getMotorVelocities(vector<double> &joinVelocities, vector<double> &target) = 0;
-        virtual void getJointPositions(vector<double> &motorPositions, vector<double> &target) = 0;
+    auto getDegreesOfFreedom() const -> unsigned int;
+    
+    auto getMotor(int motorIndex) const -> const std::unique_ptr<ArmMotor>&;
 
-        int getDegreesOfFreedom();
-        
-        ArmMotor* getMotor(int motorIndex);
+    void configSetpoint(int degreeIndex, double position, double velocity);
 
-        void configSetpoint(int degreeIndex, double position, double velocity);
+    auto exectute() -> bool;
 
-        bool exectute();
+    void stopJoint();
 
-        // virtual void configVelocityHandshake(std::string, std::string) = 0;
+protected:
+    vector<MotorHandler> motors;
 };
 
 #endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp
index 68f29ed6..e6136031 100644
--- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp
+++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp
@@ -15,6 +15,8 @@
 #include <algorithm>
 #include <csignal>
 #include <string>
+#include <array>
+#include <memory>
 #include <std_srvs/Trigger.h>
 #include "SimpleJoint.hpp"
 #include "DifferentialJoint.hpp"
@@ -23,16 +25,31 @@
 using XmlRpc::XmlRpcValue;
 
 /**
- * @brief Defines space for all ArmMotor references
+ * @brief Refresh rate of ros::Rate
  */
-const int numMotors = 6;
-ArmMotor *motors[numMotors];
+constexpr float CLOCK_RATE = 50;
+
+constexpr double IK_WARN_RATE = 1.0/2;
+
+constexpr double JOINT_SAFETY_MAX_SPEED = 0.3;
+constexpr double JOINT_SAFETY_HOLD_SPEED = 0.15;
+
+/**
+ * @brief Nessage cache size of publisher
+ */
+constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1000; 
+
+/**
+ * @brief Period between timer callback
+ */
+constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0;
 
 /**
  * @brief Defines space for all Joint references
  */
-const int numJoints = 5;
-AbstractJoint *joints[numJoints];
+constexpr int NUM_JOINTS = 5;
+std::array<std::unique_ptr<AbstractJoint>, NUM_JOINTS> joints;
+// AbstractJoint *joints[numJoints];
 /**
  * @brief The Joint State Publisher for MoveIt
  */
@@ -54,44 +71,6 @@ std::atomic_bool IKEnabled{true};
  */
 ros::ServiceClient enableServiceClient;
 
-/**
- * @brief converts a roll and pitch to joint motor positions
- * 
- * @param roll the roll of the joint in radians
- * @param pitch the pitch of the joint in radians
- * @return a 2D vector with motor positions in radians
- */
-std::vector<double> convertJointSpacetoEncoderSpace(double roll, double pitch){
-	std::vector<double> encoderSpace(2);
-
-	//TODO: add matrix transformation, implement joint in mock.py
-	encoderSpace[0] = roll;
-	encoderSpace[1] = pitch;
-
-	return encoderSpace;
-}
-
-/**
- * @brief sets a target position and pulls info from motor
- * 
- * @param names a vector with motor joint names
- * @param positions a vector with positions in radians
- * @param target the target radian value
- * @param motor a pointer to the motor
- * @return if the motor has reached its target
- */
-bool configJointSetpoint(AbstractJoint* joint, int degreeIndex, double target, float velocity){
-	// Each motor should run to its respective target position at a fixed speed
-	// TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters
-	// velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)<abs(b);}));
-	// float currPower = 0.1 * currTargetPosition.velocities[j]/velMax;
-	// currPower = abs(velMax) <= 0.0001 ? 0.1 : currPower;
-  std::cout << "config joint setup: " << degreeIndex << " " << target << std::endl;
-	joint->configSetpoint(degreeIndex, target, 0);
-	// The position has only finished if every motor is STOPped
-	return joint->getMotor(degreeIndex)->getMotorState() == MotorState::STOP;
-}
-
 /**
  * @brief Perform the given action as interpreted as moving the arm joints to specified positions
  * 
@@ -101,50 +80,46 @@ bool configJointSetpoint(AbstractJoint* joint, int degreeIndex, double target, f
 void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) {
   if (!IKEnabled) {
     as->setAborted();
-    ROS_WARN("IK is disabled");
+    ROS_WARN_THROTTLE(IK_WARN_RATE, "IK is disabled"); // NOLINT(hicpp-no-array-decay,hicpp,hicpp-vararg,cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg)
     return;
   }
 
   std::cout << "start exec: " << goal->trajectory.points.size() << std::endl;
   // For each point in the trajectory execution sequence...
-  for(int i = 0; i < goal->trajectory.points.size(); i++){
-    std::cout << "PID to trajectory point " << i << "/" <<  goal->trajectory.points.size() << std::endl;
-    // Capture the current goal for easy reference
-    trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i];
-
+  for(const auto &currTargetPosition : goal->trajectory.points){
     // Track whether or not the current position is done
     bool hasPositionFinished = false;
     // Keep max loop rate at 50 Hz
-    ros::Rate loop(200);
+    ros::Rate loop{CLOCK_RATE};
+
+    const double VELOCITY_MAX = abs(*std::max_element(
+        currTargetPosition.velocities.begin(),
+        currTargetPosition.velocities.end(),
+        [](double a, double b) -> bool{ return abs(a)<abs(b); }
+      ));
+
+    int currItr = 0;
+    for(const auto &joint : joints){
+      for(int i = 0; i < joint->getDegreesOfFreedom(); i++){
+        joint->configSetpoint(i, currTargetPosition.positions[currItr], VELOCITY_MAX == 0.F ? JOINT_SAFETY_HOLD_SPEED : JOINT_SAFETY_MAX_SPEED*currTargetPosition.velocities[currItr]/VELOCITY_MAX);
+        currItr++;
+      }
+      joint->exectute();
+    }
 
     // While the current position is not complete yet...
     while(!hasPositionFinished){
       // Assume the current action is done until proven otherwise
       hasPositionFinished = true;
       // Create the Joint State message for the current update cycle
-      double velMax = abs(*std::max_element(
-        currTargetPosition.velocities.begin(),
-        currTargetPosition.velocities.end(),
-        [](double a, double b) { return abs(a)<abs(b); }
-      ));
-      int motorIndex = 0;
-      int jointIndex = 0;
-      AbstractJoint *joint;
-      // For each joint specified in the currTargetPosition...
-
-      for(int j = 0; j < sizeof(joints); j += sizeof(joints[jointIndex-1])){
 
-        joint = joints[jointIndex];
+      // For each joint specified in the currTargetPosition...
+      for(const auto &joint : joints){
 
         for(int k = 0; k < joint->getDegreesOfFreedom(); k++){
         // Each motor should run to its respective target position at a fixed speed
         // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters
-          double targetPos = currTargetPosition.positions[motorIndex];
-          float currPower = 0.1 * currTargetPosition.velocities[j]/velMax;
-
-          bool hasMotorFinished = configJointSetpoint(joint, motorIndex-jointIndex, targetPos, currPower);
-          hasPositionFinished &= hasMotorFinished;
-          motorIndex++;
+          hasPositionFinished &= joint->getMotor(k)->getMotorState() == MotorState::STOP;
           // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power
           // std::cout<<joint->getMotor(k)->getMotorName()<<":"<<std::setw(30-motors[j]->getMotorName().length())<<motors[j]->getRads()<<std::endl;
           // std::cout<<std::setw(30)<<motors[j]->getEncoderCounts()<<std::endl;
@@ -154,16 +129,12 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server
           IKEnabled = false;
           std_srvs::Trigger srv;
           if (enableServiceClient.call(srv)) {
-            ROS_WARN("%s", (std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data());
+            ROS_WARN("%s", (std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data()); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg)
           } else {
-            ROS_WARN("Error: failed to call service PLACEHOLDER_NAME");
+            ROS_WARN("Error: failed to call service PLACEHOLDER_NAME"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg)
           }
           return;
-          // TODO: hand control back to driver
-          // standard service empty stdsrvs empty
-          // go into spin loop
         }
-        jointIndex++;
       }
 
       // DEBUGGING OUTPUT: Print a divider line for cleanliness
@@ -176,10 +147,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server
   }
 
   //When all positions have been reached, set the current task as succeeded
-
-  for(int i = 0; i < numMotors; i++){
-    motors[i]->setPower(0.f);
-  }
   
   as->setSucceeded();
 }
@@ -187,18 +154,18 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server
 /**
  * @brief publishes the arm's position
  */
-void publish(const ros::TimerEvent &event){
+void publishJointStates(const ros::TimerEvent &event){
   std::vector<std::string> names;
   std::vector<double> positions;
   sensor_msgs::JointState js_msg;
 
-  for (int i = 0; i < numMotors; i++){
-    names.push_back(motors[i]->getMotorName());
-    positions.push_back(motors[i]->getRads());
+  for(const auto &joint : joints){
+    for(int i = 0; i < joint->getDegreesOfFreedom(); i++){
+      names.push_back(joint->getMotor(i)->getMotorName());
+      positions.push_back(joint->getMotor(i)->getRads());
+    }
   }
 
-  positions[4] = positions[5] + positions[4] / 2;
-
   js_msg.name = names;
   js_msg.position = positions;
   // Publish the Joint State message
@@ -212,7 +179,7 @@ void publish(const ros::TimerEvent &event){
  * @param argv The given program arguments
  * @return int The status code on exiting the program
  */
-int main(int argc, char** argv)
+auto main(int argc, char** argv) ->int
 {
   std::cout << "start main" << std::endl;
   // Initialize the current node as ArmControlSystem
@@ -225,28 +192,24 @@ int main(int argc, char** argv)
   pn.getParam("encoder_parameters", encParams);
 
   // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node
-  motors[0] = new ArmMotor("elbow", 1, 0, static_cast<int>(encParams[0]["counts_per_rotation"]), static_cast<int>(encParams[0]["offset"]), n);
-  motors[1] = new ArmMotor("forearm_roll", 1, 1, static_cast<int>(encParams[1]["counts_per_rotation"]), static_cast<int>(encParams[1]["offset"]), n);
-  motors[2] = new ArmMotor("shoulder", 0, 1, static_cast<int>(encParams[2]["counts_per_rotation"]), static_cast<int>(encParams[2]["offset"]), n);
-  motors[3] = new ArmMotor("turntable", 0, 0, static_cast<int>(encParams[3]["counts_per_rotation"]), static_cast<int>(encParams[3]["offset"]), n);
-  motors[4] = new ArmMotor("wrist_pitch", 2, 0, static_cast<int>(encParams[4]["counts_per_rotation"]), static_cast<int>(encParams[4]["offset"]), n);
-  motors[5] = new ArmMotor("wrist_roll", 2, 1, static_cast<int>(encParams[5]["counts_per_rotation"]), static_cast<int>(encParams[5]["offset"]), n);
+  auto elbow = std::make_unique<ArmMotor>("elbow", 1, 0, static_cast<int>(encParams[0]["counts_per_rotation"]), static_cast<int>(encParams[0]["offset"]), n);
+  auto forearmRoll = std::make_unique<ArmMotor>("forearm_roll", 1, 1, static_cast<int>(encParams[1]["counts_per_rotation"]), static_cast<int>(encParams[1]["offset"]), n);
+  auto shoulder = std::make_unique<ArmMotor>("shoulder", 0, 1, static_cast<int>(encParams[2]["counts_per_rotation"]), static_cast<int>(encParams[2]["offset"]), n);
+  auto turntable = std::make_unique<ArmMotor>("turntable", 0, 0, static_cast<int>(encParams[3]["counts_per_rotation"]), static_cast<int>(encParams[3]["offset"]), n);
+  auto wristLeft = std::make_unique<ArmMotor>("wrist_pitch", 2, 0, static_cast<int>(encParams[4]["counts_per_rotation"]), static_cast<int>(encParams[4]["offset"]), n);
+  auto wristRight = std::make_unique<ArmMotor>("wrist_roll", 2, 1, static_cast<int>(encParams[5]["counts_per_rotation"]), static_cast<int>(encParams[5]["offset"]), n);
   std::cout << "init motors" << std::endl;
 
   // Initialize all Joints
-  joints[0] = new SimpleJoint(motors[0], &n);
-  joints[1] = new SimpleJoint(motors[1], &n);
-  joints[2] = new SimpleJoint(motors[2], &n);
-  joints[3] = new SimpleJoint(motors[3], &n);
-  joints[4] = new SimpleJoint(motors[4], &n);
-  DifferentialJoint* temp = new DifferentialJoint(motors[4], motors[5], &n);
-  temp->configVelocityHandshake("/control/arm/5/roll", "/control/arm/5/pitch", "/control/arm/20/", "/control/arm/21/");
-
+  joints.at(0) = std::make_unique<SimpleJoint>(std::move(elbow), n);
+  joints.at(1) =std::make_unique<SimpleJoint>(std::move(forearmRoll), n);
+  joints.at(2) = std::make_unique<SimpleJoint>(std::move(shoulder), n);
+  joints.at(3) = std::make_unique<SimpleJoint>(std::move(turntable), n);
+  joints.at(4) = std::make_unique<DifferentialJoint>(std::move(wristLeft), std::move(wristRight), n, "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", "/control/arm/21/");
   std::cout << "init joints" << std::endl;
-  joints[5] = temp;
   
   // Initialize the Joint State Data Publisher
-  jointStatePublisher = n.advertise<sensor_msgs::JointState>("/control/arm_joint_states", 1000);
+  jointStatePublisher = n.advertise<sensor_msgs::JointState>("/control/arm_joint_states", MESSAGE_CACHE_SIZE);
 
   // Initialize the Action Server
   Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
@@ -254,14 +217,14 @@ int main(int argc, char** argv)
   server.start();
   std::cout << "server started" << std::endl;
 
-  ros::Timer timer = n.createTimer(ros::Duration(1.0 / 50.0), publish);
+  ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publishJointStates);
 
   enableServiceServer = n.advertiseService("start_IK", 
     static_cast<boost::function<bool(std_srvs::Trigger::Request&, std_srvs::Trigger::Response&)>>(
       [](std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)->bool{
         IKEnabled = true;
         res.message = "Arm IK Enabled";
-        res.success = true;
+        res.success = static_cast<unsigned char>(true);
         return true;
       }
     ));
diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp
index e9fc2267..57048943 100644
--- a/src/wr_control_drive_arm/src/ArmMotor.cpp
+++ b/src/wr_control_drive_arm/src/ArmMotor.cpp
@@ -5,28 +5,30 @@
  * @date 2021-04-05
  */
 #include "ArmMotor.hpp"
+#include <stdexcept>
+#include <sstream>
 
 /// Allow for referencing the UInt32 message type easier
-typedef std_msgs::UInt32::ConstPtr Std_UInt32;
-typedef std_msgs::Float64::ConstPtr Std_Float64;
-#define Std_Bool std_msgs::Bool::ConstPtr&
+using Std_UInt32 = std_msgs::UInt32::ConstPtr;
+using Std_Float64 = std_msgs::Float64::ConstPtr;
+using Std_Bool = std_msgs::Bool::ConstPtr;
 
-double ArmMotor::corrMod(double i, double j){
+auto ArmMotor::corrMod(double i, double j) -> double{
     // Stem i%j by j, which in modular arithmetic is the same as adding 0.
     return std::fmod(std::fmod(std::abs(j)*i/j,std::abs(j))+j,std::abs(j));
 }
 
 /// Currently consistent with the rad->enc equation as specified <a target="_blank" href="https://www.desmos.com/calculator/nwxtenccc6">here</a>.
-uint32_t ArmMotor::radToEnc(double rads) const {
+auto ArmMotor::radToEnc(double rads) const -> uint32_t{
     return this->COUNTS_PER_ROTATION * ArmMotor::corrMod(rads, 2 * M_PI)/(2 * M_PI) + this->ENCODER_OFFSET;
 }
 
-double ArmMotor::encToRad(uint32_t enc) const {
+auto ArmMotor::encToRad(uint32_t enc) const -> double{
     return ArmMotor::corrMod(static_cast<double>(enc - this->ENCODER_OFFSET) / static_cast<double>(this->COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI;
 }
 
 /// Currently consistent with the enc->rad equation as specified <a target="_blank" href="https://www.desmos.com/calculator/nwxtenccc6">here</a>.
-double ArmMotor::getRads() const{
+auto ArmMotor::getRads() const -> double{
     return ArmMotor::encToRad(this->getEncoderCounts());
 }
 
@@ -41,17 +43,17 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 &msg){
 
 void ArmMotor::redirectPowerOutput(const Std_Float64 &msg){
     // Set the speed to be the contained data
-    this->setPower(msg->data);
+    if(this->getMotorState() == MotorState::RUN_TO_TARGET) this->setPower(static_cast<float>(msg->data) * this->maxPower, MotorState::RUN_TO_TARGET);
 }
 
-void ArmMotor::storeStallStatus(const Std_Bool msg) {
-    this->isStall = msg->data;
+void ArmMotor::storeStallStatus(const Std_Bool &msg) {
+    this->isStall = static_cast<bool>(msg->data);
 }
 
 /// controllerID is constrained between [0,3]
 /// motorID is constrained between [0,1]
 ArmMotor::ArmMotor(
-    const std::string &motorName,
+    std::string motor_name,
     unsigned int controllerID,
     unsigned int motorID,
     int64_t countsPerRotation,
@@ -60,18 +62,20 @@ ArmMotor::ArmMotor(
 ) : COUNTS_PER_ROTATION{countsPerRotation}, 
     ENCODER_BOUNDS{0, std::abs(countsPerRotation)}, 
     ENCODER_OFFSET{offset}, 
-    motorName{motorName}, 
+    motorName{std::move(motor_name)}, 
     controllerID{controllerID}, 
     motorID{motorID}, 
     currState{MotorState::STOP},
+    power{0.F},
+    maxPower{0.F},
     encoderVal{static_cast<uint32_t>(offset)} {
     
     // Check validity of WRoboclaw and motor IDs
-    if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]";
-    if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]";
+    if(controllerID > 3) throw std::invalid_argument{std::string{"Controller ID "} + std::to_string(controllerID) + "is only valid on [0,3]"};
+    if(motorID > 1) throw std::invalid_argument{std::string{"Motor ID "} + std::to_string(motorID) + " is only valid on [0,1]"};
 
     // Create the topic string prefix for WRoboclaw controllers
-    std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID);
+    std::string tpString = std::string{"/hsi/roboclaw/aux"} + std::to_string(controllerID);
     std::string controlString = "/control/arm/" + std::to_string(controllerID) + std::to_string(motorID);
 
     // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully
@@ -83,7 +87,7 @@ ArmMotor::ArmMotor(
     this->stallRead = n.subscribe(tpString + "/curr/over_lim/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeStallStatus, this);
 }
 
-uint32_t ArmMotor::getEncoderCounts() const{
+auto ArmMotor::getEncoderCounts() const -> uint32_t{
     return this->encoderVal;
 }
 
@@ -92,7 +96,7 @@ void ArmMotor::runToTarget(uint32_t targetCounts, float power){
     this->runToTarget(targetCounts, power, false);
 }
 
-bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const {
+auto ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const -> bool{
     // Compute the upper and lower bounds in the finite encoder space
     uint32_t lBound = ArmMotor::corrMod(static_cast<double>(targetCounts - tolerance), static_cast<double>(ArmMotor::ENCODER_BOUNDS[1]));
     uint32_t uBound = ArmMotor::corrMod(static_cast<double>(targetCounts + tolerance), static_cast<double>(ArmMotor::ENCODER_BOUNDS[1]));
@@ -104,41 +108,47 @@ bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const
 }
 
 /// Current tolerance is &pm;0.1 degree w.r.t. the current number of counts per rotation
-bool ArmMotor::hasReachedTarget(uint32_t targetCounts) const {
+auto ArmMotor::hasReachedTarget(uint32_t targetCounts) const -> bool{
     return ArmMotor::hasReachedTarget(targetCounts, ArmMotor::TOLERANCE_RATIO * static_cast<double>(std::abs(this->COUNTS_PER_ROTATION)));
 }
 
-MotorState ArmMotor::getMotorState() const {
+auto ArmMotor::getMotorState() const -> MotorState{
     return this->currState;
 }
 
-/// This method auto-publishes the speed command to the WRoboclaws
 void ArmMotor::setPower(float power){
+    this->setPower(power, power == 0.F ? MotorState::STOP : MotorState::MOVING);
+}
+
+/// This method auto-publishes the speed command to the WRoboclaws
+void ArmMotor::setPower(float power, MotorState state){
     // Check the bounds of the parameter
-    if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]";
+    if(abs(power) > 1) throw std::invalid_argument{std::string{"Power "} + std::to_string(power) + " is not on the interval [-1, 1]"};
 
     // Set up and send the speed message
+    this->power = power;
     this->powerMsg.data = power * INT16_MAX;
     this->speedPub.publish(this->powerMsg);
     // Update the cur.nt motor state based on the power command input
-    this->currState = power == 0.F ? MotorState::STOP : MotorState::MOVING;
+    this->currState = state;
 }
 
-bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){
+auto ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) -> bool{
     // Checks for stall
     if (this->isStall) {
-        if ((ros::Time::now() - begin).toSec() >= 0.5) {
-            this->setPower(0.0f);
+        if ((ros::Time::now() - begin).toSec() >= STALL_THRESHOLD_TIME) {
+            this->setPower(0.0F);
             this->currState = MotorState::STOP;
             return false;
         }
     } else {
         begin = ros::Time::now();
     }
-    
+    this->maxPower = power;
     // If we are not at our target...
     if(!this->hasReachedTarget(targetCounts)){
         // Set the power in the correct direction and continue running to the target
+        this->currState = MotorState::RUN_TO_TARGET;
         std_msgs::Float64 setpointMsg;
         setpointMsg.data = ArmMotor::encToRad(targetCounts);
         this->targetPub.publish(setpointMsg);
@@ -147,40 +157,37 @@ bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){
         // power = abs(power) * (corrMod(direction, ((long int)this->COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)this->COUNTS_PER_ROTATION)) ? 1 : -1);
         // this->setPower(power);
 
-        this->currState = MotorState::RUN_TO_TARGET;
     // Otherwise, stop the motor
     } else {
-        this->setPower(0.F);
-        this->currState = MotorState::STOP;
+        this->setPower(0.F, MotorState::RUN_TO_TARGET);
     }
     // If this is a blocking call...
     if(block){
         // Wait until the motor has reached the target, then stop
         while(!this->hasReachedTarget(targetCounts));
-        this->setPower(0.F);
-        this->currState = MotorState::STOP;
+        this->setPower(0.F, MotorState::RUN_TO_TARGET);
     }
     return true;
 }
 
-bool ArmMotor::runToTarget(double rads, float power){
+auto ArmMotor::runToTarget(double rads, float power) -> bool{
     std::cout << "run to target: " << rads << ":" << this->radToEnc(rads) << std::endl;
     
     return runToTarget(this->radToEnc(rads), power, false);
 }
 
-std::string ArmMotor::getMotorName() const {
+auto ArmMotor::getMotorName() const -> std::string{
     return this->motorName;
 }
 
-unsigned int ArmMotor::getMotorID() const {
+auto ArmMotor::getMotorID() const -> unsigned int{
     return this->motorID;
 }
 
-unsigned int ArmMotor::getControllerID() const {
+auto ArmMotor::getControllerID() const -> unsigned int{
     return this->controllerID;
 }
 
-float ArmMotor::getPower() const {
-    return ((float)this->powerMsg.data)/INT16_MAX;
+auto ArmMotor::getPower() const -> float{
+    return this->power;
 }
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp
index 0124a9b1..547c936d 100644
--- a/src/wr_control_drive_arm/src/ArmMotor.hpp
+++ b/src/wr_control_drive_arm/src/ArmMotor.hpp
@@ -1,16 +1,17 @@
 /**
  * @file ArmMotor.hpp
  * @author Ben Nowotny
- * @brief ablskjlfkejfs
+ * @brief Header file of the ArmMotor class
  * @date 2021-04-05
  */
+ 
 #include <iostream>
 #include "ros/ros.h"
 #include "std_msgs/UInt32.h"
 #include "std_msgs/Int16.h"
 #include "std_msgs/Float64.h"
 #include "std_msgs/Bool.h"
-#include "math.h"
+#include <cmath>
 #include <string>
 
 /**
@@ -29,6 +30,8 @@ enum class MotorState{
  */
 class ArmMotor{
     private:
+        /// Time tolerance of over-current detection before a stall fault is triggered
+        static constexpr float STALL_THRESHOLD_TIME = 0.5;
         /// Tolerance ratio w.r.t the counts per rotation for encoder run-to-position motion
         static constexpr double TOLERANCE_RATIO = 0.1/360;
         /// Size of ROS Topic message caches
@@ -40,7 +43,7 @@ class ArmMotor{
         /// zero position of motor
         int64_t const ENCODER_OFFSET;
         /// The current state of the motor
-        MotorState currState;
+        std::atomic<MotorState> currState;
         /// The joint name of the current motor
         std::string motorName;
         /// The ID of the WRoboclaw controller
@@ -62,11 +65,15 @@ class ArmMotor{
         /// The most recent power message sent
         std_msgs::Int16 powerMsg;
         /// If the motor is stalling or not
-        volatile bool isStall;
+        volatile bool isStall = false;
         /// The ROS Subscriber that reads stall status data
         ros::Subscriber stallRead;
         /// The time when the motor began stalling
         ros::Time begin;
+        /// Motor power
+        float power;
+        /// Maximum absolute motor power in RUN_TO_POSITION mode.
+        std::atomic<float> maxPower;
 
         /**
          * @brief A static conversion from radians to encoder counts
@@ -74,7 +81,7 @@ class ArmMotor{
          * @param rad The input number of radians
          * @return uint32_t The corresponding encoder count bounded by ENCODER_BOUNDS
          */
-        uint32_t radToEnc(double rad) const;
+        auto radToEnc(double rad) const -> uint32_t;
 
         /**
          * @brief A static conversion from encoder counts to radians
@@ -82,7 +89,7 @@ class ArmMotor{
          * @param enc The input number of encoder counts
          * @return double The corresponding radian measure
          */
-        double encToRad(uint32_t enc) const;
+        auto encToRad(uint32_t enc) const -> double;
 
         /**
          * @brief Subscriber callback for encRead, captures the encoder value of the current motor
@@ -105,6 +112,8 @@ class ArmMotor{
          */
         void storeStallStatus(const std_msgs::Bool::ConstPtr& msg);
 
+        void setPower(float power, MotorState state);
+
         /**
          * @brief Performs Euclidean correct modulus between two inputs of the same type
          * 
@@ -112,7 +121,7 @@ class ArmMotor{
          * @param j The divisor of the modulus
          * @return double The Euclidean-correct remainder bounded on [0, j)
          */
-        static double corrMod(double i, double j);
+        static auto corrMod(double i, double j) -> double;
 
     public:
         /**
@@ -123,14 +132,14 @@ class ArmMotor{
          * @param motorID The motor ID within its WRoboclaw controller
          * @param n A NodeHandle reference to the constructing Node
          */
-        ArmMotor(const std::string &motorName, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, int64_t offset, ros::NodeHandle &n);
+        ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, int64_t offset, ros::NodeHandle &n);
 
         /**
          * @brief Gets the encoder value of the motor
          * 
          * @return uint32_t The current encoder value of the motor
          */
-        uint32_t getEncoderCounts() const;
+        auto getEncoderCounts() const -> uint32_t;
 
         /**
          * @brief Sends the motor to run to a target encoder value at a given power without blocking
@@ -148,7 +157,7 @@ class ArmMotor{
          * @param block Specifies whether or not this action should block until it is complete
          * @return True if the motor had stalled, and false otherwise
          */
-        bool runToTarget(uint32_t targetCounts, float power, bool block);
+        auto runToTarget(uint32_t targetCounts, float power, bool block) -> bool;
 
         /**
          * @brief Sends the motor to run to a specified position at a given power
@@ -157,14 +166,14 @@ class ArmMotor{
          * @param power The power to move the motor at (Bounded between [-1, 1])
          * @return True if the motor had stalled, and false otherwise
          */
-        bool runToTarget(double rads, float power);
+        auto runToTarget(double rads, float power) -> bool;
 
         /**
          * @brief Get the current state of the ArmMotor
          * 
          * @return MotorState The current state of the ArmMotor
          */
-        MotorState getMotorState() const;
+        auto getMotorState() const -> MotorState;
 
         /**
          * @brief Set the motor power
@@ -178,28 +187,28 @@ class ArmMotor{
          * 
          * @return double The radian measure of the current motor's position
          */
-        double getRads() const;
+        auto getRads() const -> double;
 
         /**
          * @brief Get the name of the ArmMotor
          * 
          * @return std::string The name of the ArmMotor
          */
-        std::string getMotorName() const;
+        auto getMotorName() const -> std::string;
 
          /**
          * @brief Get the name of the ArmMotor
          * 
          * @return std::string The controller ID of the ArmMotor
          */
-        unsigned int getControllerID() const;
+        auto getControllerID() const -> unsigned int;
 
         /**
          * @brief Get the name of the ArmMotor
          * 
          * @return std::string The motor ID of the ArmMotor
          */
-        unsigned int getMotorID() const;
+        auto getMotorID() const -> unsigned int;
 
 
         /**
@@ -209,7 +218,7 @@ class ArmMotor{
          * @return true The motor was within the target tolerance
          * @return false The motor was outside of the target tolerance
          */
-        bool hasReachedTarget(uint32_t targetCounts) const;
+        auto hasReachedTarget(uint32_t targetCounts) const -> bool;
 
         /**
          * @brief Checks if the motor is currently within a given tolerance of a target
@@ -219,13 +228,13 @@ class ArmMotor{
          * @return true The motor was within the target tolerance
          * @return false The motor was outside the target tolerance
          */
-        bool hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const;
+        auto hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const -> bool;
 
         /**
          * @brief Get the most recently set motor power
          * 
          * @return float Last motor power setting
          */
-        float getPower() const;
+        auto getPower() const -> float;
 };
 
diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp
index ef600304..85c3c1c7 100644
--- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp
+++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp
@@ -1,7 +1,7 @@
 /**
- * @file AbstractJoint.hpp
- * @author Nichols Underwood
- * @brief ablskjlfkejfs
+ * @file DifferentialJoint.cpp
+ * @author Nicholas Underwood
+ * @brief Header file of the DifferentialJoint class
  * @date 2021-10-25
  */
 
@@ -9,76 +9,82 @@
 #include <memory>
 using std::vector;
 
-DifferentialJoint::DifferentialJoint(ArmMotor* leftMotor, ArmMotor* rightMotor, ros::NodeHandle* n) : AbstractJoint(n, 2) {
-    this->motors.push_back(leftMotor);
-    this->motors.push_back(rightMotor);
+constexpr std::array<std::array<double, 2>, 2> DifferentialJoint::MOTOR_TO_JOINT_MATRIX;
+constexpr std::array<std::array<double, 2>, 2> DifferentialJoint::JOINT_TO_MOTOR_MATRIX;
+
+DifferentialJoint::DifferentialJoint(std::unique_ptr<ArmMotor> leftMotor, std::unique_ptr<ArmMotor> rightMotor, ros::NodeHandle &n,
+                                     const std::string &pitchTopicName, const std::string &rollTopicName,
+                                     const std::string &leftTopicName, const std::string &rightTopicName) 
+                                     : AbstractJoint(n, DEGREES_OF_FREEDOM) {
+    this->motors.at(0) = {std::move(leftMotor), 0, 0, "", "", false};
+    this->motors.at(1) = {std::move(rightMotor), 0, 0, "", "", false};
+
+    this->pitchOutputSubscriber = n.subscribe<std_msgs::Float64>(pitchTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffPitchOutput, this);
+    this->rollOutputSubscriber = n.subscribe<std_msgs::Float64>(rollTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRollOutput, this);
+    this->leftOutputPublisher = n.advertise<std_msgs::Float64>(leftTopicName + "/output", MESSAGE_CACHE_SIZE);
+    this->rightOutputPublisher = n.advertise<std_msgs::Float64>(rightTopicName + "/output", MESSAGE_CACHE_SIZE);
+    this->leftFeedbackSubscriber = n.subscribe<std_msgs::Float64>(leftTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffLeftFeedback, this);
+    this->rightFeedbackSubscriber = n.subscribe<std_msgs::Float64>(rightTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRightFeedback, this);
+    this->pitchFeedbackPublisher = n.advertise<std_msgs::Float64>(pitchTopicName + "/feedback", MESSAGE_CACHE_SIZE);
+    this->rollFeedbackPublisher = n.advertise<std_msgs::Float64>(rollTopicName + "/feedback", MESSAGE_CACHE_SIZE);
 }
 
-void DifferentialJoint::getJointPositions(vector<double> &motorPositions, vector<double> &target){
+auto DifferentialJoint::getJointPositions(const vector<double> &motorPositions) -> vector<double>{
     // vector<double> positions;
     // target->reserve(2);
     
-    double pitch = motorPositions[0] * this->motorToJointMatrix[0][0] + motorPositions[1]*this->motorToJointMatrix[0][1];
-    double roll = motorPositions[1] * this->motorToJointMatrix[1][0] + motorPositions[1]*this->motorToJointMatrix[1][1];
+    double pitch = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(0).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(0).at(1);
+    double roll = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(1).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(1).at(1);
 
-    target.push_back(pitch);
-    target.push_back(roll);
+    //TODO: Why not return a new vector?  Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision?
+    return {pitch, roll};
 
     // return positions;
 }
 
-void DifferentialJoint::getMotorPositions(vector<double> &jointPositions, vector<double> &target){
+auto DifferentialJoint::getMotorPositions(const vector<double> &jointPositions) -> vector<double>{
     
     // std::unique_ptr<vector<double>> positions = std::make_unique<vector<double>>(2);
     // target->reserve(2);
 
-    double left = jointPositions[0] * this->jointToMotorMatrix[0][0] + jointPositions[1]*this->jointToMotorMatrix[0][1];
-    double right = jointPositions[1] * this->jointToMotorMatrix[1][0] + jointPositions[1]*this->jointToMotorMatrix[1][1];
+    double left = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(0).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(0).at(1);
+    double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(1).at(1);
 
-    target.push_back(left);
-    target.push_back(right);
+    return {left, right};
 
     // return std::move(positions);
 }
 
-void DifferentialJoint::getMotorVelocities(vector<double> &jointPositions, vector<double> &target){
-    return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself
+auto DifferentialJoint::getMotorVelocities(const vector<double> &jointPositions) -> vector<double>{
+    return getMotorPositions(jointPositions); //deritivate of linear transformation is itself
 }
 
-void DifferentialJoint::configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName){
-    this->pitchOutputSubscriber = this->n->subscribe<std_msgs::Float64>(pitchTopicName + "/output", 1000, &DifferentialJoint::handoffPitchOutput, this);
-    this->rollOutputSubscriber = this->n->subscribe<std_msgs::Float64>(rollTopicName + "/output", 1000, &DifferentialJoint::handoffRollOutput, this);
-    this->leftOutputPublisher = this->n->advertise<std_msgs::Float64>(leftTopicName + "/output", 1000);
-    this->rightOutputPublisher = this->n->advertise<std_msgs::Float64>(rightTopicName + "/output", 1000);
-    this->leftFeedbackSubscriber = this->n->subscribe<std_msgs::Float64>(leftTopicName + "/feeback", 1000, &DifferentialJoint::handoffLeftFeedback, this);
-    this->rightFeedbackSubscriber = this->n->subscribe<std_msgs::Float64>(rightTopicName + "/feeback", 1000, &DifferentialJoint::handoffRightFeedback, this);
-    this->pitchFeedbackPublisher = this->n->advertise<std_msgs::Float64>(pitchTopicName + "/feedback", 1000);
-    this->rollFeedbackPublisher = this->n->advertise<std_msgs::Float64>(rollTopicName + "/feedback", 1000);
-}
-
-void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64 msg){
-    this->cachedPitchOutput = msg.data;
+void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64::ConstPtr &msg){
+    this->cachedPitchOutput = msg->data;
     this->hasNewPitchOutput = true;
     handOffAllOutput();
 }
-void DifferentialJoint::handoffRollOutput(const std_msgs::Float64 msg){
-    this->cachedRollOutput = msg.data;
+void DifferentialJoint::handoffRollOutput(const std_msgs::Float64::ConstPtr &msg){
+    this->cachedRollOutput = msg->data;
     this->hasNewRollOutput = true;
     handOffAllOutput();
 }
 
+// TODO: Do these ouputs need to be synced?  We're just referring to the motors individually, maybe we can transform and send the data right away
 void DifferentialJoint::handOffAllOutput(){
     if(!this->hasNewPitchOutput || !this->hasNewRollOutput){ return; }
 
     vector<double> outputs;
-    outputs[0] = this->cachedPitchOutput;
-    outputs[1] = this->cachedRollOutput;
-    getMotorVelocities(outputs, outputs); 
-
-    std_msgs::Float64 msg1 = std_msgs::Float64();
-    std_msgs::Float64 msg2 = std_msgs::Float64();
-    msg1.data = outputs[0];
-    msg2.data = outputs[1];
+    outputs.resize(2);
+    outputs.at(0) = this->cachedPitchOutput;
+    outputs.at(1) = this->cachedRollOutput;
+    //TODO: Still confused as to the purpose of this family of functions
+    outputs = getMotorVelocities(outputs); 
+
+    std_msgs::Float64 msg1;
+    std_msgs::Float64 msg2;
+    msg1.data = outputs.at(0);
+    msg2.data = outputs.at(1);
     pitchFeedbackPublisher.publish(msg1);
     rollFeedbackPublisher.publish(msg2);
 
@@ -90,13 +96,13 @@ void DifferentialJoint::handOffAllOutput(){
     
 }
 
-void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64 msg){
-    this->cachedRightFeedback = msg.data;
+void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64::ConstPtr &msg){
+    this->cachedRightFeedback = msg->data;
     this->hasNewRightFeedback = true;
     handOffAllFeedback();
 }
-void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64 msg){
-    this->cachedLeftFeedback = msg.data;
+void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64::ConstPtr &msg){
+    this->cachedLeftFeedback = msg->data;
     this->hasNewLeftFeedback = true;
     handOffAllFeedback();
 }
@@ -105,14 +111,14 @@ void DifferentialJoint::handOffAllFeedback(){
     if(!this->hasNewLeftFeedback || !this->hasNewRightFeedback){ return; }
 
     vector<double> outputs;
-    outputs[0] = this->cachedLeftFeedback;
-    outputs[1] = this->cachedRightFeedback;
-    getMotorVelocities(outputs, outputs); 
-
-    std_msgs::Float64 msg1 = std_msgs::Float64();
-    std_msgs::Float64 msg2 = std_msgs::Float64();
-    msg1.data = outputs[0];
-    msg2.data = outputs[1];
+    outputs.at(0) = this->cachedLeftFeedback;
+    outputs.at(1) = this->cachedRightFeedback;
+    outputs = getMotorVelocities(outputs); 
+
+    std_msgs::Float64 msg1;
+    std_msgs::Float64 msg2;
+    msg1.data = outputs.at(0);
+    msg2.data = outputs.at(1);
     pitchFeedbackPublisher.publish(msg1);
     rollFeedbackPublisher.publish(msg2);
 
diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp
index 07606c2a..7bfaec4f 100644
--- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp
+++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp
@@ -1,29 +1,37 @@
+/**
+ * @file DifferentialJoint.hpp
+ * @author Nichols Underwood
+ * @brief Header file of the DifferentialJoint class
+ * @date 2021-10-25
+ */
+
 #include "AbstractJoint.hpp"
 
 class DifferentialJoint : public AbstractJoint {
     public:
-        ~DifferentialJoint();
-        DifferentialJoint(ArmMotor *leftMotor, ArmMotor *rightMotor, ros::NodeHandle *n);
-
-        void getMotorPositions(vector<double> &jointPositions, vector<double> &target);
-        void getMotorVelocities(vector<double> &jointVelocities, vector<double> &target);
-        void getJointPositions(vector<double> &motorPositions, vector<double> &target);
+        DifferentialJoint(std::unique_ptr<ArmMotor> leftMotor, std::unique_ptr<ArmMotor> rightMotor, ros::NodeHandle &n,
+                          const std::string &pitchTopicName, const std::string &rollTopicName,
+                          const std::string &leftTopicName, const std::string &rightTopicName);
 
-        void configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName);
+        auto getMotorPositions(const vector<double> &jointPositions) -> vector<double> override;
+        auto getMotorVelocities(const vector<double> &jointVelocities) -> vector<double> override;
+        auto getJointPositions(const vector<double> &motorPositions) -> vector<double> override;
 
     private:
+        static constexpr uint32_t DEGREES_OF_FREEDOM = 2;
+        static constexpr uint32_t MESSAGE_CACHE_SIZE = 10;
         // linear transformations works for position and velocity
         // [0.5 0.5]   [left motor ]    [pitch]
         // [ -1  1 ] * [right motor]  = [roll ]
         // double motorToJointMatrix[2][2] = {{0.5, 0.5}, {-1, 1}};
-        double motorToJointMatrix[2][2] = {{1, 0}, {0, 1}};
+        static constexpr std::array<std::array<double, 2>, 2> MOTOR_TO_JOINT_MATRIX{{{1, 0}, {0, 1}}};
         // [1 -0.5]   [pitch]    [left motor ]
         // [1  0.5] * [roll ]  = [right motor]
         // double jointToMotorMatrix[2][2] = {{1, 0.5}, {1, -0.5}};
-        double jointToMotorMatrix[2][2] = {{1, 0}, {0, 1.0}};
+        static constexpr std::array<std::array<double, 2>, 2> JOINT_TO_MOTOR_MATRIX{{{1, 0}, {0, 1}}};
 
-        void handoffPitchOutput(std_msgs::Float64);
-        void handoffRollOutput(std_msgs::Float64);
+        void handoffPitchOutput(const std_msgs::Float64::ConstPtr&);
+        void handoffRollOutput(const std_msgs::Float64::ConstPtr&);
         void handOffAllOutput();
         ros::Subscriber pitchOutputSubscriber;
         ros::Subscriber rollOutputSubscriber;
@@ -34,8 +42,8 @@ class DifferentialJoint : public AbstractJoint {
         bool hasNewPitchOutput = false;
         bool hasNewRollOutput = false;
 
-        void handoffLeftFeedback(std_msgs::Float64);
-        void handoffRightFeedback(std_msgs::Float64);
+        void handoffLeftFeedback(const std_msgs::Float64::ConstPtr&);
+        void handoffRightFeedback(const std_msgs::Float64::ConstPtr&);
         void handOffAllFeedback();
         ros::Subscriber leftFeedbackSubscriber;
         ros::Subscriber rightFeedbackSubscriber;
diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp
index b846f219..d7ca2ee2 100644
--- a/src/wr_control_drive_arm/src/SimpleJoint.cpp
+++ b/src/wr_control_drive_arm/src/SimpleJoint.cpp
@@ -1,48 +1,24 @@
-#ifndef SIMPLE_JOINT_GUARD
-#define SIMPLE_JOINT_GUARD
 /**
- * @file AbstractJoint.cpp
+ * @file SimpleJoint.cpp
  * @author Nichols Underwood
- * @brief ablskjlfkejfs
+ * @brief Implementation of the SimpleJoint class
  * @date 2021-10-25
  */
 
 #include "SimpleJoint.hpp"
 
-SimpleJoint::SimpleJoint(ArmMotor* motor, ros::NodeHandle *n) : AbstractJoint(n, 1) {
-    this->motors.push_back(motor);
+SimpleJoint::SimpleJoint(std::unique_ptr<ArmMotor> motor, ros::NodeHandle &n) : AbstractJoint(n, 1) {
+    this->motors.at(0) = MotorHandler{std::move(motor), 0, 0, "", "", false};
 }
 
-void SimpleJoint::getJointPositions(vector<double> &motorPositions, vector<double> &target){
-    // vector<double> positions = *(new vector<double>()); // makes the red lines go away
-    
-    // target->reserve(1);
-    target.push_back(motorPositions[0]);
-
-    // return positions;
-}
-
-void SimpleJoint::getMotorPositions(vector<double> &jointPositions, vector<double> &target){
-  
-    target.reserve(1);
-    double position = jointPositions[0];
-    target.push_back(position);
-    // return positions;
+auto SimpleJoint::getMotorPositions(const vector<double> &jointPositions) -> vector<double> {
+    return {jointPositions[0]};
 }
 
-void SimpleJoint::getMotorVelocities(vector<double> &jointPositions, vector<double> &target){
-    
-    // target->reserve(1);
-    target.push_back(jointPositions[0]);
-
-    // return setpoints;
+auto SimpleJoint::getMotorVelocities(const vector<double> &jointVelocities) -> vector<double> {
+    return {jointVelocities[0]};
 }
 
-void SimpleJoint::handoffOutput(const std_msgs::Float64 msg){
-    this->motorOutputPublisher.publish(msg);
-}
-
-void SimpleJoint::handoffFeedback(const std_msgs::Float64 msg){
-    this->jointFeedbackPublisher.publish(msg);
-}
-#endif
\ No newline at end of file
+auto SimpleJoint::getJointPositions(const vector<double> &jointPositions) -> vector<double> {
+    return {jointPositions[0]};
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp
index 9be7198d..2c1bf76b 100644
--- a/src/wr_control_drive_arm/src/SimpleJoint.hpp
+++ b/src/wr_control_drive_arm/src/SimpleJoint.hpp
@@ -1,28 +1,18 @@
+/**
+ * @file SimpleJoint.hpp
+ * @author Nichols Underwood
+ * @brief Header file of the SimpleJoint class
+ * @date 2021-10-25
+ */
+
 #include "AbstractJoint.hpp"
+
 using std::vector;
 
 class SimpleJoint : public AbstractJoint {
     public:
-        ~SimpleJoint();
-        SimpleJoint(ArmMotor* motor, ros::NodeHandle* n);
-
-        int a(){return 2;}
-        int b(){return 3;}
-
-        void getMotorPositions(vector<double> &jointPositions, vector<double> &target);
-        void getMotorVelocities(vector<double> &ointVelocities, vector<double> &target);
-        void getJointPositions(vector<double> &motorPositions, vector<double> &target);
-
-        void configMotor(ArmMotor* motor);
-        void configVelocityHandshake(std::string jointTopicName, std::string motorTopicName);
-        void handoffOutput(std_msgs::Float64);
-        void handoffFeedback(std_msgs::Float64);
-
-    private:
-        ros::Subscriber jointOutputSubscriber;
-        ros::Publisher motorOutputPublisher;
-
-        ros::Subscriber motorFeedbackSubscriber;
-        ros::Publisher jointFeedbackPublisher;
-        
+        SimpleJoint(std::unique_ptr<ArmMotor> motor, ros::NodeHandle& n);
+        auto getMotorPositions(const vector<double> &jointPositions) -> vector<double> override;
+        auto getMotorVelocities(const vector<double> &joinVelocities) -> vector<double> override;
+        auto getJointPositions(const vector<double> &motorPositions) -> vector<double> override;
 };