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@1:013fa8604413, 2017-09-21 (annotated)
- Committer:
- Elefantul_umilit
- Date:
- Thu Sep 21 12:32:29 2017 +0300
- Revision:
- 1:013fa8604413
- Parent:
- 0:70797f033fe0
solved bug in setBaudRate. Now the baud is updated on the mcu as well
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:013fa8604413 | 130 | case 9600: default: value = 2; baudRate = 9600; 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 | 1:013fa8604413 | 136 | _sabertoothserial.baud(baudRate); |
Elefantul_umilit | 0:70797f033fe0 | 137 | // Sabertooth takes about 200 ms after setting the baud rate to |
Elefantul_umilit | 0:70797f033fe0 | 138 | // respond to commands again (it restarts). |
Elefantul_umilit | 0:70797f033fe0 | 139 | // So, this 500 ms wait should deal with this. |
Elefantul_umilit | 0:70797f033fe0 | 140 | wait(0.5); |
Elefantul_umilit | 0:70797f033fe0 | 141 | } |
Elefantul_umilit | 0:70797f033fe0 | 142 |