I think the best way to control the sabertooth would be with R/C.
The motors do not have encoders fitted
OK, let's forget PID's and start back from Joystick bluetooth Commander demo sketch
#define VERSION "AndroTest V1.3A - @kas2014\ndemo for V4.X (6 button version)"
// V1.3A Sabertooth version
// Demo setup: motor only, no buttons
// Arduino pin#2 to TX BlueTooth module
// Arduino pin#3 to RX BlueTooth module
// make sure your BT board is set @57600 bps
// better remove SoftSerial for PWM based projects
// For Mega 2560:
// remove #include "SoftwareSerial.h", SoftwareSerial mySerial(2,3);
// search/replace mySerial >> Serial1
// pin#18 to RX bluetooth module, pin#19 to TX bluetooth module
#include "SoftwareSerial.h"
#include <Servo.h> // Sabertooth XXX
Servo motor_R; // Sabertooth XXX
Servo motor_L; // Sabertooth XXX
#define PWM_R 5 // Sabertooth XXX PWM output
#define PWM_L 6 // Sabertooth XXX PWM output
#define STX 0x01
#define ETX 0x00
#define ledPin 13
#define SLOW 1000 // Datafields refresh rate (ms)
#define FAST 250 // Datafields refresh rate (ms)
boolean DEBUG = false;
SoftwareSerial mySerial(2,3); // BlueTooth module: pin#2=TX pin#3=RX
int i=0;
byte cmd[6] = {0, 0, 0, 0, 0, 0}; // bytes received
//byte buttonStatus = 0; // first Byte sent to Android device
//long previousMillis = 0; // will store last time Buttons status was updated
//boolean setButtonFeedback = false; // momentary buttons feedback to Android device
//long sendInterval = SLOW; // interval between Buttons status transmission (milliseconds)
//String displayStatus = "xxxx"; // message to Android device
void setup() {
Serial.begin(57600);
mySerial.begin(57600); // 57600 = max value for softserial
pinMode(ledPin, OUTPUT);
Serial.println(VERSION);
motor_R.attach(PWM_R); // Sabertooth XXX
motor_R.attach(PWM_L); // Sabertooth XXX
}
void loop() {
if(mySerial.available()) { // data received from smartphone
delay(5);
cmd[0] = mySerial.read();
if(cmd[0] == STX) {
i=1;
while(mySerial.available()) {
cmd[i] = mySerial.read();
if(cmd[i] == ETX) {
if(i==2 && cmd[1]>48) break; // Button data
if(i==5 && cmd[1]<3 && cmd[3]<3) break; // Joystick data
}
if(i>5) break;
i++;
}
if (i==2) getButtonState(cmd[1]); // 3 Bytes
else if(i==5) getJoystickState(cmd); // 6 Bytes
else Serial.println("** Communication error **");
if(DEBUG) printDebug(i);
}
} else
sendBlueToothData();
delay(5);
}
void getJoystickState(byte data[5]) {
int joyX = (data[1]<<7) + data[2];
int joyY = (data[3]<<7) + data[4];
joyX = joyX - 200; // Offset to avoid
joyY = joyY - 200; // transmitting negative numbers
if(!DEBUG) {
Serial.print("Joystick position: ");
Serial.print(joyX);
Serial.print(", ");
Serial.println(joyY);
}
// Your code here ...
joyX = map(joyX, -100, 100, 0, 180); // Sabertooth XXX
joyY = map(joyY, -100, 100, 0, 180); // Sabertooth XXX
// 180: full forward, 0: full reverse, 90: stop
motor_R.write(joyX); // Sabertooth XXX
motor_L.write(joyY); // Sabertooth XXX
}
void getButtonState(int bStatus) {
// do nothing
}
void sendBlueToothData() {
// do nothing
}
//String getButtonStatusString() {
//}
//int GetdataInt1() { // Data dummy values sent to Android device for demo purpose
//}
//float GetdataFloat2() { // Data dummy values sent to Android device for demo purpose
//}
void printDebug(int nByte) {
if(nByte ==2) {
// Serial.print("buttonStatus: "); Serial.print(buttonStatus);
// Serial.print(" bin: "); Serial.println(getButtonStatusString());
// Serial.print("Button: < ");
}
else if(nByte ==5) Serial.print("Joystick: < ");
else Serial.print("*error*: < ");
for(int j =0; j<nByte+1; j++) { Serial.print(cmd[j]); Serial.print(" "); }
Serial.println(">");
}
Sabertooth board:
Connect
- Arduino PWM_R to Sabertooth S1
- Arduino PWM_L to Sabertooth S2
- Arduino Ground to Sabertooth Ground
Don't forget dip switches setup
Can't test the code, let me know if it works
Without encoders, you won't have much torque at low speed
Look here for additional info's