ar

1. //TWIN WHEELER MODIFIED FOR ARDUINO SIMPLIFIED SERIAL PROTOCOL TO
SABERTOOTH V2
//**********THIS VERSION JUST TESTS COMMUNICATION BETWEEN ARDUINO AND
SABERTOOTH AND MOTORS PLUS DEADMAN SWITCH****************
//*********** DEADMAN SWITCH MUST BE PRESSED IN, MOTORS WILL STOP IF
RELEASED **********************
//MOTORS WILL INCREASE IN SPEED BY 10% EVERY 5 SECONDS UNTIL 50% OF FULL
POWER IS REACHED
//THEY WILL THEN GO DOWN TO ZERO IN STEPS OF 10%.
//BOTH WHEELS SHOULD BE SPINNING IN SAME DIRECTION AT THE SAME SPEED AT ALL
TIMES ELSE YOU WILL HAVE WIRED ONE MOTOR UP BACK TO FRONT
//SABERTOOTH MUST HAVE DIP SWITCHES SET TO THE SERIAL MODE:i.e. DIP
switches 1, 3, 5 and 6 set to ON, the remainder set to OFF
//IMU NOT CONNECTED AT THIS POINT (CAN BE PHYSICALLY CONNECTED IF YOU LIKE
BUT ARDUINO WILL NOT BE READING IT AT ALL)
//ONLY THING YOU NEED IS DEAD MAN SWITCH TO DIGITAL PIN 9 (+5V TO PIN 9
WHEN SWITCH PRESSED, PULL DOWN RESISTOR FROM PIN 9 TO GND KEEPS IT AT 0V
OTHERWISE)
//J. Dingley March 1 2011 For Arduino 328 Duemalinove or similar with a
3.3V accessory power output
//i.e. the current standard Arduino board.
#include <SoftwareSerial.h>
//******************** IMPORTANT ***************************
//Set dip switches on the Sabertooth for simplified serial and 9600
Buadrate. Diagram of this on my Instructables page//
//i.e. DIP switches 1, 3, 5 and 6 set to ON, the remainder set to OFF
//******************** IMPORTANT ***************************
//Digital pin 13 is serial transmit pin to sabertooth
#define SABER_TX_PIN 13
//Not used but still initialised, Digital pin 12 is serial receive from
Sabertooth
#define SABER_RX_PIN 12
//set baudrate to match sabertooth dip settings
#define SABER_BAUDRATE 9600
//simplifierd serial limits for each motor
#define SABER_MOTOR1_FULL_FORWARD 127
#define SABER_MOTOR1_FULL_REVERSE 1
#define SABER_MOTOR2_FULL_FORWARD 255
#define SABER_MOTOR2_FULL_REVERSE 128
//motor level to send when issuing full stop command

2. #define SABER_ALL_STOP 0
SoftwareSerial SaberSerial = SoftwareSerial (SABER_RX_PIN, SABER_TX_PIN );
void initSabertooth (void) {
//initialise software to communicate with sabertooth
pinMode ( SABER_TX_PIN, OUTPUT );
SaberSerial.begin( SABER_BAUDRATE );
//2 sec delay to let it settle
delay (2000);
SaberSerial.print(0, BYTE); //kill motors when first switched on
}
float level = 0;
int u;
//float k1;
//int k2;
//int k3;
int k4;
signed char Motor1percent;
signed char Motor2percent;
//DIGITAL INPUT
int deadmanbuttonPin = 9; // deadman button is digital input pin 9
void setup() // run once, when the sketch starts
{
initSabertooth( );
//analogINPUTS
//digital inputs
pinMode(deadmanbuttonPin, INPUT);
Serial.begin(9600); // HARD wired Serial feedback to PC for
debugging in Wiring
}

3. void sample_inputs() {
k4 = digitalRead(deadmanbuttonPin); //this is needed - if you let go the
motors both stop for your own safety
}
void set_motor() {
unsigned char cSpeedVal_Motor1 = 0;
unsigned char cSpeedVal_Motor2 = 0;
//set motors using the simplified serial Sabertooth protocol (same for
smaller 2 x 5 Watt Sabertooth by the way)
Motor1percent = (signed char) level;
Motor2percent = (signed char) level;
Serial.print("level ");
Serial.println(level);
if (Motor1percent > 100) Motor1percent = 100;
if (Motor1percent < -100) Motor1percent = -100;
if (Motor2percent > 100) Motor2percent = 100;
if (Motor2percent < -100) Motor2percent = -100;
//if not pressing deadman button on hand controller - cut everything
if (k4 < 1) {
level = 0;
Motor1percent = 0;
Motor2percent = 0;
Serial.println("Pressing Dead Man Button");
delay(1000);
}
cSpeedVal_Motor1 = map (Motor1percent,
-100,
100,
SABER_MOTOR1_FULL_REVERSE,
SABER_MOTOR1_FULL_FORWARD);
cSpeedVal_Motor2 = map (Motor2percent,
-100,
100,

4. SABER_MOTOR2_FULL_REVERSE,
SABER_MOTOR2_FULL_FORWARD);
SaberSerial.print (cSpeedVal_Motor1, BYTE);
SaberSerial.print (cSpeedVal_Motor2, BYTE);
/*
//debugging
Serial.print("Motor1percent = ");
Serial.print(Motor1percent);
Serial.print (" level = ");
Serial.println (level);
Serial.print("Motor2percent = ");
Serial.println(Motor2percent);
Serial.print("cSpeedVal_Motor1 = ");
Serial.println(cSpeedVal_Motor1);
Serial.print("cSpeedVal_Motor2 = ");
Serial.println(cSpeedVal_Motor2);
*/
delay(5000); //5 second delay
}
void loop () {
for (u=0; u<5; u++) {
level= u * 10; //level = 0 then 10, then 20 until it gets to 50% of max
motor power. each set motor routine has 5 sec delay at end of it
sample_inputs(); //check the dead man button still pressed in
set_motor();
}
for (u=0; u<5; u++) {
level= (5- u) * 10; //level = 50 then 40, then 30 then 20 until it gets
to 0% of max motor power. each set motor routine has 5 sec delay at end of
it
sample_inputs();//check the dead man button still pressed in
set_motor();
}

5. }