diff --git a/chassis/CMakeLists.txt b/chassis/CMakeLists.txt
index 2409615..3324111 100644
--- a/chassis/CMakeLists.txt
+++ b/chassis/CMakeLists.txt
@@ -107,7 +107,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES chassis
-#  CATKIN_DEPENDS joy roscpp turtlesim learning_joy
+#  CATKIN_DEPENDS joy roscpp turtlesim learning_joy roboteq_msgs
 #  DEPENDS system_lib
 )
 
@@ -143,6 +143,12 @@ target_link_libraries(teleop ${catkin_LIBRARIES})
 add_executable(teleop_inverted src/teleop_inverted.cpp src/chassis.cpp)
 target_link_libraries(teleop_inverted ${catkin_LIBRARIES})
 
+add_executable(control src/chassis_control.cpp src/chassis.cpp)
+target_link_libraries(control ${catkin_LIBRARIES})
+
+add_executable(create_cmd_vel src/chassis_create_cmd_vel.cpp src/chassis.cpp)
+target_link_libraries(create_cmd_vel ${catkin_LIBRARIES})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
diff --git a/chassis/include/Configuration.h b/chassis/include/Configuration.h
index 94bb1ad..0681055 100644
--- a/chassis/include/Configuration.h
+++ b/chassis/include/Configuration.h
@@ -2,3 +2,4 @@
 #define TRACK_WIDTH 0.4  //Track width in meters
 #define MAX_SPEED 1  //Max speed in meters per second
 #define MAX_TURN 1  //Max turn in Radians per second
+#define WHEEL_DIAMETER 0.2032 // 8 in Andymark Wheels
\ No newline at end of file
diff --git a/chassis/include/chassis.h b/chassis/include/chassis.h
index 58a9246..1cdaa7e 100644
--- a/chassis/include/chassis.h
+++ b/chassis/include/chassis.h
@@ -6,9 +6,10 @@
 #include <std_msgs/String.h>
 #include <stdio.h>
 #include <sensor_msgs/Joy.h>
-#include "roboteq_msgs/Command.h"
+//#include "roboteq_msgs/Command.h"
 #include <std_msgs/String.h>
 #include <stdio.h>
+#include <geometry_msgs/Twist.h>
 
 #include <string>
 #include <iostream>
@@ -16,15 +17,16 @@
 #include <unistd.h>
 #include "serial/serial.h"
 #include <math.h>
+#include <utility>      // std::pair, std::make_pair
+#include <exception>
+#include <ros/console.h>
+
 
 class Chassis
 {
 
 public:
 
-  // floats to store most-recent data from joystick subscriber
-  inline static float joyDataLeft;
-  inline static float joyDataRight;
 
   // Joystick data subscriber callback to assign member variables with data
   static void chatterCallback(const sensor_msgs::Joy::ConstPtr& joy)
@@ -38,14 +40,56 @@ class Chassis
 
   }
 
+  static void create_cmd_vel(const sensor_msgs::Joy::ConstPtr& joy)
+  {
+    ros::NodeHandle nh;
+    joyDataLeft = joy->axes[1];  // Left Y Axis
+    joyDataRight = joy->axes[4]; // Right Y Axis
+
+    vel_pub = nh.advertise<geometry_msgs::Twist>("chassis/cmd_vel", 10);
+
+    std::string teleop_control_scheme;
+    nh.getParam("teleop_control_scheme", teleop_control_scheme);
+
+        geometry_msgs::Twist twist;
+
+
+    if(teleop_control_scheme == "tank")
+    {
+      twist.linear.x = (joyDataRight + joyDataLeft) / 2; // Set forward/backward axis data
+      twist.angular.z = (joyDataRight - joyDataLeft) / 2; // Set rotation axis data
+    }
+    else if(teleop_control_scheme == "arcade")
+    {
+      twist.linear.x = joy->axes[1]; // Set forward/backward axis data
+      twist.angular.z = joy->axes[0]; // Set rotation axis data
+    }
+    else
+    {
+      ROS_ERROR("Error: Joystick control scheme not specified as parameter in launch file or via rosrun!");
+    }
+    
+    vel_pub.publish(twist);
+  }
+
+  static void handle_cmd_vel(const geometry_msgs::Twist& cmd_vel)
+  {
+    /* NOTE: THIS TAKES A VELOCITY AND UPDATES THE CHASSIS! TO CHANGE THE CONTROLLER
+     *       SPEED CURVE, ETC., UPDATE joystick_handler!
+     */
+
+    set_linear_velocity = 0.8 * MAX_SPEED * cmd_vel.linear.x;     // m/s per Configuration.h
+    set_rotational_velocity = 0.8 * MAX_TURN * cmd_vel.angular.z; // rad/s per Configuration.h
+  }
+
   static void tank_drive(bool inverted = 0)
   {
-    //
+    // Update inverted_controls each time
     inverted_controls = inverted;
 
     // Subscriber node to handle joystick input data
     ros::NodeHandle joy_handle;
-    ros::Subscriber sub = joy_handle.subscribe("/joy0", 1000, chatterCallback);
+    ros::Subscriber sub = joy_handle.subscribe("/j0", 1000, chatterCallback);
 
     // Serial objects used to communicate with RoboteQ motor controllers
     serial::Serial my_serial_l("/dev/ttyACM0", 115200, serial::Timeout::simpleTimeout(10)); // 10 ms timeout
@@ -105,8 +149,149 @@ class Chassis
     }
   }
 
-  private:
+  static std::pair<float, float> calculate_differential_vel(float linear_velocity, float rotational_velocity)
+  {
+    // Equations from: http://www.openrobots.org/morse/doc/1.3/user/actuators/v_omega_diff_drive.html
+
+    std::pair<float, float> diff_vel;
+
+    float l_vel = (linear_velocity - (1/2 * TRACK_WIDTH) * rotational_velocity) / WHEEL_DIAMETER;
+    float r_vel = (linear_velocity + (1/2 * TRACK_WIDTH) * rotational_velocity) / WHEEL_DIAMETER;
+
+    diff_vel.first = l_vel;
+    diff_vel.second = r_vel;
+
+    ROS_INFO("%f, %f", l_vel, r_vel);
+
+    return diff_vel;
+  }
+
+  static void control(bool inverted = 0)
+  {
+    // Update inverted_controls each time
+    inverted_controls = inverted;
+
+    // Subscriber node to handle joystick input data
+    ros::NodeHandle joy_handle;
+    ros::Subscriber sub = joy_handle.subscribe("/chassis/cmd_vel", 1000, handle_cmd_vel);
+
+    bool initialized_motor_controllers = 0;
+
+    // Serial objects used to communicate with RoboteQ motor controllers
+    serial::Serial my_serial_l, my_serial_r; 
+
+    while(!initialized_motor_controllers)
+    {
+      try
+      {
+        my_serial_l.setPort("/dev/ttyACM0");
+        my_serial_l.setBaudrate(115200);
+        my_serial_r.setPort("/dev/ttyACM1");
+        my_serial_r.setBaudrate(115200);
+
+        // FIXME: Add setTimeout to 10 ms?
+
+        my_serial_l.open();
+        my_serial_r.open();
+        if(my_serial_l.isOpen() && my_serial_r.isOpen())
+          initialized_motor_controllers = 1;
+        else
+          ROS_ERROR("Unhandled Exception: Serial connections not initialized.");
+      }
+      catch(std::exception &e)
+      {
+        ROS_ERROR("Unhandled Exception: %s", e.what());
+      }
+    }
+
+    // Initial debug statements to check for left motor controller port opened
+    std::cout << "Is the serial port open?";
+    if(my_serial_l.isOpen())
+      std::cout << " Yes." << std::endl;
+    else
+      std::cout << " No." << std::endl;
+
+    // String used to fill with data and send over serial
+    std::string data;
+    ros::Rate loop_rate(1000);
+
+    std::pair<float, float> diff_vel;
+
+    while(ros::ok())
+    {
+
+      diff_vel = calculate_differential_vel(set_linear_velocity, set_rotational_velocity);
+      std::string result;
+      try
+      {
+        result = my_serial_l.read(500); // Read "all" of left motor controller feedback buffer
+        result += my_serial_r.read(500);  // Read "all" of right motor controller feedback buffer
+      }
+      catch(std::exception &e)
+      {
+        result = "";
+        std::string exc = "Unhandled Exception: ";
+        exc += e.what();
+        ROS_ERROR("Unhandled Exception: %s", e.what());
+      }
+
+
+      // Display feedback info from both motor controllers
+      std::cout << "Bytes read: ";
+      std::cout << result.length() << ", String read: " << result << std::endl;
+
+      // Send new joystick data to motor controllers
+
+      size_t bytes_wrote = 0;
+
+      try
+      {
+        if(!inverted_controls) // Normal control layout
+        {
+          // Write to left motor controller
+          data = "!G 1 " + std::to_string(-diff_vel.first) + "\r\n!G 2 " + std::to_string(-diff_vel.first * 1000) + "\r\n";// + std::to_string(joyDataLeft) + "\r\n";
+          bytes_wrote = my_serial_l.write(data);
+
+          // Write to right motor controller
+          data = "!G 1 " + std::to_string(diff_vel.second * 1000) + "\r\n!G 2 " + std::to_string(diff_vel.second * 1000) + "\r\n";// + std::to_string(joyDataLeft) + "\r\n";
+          bytes_wrote += my_serial_r.write(data);
+        }
+        else // Inverted (Drive the rover backward!)
+        {
+          // Write to left motor controller
+          data = "!G 1 " + std::to_string(diff_vel.second * 1000) + "\r\n!G 2 " + std::to_string(diff_vel.second * 1000) + "\r\n";// + std::to_string(joyDataLeft) + "\r\n";
+          bytes_wrote = my_serial_l.write(data);
+
+          // Write to right motor controller
+          data = "!G 1 " + std::to_string(-diff_vel.first * 1000) + "\r\n!G 2 " + std::to_string(-diff_vel.first * 1000) + "\r\n";// + std::to_string(joyDataLeft) + "\r\n";
+          bytes_wrote += my_serial_r.write(data);
+        }      
+      }
+      catch(std::exception &e)
+      {
+        std::string exc = "Unhandled Exception: ";
+        exc += e.what();
+        ROS_ERROR("Unhandled Exception: %s", e.what());
+      }
+
+
+      // Verify the data was sent correctly
+      std::cout << "BYTES: " << bytes_wrote << " ; command: " << data << std::endl;
+
+      // Delay for threaded looping
+      ros::spinOnce();
+      loop_rate.sleep();
+    }
+  }
+
+  protected:
     static bool inverted_controls;
+    // floats to store most-recent data from joystick subscriber
+    static float joyDataLeft;
+    static float joyDataRight;
+    static float set_linear_velocity;
+    static float set_rotational_velocity;
+    static ros::Publisher vel_pub;
 };
 
 #endif
diff --git a/chassis/package.xml b/chassis/package.xml
index ce9925f..8cf9d5f 100644
--- a/chassis/package.xml
+++ b/chassis/package.xml
@@ -64,6 +64,8 @@
   <build_depend>serial</build_depend>
   <build_export_depend>serial</build_export_depend>
   <exec_depend>serial</exec_depend>
+  <build_depend>roboteq_msgs</build_depend>
+  <exec_depend>roboteq_msgs</exec_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/chassis/src/chassis.cpp b/chassis/src/chassis.cpp
index c08ab56..5c2b443 100644
--- a/chassis/src/chassis.cpp
+++ b/chassis/src/chassis.cpp
@@ -1,6 +1,11 @@
 #include "chassis.h"
 
 bool Chassis::inverted_controls = 0;
+float Chassis::joyDataLeft = 0;
+float Chassis::joyDataRight = 0;
+float Chassis::set_linear_velocity = 0;
+float Chassis::set_rotational_velocity = 0;
+ros::Publisher Chassis::vel_pub;
 
 // ros::NodeHandle Chassis::nh_l("","");
 //
@@ -12,3 +17,4 @@ bool Chassis::inverted_controls = 0;
 //   nh_l.initNode(cont_port_1);
 //   nh_r.initNode(cont_port_2);
 // }
+
diff --git a/chassis/src/chassis_control.cpp b/chassis/src/chassis_control.cpp
new file mode 100644
index 0000000..fe57993
--- /dev/null
+++ b/chassis/src/chassis_control.cpp
@@ -0,0 +1,13 @@
+#include <ros/ros.h>
+#include "chassis.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "chassis control");
+
+  ROS_INFO_STREAM("Chassis Control Enabled");
+
+  Chassis::control(1);
+
+  ROS_INFO_STREAM("Chassis Control Disabled");
+}
diff --git a/chassis/src/chassis_create_cmd_vel.cpp b/chassis/src/chassis_create_cmd_vel.cpp
new file mode 100644
index 0000000..af0869a
--- /dev/null
+++ b/chassis/src/chassis_create_cmd_vel.cpp
@@ -0,0 +1,20 @@
+#include <ros/ros.h>
+#include "chassis.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "chassis create cmd_vel");
+
+  ROS_INFO_STREAM("Starting conversion of joystick input to chassis cmd_vel");
+
+  // Subscriber node to handle joystick input data
+  ros::NodeHandle joy_handle;
+  ros::Subscriber sub = joy_handle.subscribe("joy0", 1000, Chassis::create_cmd_vel);
+
+  while(ros::ok())
+  {
+    ros::spinOnce();
+  }
+
+  ROS_INFO_STREAM("Ending conversion of joystick input to chassis cmd_vel");
+}
diff --git a/chassis/src/control.launch b/chassis/src/control.launch
new file mode 100644
index 0000000..a64938b
--- /dev/null
+++ b/chassis/src/control.launch
@@ -0,0 +1,4 @@
+<launch>
+  <include file="$(find joystick_handler)/joystick_handler.launch" />
+  <node pkg="chassis" type="control" name="chassis_control" output="screen"/>
+</launch>
\ No newline at end of file
diff --git a/chassis/src/create_cmd_vel.launch b/chassis/src/create_cmd_vel.launch
new file mode 100644
index 0000000..7736e8e
--- /dev/null
+++ b/chassis/src/create_cmd_vel.launch
@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find joystick_handler)/joystick_handler.launch" />
+  <node pkg="chassis" type="create_cmd_vel" name="chassis_cmd_vel" output="screen"/>
+  <param name='teleop_control_scheme' type='string' value='arcade'/>
+</launch>
\ No newline at end of file
diff --git a/control/joystick_handler/joystick_handler.launch b/control/joystick_handler/joystick_handler.launch
index f264c18..3707503 100644
--- a/control/joystick_handler/joystick_handler.launch
+++ b/control/joystick_handler/joystick_handler.launch
@@ -4,8 +4,8 @@
 
  <!-- joy node -->
  <group>
-     <remap from='joy' to='j0'/>
-     <node pkg='joy' name='j0' type='joy_node'>
+     <remap from='joy' to='joy0'/>
+     <node pkg='joy' name='joy0' type='joy_node'>
          <param name='dev' type='string' value='/dev/SDRCDualshock4'/>
      </node>
  </group>
@@ -22,8 +22,8 @@
   <param name="axis_angular" value="0" type="int"/>
   <param name="scale_linear" value="2" type="double"/>
   <param name="scale_angular" value="2" type="double"/>
-  <param name='joy_pub_topic' type='string' value='chassis/cmd_vel'/>
+  <param name='joy_pub_topic' type='string' value='joy0'/>
 
   <!-- NOTE: pgk and type do not have to match-->
-  <node pkg="joystick_handler" type="joystick_handler" name="teleop" output="screen"/>
+  <node pkg="joystick_handler" type="joystick_handler" name="joystick_handler" output="screen"/>
 </launch>
diff --git a/control/joystick_handler/joystick_handler_turtle.launch b/control/joystick_handler/joystick_handler_turtle.launch
index 0c6477e..6af89a4 100644
--- a/control/joystick_handler/joystick_handler_turtle.launch
+++ b/control/joystick_handler/joystick_handler_turtle.launch
@@ -4,8 +4,8 @@
 
  <!-- joy node -->
  <group>
-     <remap from='joy' to='j0'/>
-     <node pkg='joy' name='j0' type='joy_node'>
+     <remap from='joy' to='joy0'/>
+     <node pkg='joy' name='joy0' type='joy_node'>
          <param name='dev' type='string' value='/dev/SDRCDualshock4'/>
      </node>
  </group>
diff --git a/control/joystick_handler/src/joystick_handler.cpp b/control/joystick_handler/src/joystick_handler.cpp
index 74048de..9a67c21 100644
--- a/control/joystick_handler/src/joystick_handler.cpp
+++ b/control/joystick_handler/src/joystick_handler.cpp
@@ -77,13 +77,13 @@ JoystickHandler::JoystickHandler(int joynum):
   nh_.param("scale_linear", l_scale_, l_scale_);
 
   // Get joystick info from /joy
-  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("j"+std::to_string(joynum), 10, &JoystickHandler::joyCallback, this);
+  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy"+std::to_string(joynum), 10, &JoystickHandler::joyCallback, this);
 
   std::string topic_name;
   nh_.getParam("joy_pub_topic", topic_name);
 
   // Set joystick values in /cmd_vel
-  vel_pub_ = nh_.advertise<geometry_msgs::Twist>(topic_name, 1);
+  vel_pub_ = nh_.advertise<sensor_msgs::Joy>(topic_name, 1);
 
   id = joynum;
 }
@@ -116,10 +116,10 @@ void JoystickHandler::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
   std::cout << "\033[0m\n";
   // printf("\n");
 
-  geometry_msgs::Twist twist;
-  twist.angular.z = joy->axes[angular_]; // Set rotation axis data
-  twist.linear.x = joy->axes[linear_]; // Set forward/backward axis data
-  vel_pub_.publish(twist);
+  // geometry_msgs::Twist twist;
+  // twist.angular.z = joy->axes[angular_]; // Set rotation axis data
+  // twist.linear.x = joy->axes[linear_]; // Set forward/backward axis data
+  // vel_pub_.publish(twist);
 }
 
 void callback(const geometry_msgs::Twist::ConstPtr& msg){