   rm_cmd += R_PIDout;
 
   // Populate messages
-  //l_enc_msg.data = lm_cmd;
-  //r_enc_msg.data = rm_cmd;
+  // l_enc_msg.data = L_encoderValue;
+  // r_enc_msg.data = rm_cmd;
   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_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 );
   

  R_LastEncoderDelta = R_EncoderDelta;
}

ros::Subscriber<scr_proto::SpeedCommand> motor_sub("speed_command", MotorCallback);


// Rx, Tx, Reset Pins
PololuQik2s12v10 pmd(PMD_RX, PMD_TX, PMD_RESET);


  nh.subscribe(motor_sub);
  
  // Encoder Pubs
  // nh.advertise(left_encoder_pub);
  // nh.advertise(right_encoder_pub);
  
  // Wheel Speeds in rad/s
  nh.advertise(right_wheel_pub);
