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


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);

 */
#include <ros.h>
#include <std_msgs/Int32.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)
