Litan Virgil / Sabertooth

Library for Sabertooth motor driver ported from the oficial Arduino library. Packetized Serial Mode

I ported the official arduino library for Sabertooth dual motor drivers. In the porting process I did not used const methods as the original library does, but the functionality stays the same. I also added some methods that I find useful. According to the documentation of the driver this code should be compatible with SyRen drivers.

Sabertooth.cpp@0:70797f033fe0, 2017-02-20 (annotated)

Committer:
Elefantul_umilit
Date:
Mon Feb 20 11:10:08 2017 +0000
Revision:
0:70797f033fe0
Child:
1:013fa8604413
Library for Sabertooth ported from the oficial Arduino library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Elefantul_umilit 0:70797f033fe0 1 /*
Elefantul_umilit 0:70797f033fe0 2 Arduino Library for SyRen/Sabertooth Packet Serial
Elefantul_umilit 0:70797f033fe0 3 Copyright (c) 2012-2013 Dimension Engineering LLC
Elefantul_umilit 0:70797f033fe0 4 http://www.dimensionengineering.com/arduino
Elefantul_umilit 0:70797f033fe0 5
Elefantul_umilit 0:70797f033fe0 6 Permission to use, copy, modify, and/or distribute this software for any
Elefantul_umilit 0:70797f033fe0 7 purpose with or without fee is hereby granted, provided that the above
Elefantul_umilit 0:70797f033fe0 8 copyright notice and this permission notice appear in all copies.
Elefantul_umilit 0:70797f033fe0 9
Elefantul_umilit 0:70797f033fe0 10 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
Elefantul_umilit 0:70797f033fe0 11 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
Elefantul_umilit 0:70797f033fe0 12 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
Elefantul_umilit 0:70797f033fe0 13 SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
Elefantul_umilit 0:70797f033fe0 14 RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
Elefantul_umilit 0:70797f033fe0 15 NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
Elefantul_umilit 0:70797f033fe0 16 USE OR PERFORMANCE OF THIS SOFTWARE.
Elefantul_umilit 0:70797f033fe0 17 */
Elefantul_umilit 0:70797f033fe0 18
Elefantul_umilit 0:70797f033fe0 19 /*
Elefantul_umilit 0:70797f033fe0 20 * Ported to mbed by Litan Virgil, 2017
Elefantul_umilit 0:70797f033fe0 21 * I added some features that I found useful.
Elefantul_umilit 0:70797f033fe0 22 * Also I removed "const" methods
Elefantul_umilit 0:70797f033fe0 23 */
Elefantul_umilit 0:70797f033fe0 24
Elefantul_umilit 0:70797f033fe0 25 #include "Sabertooth.h"
Elefantul_umilit 0:70797f033fe0 26 #include "ArduinoMath.h"//compatibility with arduino math methods
Elefantul_umilit 0:70797f033fe0 27
Elefantul_umilit 0:70797f033fe0 28 Sabertooth::Sabertooth(PinName Tx, uint8_t address, int baudrate) : _sabertoothserial(Tx, NC) {
Elefantul_umilit 0:70797f033fe0 29 _address = address;
Elefantul_umilit 0:70797f033fe0 30 _sabertoothserial.baud(baudrate);
Elefantul_umilit 0:70797f033fe0 31 }
Elefantul_umilit 0:70797f033fe0 32
Elefantul_umilit 0:70797f033fe0 33 void Sabertooth::initializeComunication(void){
Elefantul_umilit 0:70797f033fe0 34 wait(1.5); // wait for sabertooth to initialized
Elefantul_umilit 0:70797f033fe0 35 _sabertoothserial.putc(START_CHARACTER);
Elefantul_umilit 0:70797f033fe0 36 wait(0.5);
Elefantul_umilit 0:70797f033fe0 37 }
Elefantul_umilit 0:70797f033fe0 38
Elefantul_umilit 0:70797f033fe0 39 void Sabertooth::go(int aSpeed, int bSpeed){
Elefantul_umilit 0:70797f033fe0 40 motor(1, aSpeed);
Elefantul_umilit 0:70797f033fe0 41 motor(2, bSpeed);
Elefantul_umilit 0:70797f033fe0 42 }
Elefantul_umilit 0:70797f033fe0 43
Elefantul_umilit 0:70797f033fe0 44 void Sabertooth::stop(){
Elefantul_umilit 0:70797f033fe0 45 go(0, 0);
Elefantul_umilit 0:70797f033fe0 46 }
Elefantul_umilit 0:70797f033fe0 47
Elefantul_umilit 0:70797f033fe0 48 void Sabertooth::goDT(int drivePower, int turnPower){
Elefantul_umilit 0:70797f033fe0 49 drive(drivePower);
Elefantul_umilit 0:70797f033fe0 50 turn(turnPower);
Elefantul_umilit 0:70797f033fe0 51 }
Elefantul_umilit 0:70797f033fe0 52
Elefantul_umilit 0:70797f033fe0 53 void Sabertooth::go(int aSpeed, int bSpeed, float secs){
Elefantul_umilit 0:70797f033fe0 54 go(aSpeed, bSpeed);
Elefantul_umilit 0:70797f033fe0 55 timeOff.attach(callback(this,&Sabertooth::stop), secs); // time to off
Elefantul_umilit 0:70797f033fe0 56 }
Elefantul_umilit 0:70797f033fe0 57
Elefantul_umilit 0:70797f033fe0 58 void Sabertooth::goDT(int drivePower, int turnPower, float secs){
Elefantul_umilit 0:70797f033fe0 59 goDT(drivePower, turnPower);
Elefantul_umilit 0:70797f033fe0 60 timeOff.attach(callback(this,&Sabertooth::stop), secs); // time to off
Elefantul_umilit 0:70797f033fe0 61 }
Elefantul_umilit 0:70797f033fe0 62
Elefantul_umilit 0:70797f033fe0 63
Elefantul_umilit 0:70797f033fe0 64 void Sabertooth::command(uint8_t command, uint8_t value) {
Elefantul_umilit 0:70797f033fe0 65 _sabertoothserial.putc(_address);
Elefantul_umilit 0:70797f033fe0 66 _sabertoothserial.putc(command);
Elefantul_umilit 0:70797f033fe0 67 _sabertoothserial.putc(value);
Elefantul_umilit 0:70797f033fe0 68 _sabertoothserial.putc((_address+command+value) & 0x7f); //checksum
Elefantul_umilit 0:70797f033fe0 69 }
Elefantul_umilit 0:70797f033fe0 70
Elefantul_umilit 0:70797f033fe0 71 void Sabertooth::throttleCommand(uint8_t command, int power)
Elefantul_umilit 0:70797f033fe0 72 {
Elefantul_umilit 0:70797f033fe0 73 power = ArduinoMath::constrain(power, -126, 126);
Elefantul_umilit 0:70797f033fe0 74 this->command(command, (uint8_t)abs(power));
Elefantul_umilit 0:70797f033fe0 75 }
Elefantul_umilit 0:70797f033fe0 76
Elefantul_umilit 0:70797f033fe0 77 void Sabertooth::motor(int power)
Elefantul_umilit 0:70797f033fe0 78 {
Elefantul_umilit 0:70797f033fe0 79 motor(1, power);
Elefantul_umilit 0:70797f033fe0 80 }
Elefantul_umilit 0:70797f033fe0 81
Elefantul_umilit 0:70797f033fe0 82 void Sabertooth::motor(uint8_t motor, int power)
Elefantul_umilit 0:70797f033fe0 83 {
Elefantul_umilit 0:70797f033fe0 84 if (motor < 1 || motor > 2) { return; }
Elefantul_umilit 0:70797f033fe0 85 throttleCommand((motor == 2 ? 4 : 0) + (power < 0 ? 1 : 0), power);
Elefantul_umilit 0:70797f033fe0 86 }
Elefantul_umilit 0:70797f033fe0 87
Elefantul_umilit 0:70797f033fe0 88 void Sabertooth::drive(int power)
Elefantul_umilit 0:70797f033fe0 89 {
Elefantul_umilit 0:70797f033fe0 90 throttleCommand(power < 0 ? 9 : 8, power);
Elefantul_umilit 0:70797f033fe0 91 }
Elefantul_umilit 0:70797f033fe0 92
Elefantul_umilit 0:70797f033fe0 93 void Sabertooth::turn(int power)
Elefantul_umilit 0:70797f033fe0 94 {
Elefantul_umilit 0:70797f033fe0 95 throttleCommand(power < 0 ? 11 : 10, power);
Elefantul_umilit 0:70797f033fe0 96 }
Elefantul_umilit 0:70797f033fe0 97
Elefantul_umilit 0:70797f033fe0 98 void Sabertooth::setMinVoltage(uint8_t value)
Elefantul_umilit 0:70797f033fe0 99 {
Elefantul_umilit 0:70797f033fe0 100 command(2, (uint8_t)ArduinoMath::min(value, 120));
Elefantul_umilit 0:70797f033fe0 101 }
Elefantul_umilit 0:70797f033fe0 102
Elefantul_umilit 0:70797f033fe0 103 void Sabertooth::setMaxVoltage(uint8_t value)
Elefantul_umilit 0:70797f033fe0 104 {
Elefantul_umilit 0:70797f033fe0 105 command(3, (uint8_t)ArduinoMath::min(value, 127));
Elefantul_umilit 0:70797f033fe0 106 }
Elefantul_umilit 0:70797f033fe0 107
Elefantul_umilit 0:70797f033fe0 108 void Sabertooth::setDeadband(uint8_t value)
Elefantul_umilit 0:70797f033fe0 109 {
Elefantul_umilit 0:70797f033fe0 110 command(17, (uint8_t)ArduinoMath::min(value, 127));
Elefantul_umilit 0:70797f033fe0 111 }
Elefantul_umilit 0:70797f033fe0 112
Elefantul_umilit 0:70797f033fe0 113 void Sabertooth::setRamping(uint8_t value)
Elefantul_umilit 0:70797f033fe0 114 {
Elefantul_umilit 0:70797f033fe0 115 command(16, (uint8_t)ArduinoMath::constrain(value, 0, 80));
Elefantul_umilit 0:70797f033fe0 116 }
Elefantul_umilit 0:70797f033fe0 117
Elefantul_umilit 0:70797f033fe0 118 void Sabertooth::setTimeout(int milliseconds)
Elefantul_umilit 0:70797f033fe0 119 {
Elefantul_umilit 0:70797f033fe0 120 command(14, (uint8_t)((ArduinoMath::constrain(milliseconds, 0, 12700) + 99) / 100));
Elefantul_umilit 0:70797f033fe0 121 }
Elefantul_umilit 0:70797f033fe0 122
Elefantul_umilit 0:70797f033fe0 123
Elefantul_umilit 0:70797f033fe0 124 void Sabertooth::setBaudRate(int baudRate)
Elefantul_umilit 0:70797f033fe0 125 {
Elefantul_umilit 0:70797f033fe0 126 uint8_t value;
Elefantul_umilit 0:70797f033fe0 127 switch (baudRate)
Elefantul_umilit 0:70797f033fe0 128 {
Elefantul_umilit 0:70797f033fe0 129 case 2400: value = 1; break;
Elefantul_umilit 0:70797f033fe0 130 case 9600: default: value = 2; break;
Elefantul_umilit 0:70797f033fe0 131 case 19200: value = 3; break;
Elefantul_umilit 0:70797f033fe0 132 case 38400: value = 4; break;
Elefantul_umilit 0:70797f033fe0 133 case 115200: value = 5; break;
Elefantul_umilit 0:70797f033fe0 134 }
Elefantul_umilit 0:70797f033fe0 135 command(15, value);
Elefantul_umilit 0:70797f033fe0 136 // Sabertooth takes about 200 ms after setting the baud rate to
Elefantul_umilit 0:70797f033fe0 137 // respond to commands again (it restarts).
Elefantul_umilit 0:70797f033fe0 138 // So, this 500 ms wait should deal with this.
Elefantul_umilit 0:70797f033fe0 139 wait(0.5);
Elefantul_umilit 0:70797f033fe0 140 }
Elefantul_umilit 0:70797f033fe0 141