Twin wheeler modified for arduino simplified serial protocol to sabertooth v2
- 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. }