 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()
{

/*
 * Squad Collaborative Robotics
 * Prototype Embedded Collector Bot Code
   Arduino Mega 2560 R3
 */


#include <ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float64.h>
#include <Event.h>
#include <Timer.h>
#include <Encoder.h>
#include <PololuQik.h>
#include <SoftwareSerial.h>
#include <scr_proto/DiffCommand.h>

// Encoder stuff
#define LEFT_ENCODER_A_PIN 2
