   
   //Command Subscriber
   nh.subscribe(motor_sub);
+  nh.subscribe(command_sub);
   
   // Wheel Speeds in rad/s
   nh.advertise(right_wheel_pub);

PID L_DCMotorPID(&L_WheelVelocity,
                 &L_PIDout,
                 &lw_cmd_spd,
                 0.65,
                 0.00,
                 0.10,
                 REVERSE);

PID R_DCMotorPID(&R_WheelVelocity,
                 &R_PIDout,
                 &rw_cmd_spd,
                 0.6,
                 0.00,
                 0.10,
                 REVERSE);

// Motor Callbacks
void MotorCallback(const scr_proto::SpeedCommand& motor_com){

#define PMD_RESET 8

// Ros Initialization stuff
ros::NodeHandle_<ArduinoHardware, 10, 10, 256, 256>  nh;

// Wheel Speed Messages
std_msgs::Float32 l_wheel_msg;
