I'm working with some Kangaroos to command a mecanum wheeled robot. Using just the Sabertooth motor controllers works fine, but 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.
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. All I'm trying to do is set a speed that I want that motor to turn at and then update it with a new speed when the next command is set. Once I have the motion controller worked out, the full firmware will send down a packet every 100ms or so.
Motor Controller: Sabertooth 2x25
http://www.dimensionengineering.com/products/sabertooth2x25
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();
}