ROS Arduino Sabertooth Controller

edit

Hi, I am trying to control a sabertooth 2x60 motor controller with an arduino interfaced to a raspberry pi using rosserial_arduino. I have the connection working great, but whenever I try to publish a command to the topic that the arduino is subscribed to, the sabertooth controller does nothing. I can verify that the callback is working because of the receipt of various loginfo back to the terminal. Also, I uploaded code to control the sabertooth (with no ROS), and it worked fine, so I know I have everything connected right. Any ideas what is wrong/missing in the following code? Thanks

#include <Sabertooth.h>
#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

Sabertooth ST(128);


void sabertooth_callback(const std_msgs::UInt16& cmd_msg){
  int power = cmd_msg.data;
  nh.loginfo("Got Command");
  // Ramp motor 1 from -127 to 127 (full reverse to full forward),
  // waiting 20 ms (1/50th of a second) per value.
  for (int power1 = -power; power1 <= 127; power1 ++)
  {
    ST.drive(power);
    delay(20);
    nh.loginfo("Got Command to Drive"); 
  }

  // Now go back the way we came.
  for (power = 127; power >= -127; power --)
  {
    ST.turn(power); // Tip for SyRen users: Typing ST.motor(power) does the same thing as ST.motor(1, power).
    nh.loginfo("Got Command to Turn"); 
    delay(20); 
  }
} 

ros::Subscriber<std_msgs::UInt16> sub("Sabertooth", sabertooth_callback);


void setup()
{ 
  ST.drive(0);
  ST.turn(0);
  SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
  nh.initNode();
  nh.subscribe(sub);
}

void loop()
{
  nh.spinOnce();
}