 double R_EncoderAngle = 0; // In revolutions (360.0 degrees = 1.0)
 
 // Motor Callbacks
-void leftMotorCallback(const std_msgs::Int32& lm_msg){
-  lm_cmd = lm_msg.data;
-}
-
-void rightMotorCallback(const std_msgs::Int32& rm_msg){
-  rm_cmd = rm_msg.data;
+void MotorCallback(const scr_proto::DiffCommand& motor_com){
+  lm_cmd = motor_com.left_motor;
+  rm_cmd = motor_com.right_motor;
 }
 
 void updateEncoderReading() {

  L_EncoderVelocity = 1000.0*(float)(L_encoderValue-L_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;
  R_EncoderVelocity = 1000.0*(float)(R_encoderValue-L_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;

  // Convert to radians/sec
  L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
  R_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
  
  // Set last known encoder ticks
  L_LastEncoderValue = L_encoderValue;
  R_LastEncoderValue = R_encoderValue;
}

ros::Subscriber<std_msgs::Int32> leftMotorSub("left_motor", leftMotorCallback);
ros::Subscriber<std_msgs::Int32> rightMotorSub("right_motor", rightMotorCallback);


// Rx, Tx, Reset Pins
//PololuQik2s12v10 pmd(2, 3, 4);

void setup()
{

  nh.advertise(right_encoder_pub);

  // Motor Driver Init
  pmd.init();
}

void loop()
{
  // Get Freshest Values
  updateEncoderReading();

  // Populate messages
  l_enc_msg.data = L_LastEncoderValue;
  r_enc_msg.data = R_LastEncoderValue;
  l_wheel_msg.data = L_WheelVelocity;
  r_wheel_msg.data = R_WheelVelocity;

  // Publish Data
  left_encoder_pub.publish( &l_enc_msg );
  right_encoder_pub.publish( &r_enc_msg );
  left_wheel_pub.publish( &l_wheel_msg );
  right_wheel_pub.publish( &r_wheel_msg );
  
  // Handle Callbacks
  nh.spinOnce();
  
  // Write Speeds to motor driver
  pmd.setSpeeds(lm_cmd, rm_cmd);
  
  delay(10);
}

