I am working on a project where I have an Arduino Uno connected to a Sabertooth 2x12 that controls both of my motors. I have it communicating over Digital port 11 because I have an XBee that needed sole control of the Hardware UART. Therefore, I am using the SoftwareSerial Library and I am running into an issue. When I try to use the same .drive() and .turn() methods from before, the motor controller does nothing. However, if I use the ST.motor() commands instead, I am able to control them. This setup is not completely ideal because I am trying to use ROS Twist messages with the Sabertooth, so I need to be able to use the .drive() and .turn() methods to convert those into actual control values. Is there something I am missing in the code below? There is communication between the Arduino and the Sabertooth, and I can control it with the .motor() command, but not with .turn() or .drive(). However before using Software Serial, I was able to use the latter two methods.
Thanks.
My Code for Reference:
#include <ros.h>
#include <std_msgs/Int32.h>
#include <SabertoothSimplified.h>
#include <SoftwareSerial.h>
// Configure Sabertooth
SoftwareSerial SWSerial(NOT_A_PIN, 11);
SabertoothSimplified ST(SWSerial);
// Speed Callback
// We will use the code block below to ramp up or down the speed to the
// specified value received from the roscore.
int currentSpeed = 0;
void messageCb(const std_msgs::Int32 &vel_msg){
int goalSpeed = vel_msg.data;
int startSpeed = currentSpeed;
if(goalSpeed > currentSpeed){
for(currentSpeed = startSpeed; currentSpeed < goalSpeed; currentSpeed++){
ST.motor(1, currentSpeed);
ST.motor(2, currentSpeed);
delay(20);
}
} else if(goalSpeed < currentSpeed){
for(currentSpeed = startSpeed; currentSpeed > goalSpeed; currentSpeed--){
ST.motor(1, currentSpeed);
ST.motor(2, currentSpeed);
delay(20);
}
}
}
// Setup ROS Publisher and Subscriber
ros::Subscriber<std_msgs::Int32> sub("cmd_vel", &messageCb );
std_msgs::Int32 publishedSpeed;
ros::Publisher speedReport("speedReport", &publishedSpeed);
ros::NodeHandle nh;
void setup() {
// Set Baud Rate to 9600 for Sabertooth and XBee Communications.
nh.getHardware()->setBaud(9600);
SWSerial.begin(9600);
ST.motor(1, 0);
ST.motor(2, 0);
// Initialize the Node Handler
nh.initNode();
nh.advertise(speedReport);
nh.subscribe(sub);
}
void loop() {
// Get the speed values.
publishedSpeed.data = currentSpeed;
speedReport.publish(&publishedSpeed);
nh.spinOnce();
delay(100);
}
Your code makes not reference to .turn() or .drive() so how can we tell if things are correct? I would suggest you try the Tank Style Sweep example that comes with the library to see if that works. It is a simpler way to verify things before adding more complications.
Here is what the code looks like with the references added in:
#include <ros.h>
#include <std_msgs/Int32.h>
#include <SabertoothSimplified.h>
#include <SoftwareSerial.h>
// Configure Sabertooth
SoftwareSerial SWSerial(NOT_A_PIN, 11);
SabertoothSimplified ST(SWSerial);
// Speed Callback
// We will use the code block below to ramp up or down the speed to the
// specified value received from the roscore.
int currentSpeed = 0;
void messageCb(const std_msgs::Int32 &vel_msg){
int goalSpeed = vel_msg.data;
int startSpeed = currentSpeed;
if(goalSpeed > currentSpeed){
for(currentSpeed = startSpeed; currentSpeed < goalSpeed; currentSpeed++){
ST.drive(currentSpeed);
delay(20);
}
} else if(goalSpeed < currentSpeed){
for(currentSpeed = startSpeed; currentSpeed > goalSpeed; currentSpeed--){
ST.drive(currentSpeed);
delay(20);
}
}
}
// Setup ROS Publisher and Subscriber
ros::Subscriber<std_msgs::Int32> sub("cmd_vel", &messageCb );
std_msgs::Int32 publishedSpeed;
ros::Publisher speedReport("speedReport", &publishedSpeed);
ros::NodeHandle nh;
void setup() {
// Set Baud Rate to 9600 for Sabertooth and XBee Communications.
nh.getHardware()->setBaud(9600);
SWSerial.begin(9600);
ST.drive(0);
// Initialize the Node Handler
nh.initNode();
nh.advertise(speedReport);
nh.subscribe(sub);
}
void loop() {
// Get the speed values.
publishedSpeed.data = currentSpeed;
speedReport.publish(&publishedSpeed);
nh.spinOnce();
delay(100);
}
And here is what the demo looks like for the Tank-Sweep style. One difference to note is that they wire the sabertooth into the default TX/RX port. However, I cannot do this due to other hardware I have on the Arduino. I feel that using a SotftwareSerial port should make no difference, but it clearly does and I cannot tell why:
// Tank-Style Sweep Sample
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <SabertoothSimplified.h>
// Mixed mode is for tank-style diff-drive robots.
// Only Packet Serial actually has mixed mode, so this Simplified Serial library
// emulates it (to allow easy switching between the two libraries).
SabertoothSimplified ST; // We'll name the Sabertooth object ST.
// For how to configure the Sabertooth, see the DIP Switch Wizard for
// http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
// Be sure to select Simplified Serial Mode for use with this library.
// This sample uses a baud rate of 9600.
//
// Connections to make:
// Arduino TX->1 -> Sabertooth S1
// Arduino GND -> Sabertooth 0V
// Arduino VIN -> Sabertooth 5V (OPTIONAL, if you want the Sabertooth to power the Arduino)
//
// If you want to use a pin other than TX->1, see the SoftwareSerial example.
void setup()
{
SabertoothTXPinSerial.begin(9600); // This is the baud rate you chose with the DIP switches.
ST.drive(0); // The Sabertooth won't act on mixed mode until
ST.turn(0); // it has received power levels for BOTH throttle and turning, since it
// mixes the two together to get diff-drive power levels for both motors.
// So, we set both to zero initially.
}
// Mixed mode tips:
// drive() should go forward and back, turn() should go right and left.
// If this is reversed, swap M2A and M2B.
// Positive on drive() should go forward, negative should go backward.
// If this is reversed, swap A and B on both M1 and M2.
// Positive on turn() should go right, negative should go left.
// If this is reversed, swap M1 and M2.
// In this sample, the SLOW sweep (left-to-right) here is turning,
// and the FAST sweep (backwards-to-forwards) is throttle.
void loop()
{
int power;
// Don't turn. Ramp from going backwards to going forwards, waiting 20 ms (1/50th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.drive(power);
delay(20);
}
// Now, let's use a power level of 20 (out of 127) forward.
// This way, our turning will have a radius. Mostly, the command
// is just to demonstrate you can use drive() and turn() at the same time.
ST.drive(20);
// Ramp turning from full left to full right SLOWLY by waiting 50 ms (1/20th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.turn(power);
delay(50);
}
// Now stop turning, and stop driving.
ST.turn(0);
ST.drive(0);
// Wait a bit. This is so you can catch your robot if you want to. :-)
delay(5000);
}
[\code]