 /*
  * 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 <PololuQik.h>
+#include <SoftwareSerial.h>
+#include <scr_proto/DiffCommand.h>
 
 // Encoder stuff
 #define LEFT_ENCODER_A_PIN 2

#define RIGHT_ENCODER_B_PIN 5
#define ENCODER_TIME_DELAY_MS 100
#define TICKS_PER_REVOLUTION 750

// Ros Initialization stuff
ros::NodeHandle  nh;
std_msgs::Int32 l_enc_msg;
std_msgs::Int32 r_enc_msg;

ros::Publisher left_encoder_pub("left_encoder", &l_enc_msg);
ros::Publisher right_encoder_pub("right_encoder", &r_enc_msg);

// Placeholders for motor commands
int lm_cmd = 0;

double R_EncoderAngle = 0; // In revolutions (360.0 degrees = 1.0)

// Motor Callbacks
void MotorCallback(const scr_proto::DiffCommand& motor_com){
  lm_cmd = motor_com.left_motor;
  rm_cmd = motor_com.right_motor;
}

void updateEncoderReading() {
