I’m working with some Kangaroos to command a mecanum wheeled robot. Using just the Sabertooth motor controllers works fine, but now I’m trying to add some speed control into the mix and I’m having difficulty. Contacting their support is like pulling teeth, so I figure I would ask here just in case anyone else has had similar issues.
Now, the code below “works” but the issue that I’m having is that the speed commands seem to take up to 30 seconds to complete. From what I can tell the Kangaroos get the command but they are having issues “getting there” and moving on. I seem to be unable to find a way to 1.) set a deadband around the desired velocity 2.) assign the velocity to the motor controller without blocking my program. I’m going to be controlling the robot over Ethernet and it’s unacceptable to have to wait 10-20 seconds before the next command is executed.
Motor Controller: Sabertooth 2x25
http://www.dimensionengineering.com/pro … rtooth2x25
Motion Controller: Kangaroo x2
http://www.dimensionengineering.com/products/kangaroo
#include <SoftwareSerial.h>
#include <Kangaroo.h>
#define TX_PIN 10
#define RX_PIN 11
unsigned long currentTime = 0;
unsigned long previousTime = 0;
SoftwareSerial SerialPort(RX_PIN, TX_PIN);
KangarooSerial K_FRONT(SerialPort);
KangarooChannel K1_F(K_FRONT, '1', 128);
KangarooChannel K2_F(K_FRONT, '2', 128);
KangarooSerial K_REAR(SerialPort);
KangarooChannel K1_R(K_REAR, '1', 129);
KangarooChannel K2_R(K_REAR, '2', 129);
KangarooStatus K_STATUS;
void setup()
{
Serial.begin(115200);
SerialPort.begin(9600);
SerialPort.listen();
delay(2000);
K1_F.start();
K1_F.home();//.wait();
K2_F.start();
K2_F.home();//.wait();
K1_R.start();
K1_R.home();//.wait();
K2_R.start();
K2_R.home();//.wait();
}
void loop()
{
int i = 0;
int j = 0;
unsigned char kERROR;
previousTime = millis();
for(i = 0; i < 1250; i = i + 100)
{
j = -1 * i;
Serial.print("[1]");
K1_F.s(i);
printTime();
K2_F.s(i);
printTime();
K1_R.s(j);
printTime();
K2_R.s(j);
printTime();
delay(100);
Serial.println();
}
for(i = 1250; i > -1250; i = i - 100)
{
j = -1 * i;
Serial.print("[2]");
K1_F.s(i);
printTime();
K2_F.s(i);
printTime();
K1_R.s(j);
printTime();
K2_R.s(j);
printTime();
delay(100);
Serial.println();
}
for(i = -1250; i <= 0; i = i + 100)
{
j = -1 * i;
Serial.print("[3]");
K1_F.s(i);
printTime();
K2_F.s(i);
printTime();
K1_R.s(j);
printTime();
K2_R.s(j);
delay(100);
Serial.println();
}
}
void printTime()
{
currentTime = millis();
Serial.print("[");Serial.print(currentTime-previousTime);Serial.print("]");
previousTime=millis();
}