  */
 #include <ros.h>
 #include <std_msgs/Int32.h>
-#include <Encoder.h>
 #include <command2ros/ManualCommand.h>
+#include <EncoderFL.h>
+#include <EncoderML.h>
+#include <EncoderFR.h>
+#include <EncoderMR.h>
+#include <EncoderRL.h>
+#include <EncoderRR.h>
 #include "SabertoothDriverROS.h"
-
 
 std_msgs::Int32 msg;
 ros::Publisher pubResults("testing", &msg);
-
+EncoderFL efl;
+EncoderML eml;
+EncoderFR efr;
+EncoderMR emr;
+EncoderRL erl;
+EncoderRR err;
 // Have we done a software E-stop?
 // By default, we have (this way the robot won't move until we get 
 // valid control input)


void updateArticulationValues()
{
  int temp = Encoder::getPosition();
  if (temp < 0)
  {
    temp += 400;
  }
  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
}

void setup(){
  
  // Communicate with the computer
  Encoder::setupEncoder();
  sabertoothDriverNode.initNode();
  sabertoothDriverNode.subscribe(mcSUB);
  sabertoothDriverNode.advertise(pubResults);



/* 
 / This is a simple Arduino program that demonstrates the
 / "Packetized Serial" method of using the Sabertooth Motor
