adeck/rib-test-code
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add sabertooth lib, start enabling all motor drive

Browse Source
This commit is contained in:
Cole Deck 2023-09-13 11:30:59 -05:00
parent 8f59f0fb15
commit 109b2c92c4
19 changed files with 1158 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
lib/Sabertooth/changes.txt Normal file
View File

@ -0,0 +1,37 @@
Arduino Libraries for SyRen/Sabertooth
Copyright (c) 2012-2013 Dimension Engineering LLC
http://www.dimensionengineering.com/arduino
1 July 2013, Version 1.5
- USB Sabertooth Packet Serial Library
- Initial release.
1 April 2013, Version 1.4
- Added documentation.
- Improved support for Arduino Leonardo.
- Packet Serial Library
- Simplified the code to save space and increase readability.
3 May 2012, Version 1.3
- Improved compatibility with earlier versions of the Arduino software.
Previous versions required Arduino 1.0 or newer, but this version
should work back to Arduino 0018 (still in use by Debian Linux 6.0)
and possibly earlier.
If you are using a version of Arduino older than 1.0, you will need
to rename the examples from .ino to .pde to see them in the Examples
menu.
20 April 2012, Version 1.2
- Packet Serial Library
- Added a stop() function to match the command set of the Simplified
Serial libraries.
19 April 2012, Version 1.1
- Packet Serial Library
- Added a single-argument motor() function as a convenience for SyRen.
18 April 2012, Version 1.0
- Initial release.
Please let us know if you run into any bugs! Thanks and enjoy the library!

46
lib/Sabertooth/examples/1.Basics/Jolty/Jolty.ino Normal file
View File

@ -0,0 +1,46 @@
// Jolty Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <Sabertooth.h>
Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
// If you've set up your Sabertooth on a different address, of course change
// that here. For how to configure address, etc. see the DIP Switch Wizard for
// Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
// SyRen - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
// Be sure to select Packetized Serial Mode for use with this library.
//
// On that note, you can use this library for SyRen just as easily.
// The diff-drive commands (drive, turn) do not work on a SyRen, of course, but it will respond correctly
// if you command motor 1 to do something (ST.motor(1, ...)), just like a Sabertooth.
//
// In this sample, hardware serial TX connects to S1.
// See the SoftwareSerial example in 3.Advanced for how to use other pins.
void setup()
{
SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
// NOTE: *Not all* Sabertooth controllers need this command.
// It doesn't hurt anything, but V2 controllers use an
// EEPROM setting (changeable with the function setBaudRate) to set
// the baud rate instead of detecting with autobaud.
//
// If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
// the autobaud line and save yourself two seconds of startup delay.
}
void loop()
{
ST.motor(1, 127); // Go forward at full power.
delay(2000); // Wait 2 seconds.
ST.motor(1, 0); // Stop.
delay(2000); // Wait 2 seconds.
ST.motor(1, -127); // Reverse at full power.
delay(2000); // Wait 2 seconds.
ST.motor(1, 0); // Stop.
delay(2000);
}

53
lib/Sabertooth/examples/1.Basics/Sweep/Sweep.ino Normal file
View File

@ -0,0 +1,53 @@
// Sweep Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <Sabertooth.h>
Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
// If you've set up your Sabertooth on a different address, of course change
// that here. For how to configure address, etc. see the DIP Switch Wizard for
// Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
// SyRen - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
// Be sure to select Packetized Serial Mode for use with this library.
//
// On that note, you can use this library for SyRen just as easily.
// The diff-drive commands (drive, turn) do not work on a SyRen, of course, but it will respond correctly
// if you command motor 1 to do something (ST.motor(1, ...)), just like a Sabertooth.
//
// In this sample, hardware serial TX connects to S1.
// See the SoftwareSerial example in 3.Advanced for how to use other pins.
void setup()
{
SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
// NOTE: *Not all* Sabertooth controllers need this command.
// It doesn't hurt anything, but V2 controllers use an
// EEPROM setting (changeable with the function setBaudRate) to set
// the baud rate instead of detecting with autobaud.
//
// If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
// the autobaud line and save yourself two seconds of startup delay.
}
void loop()
{
int power;
// Ramp motor 1 from -127 to 127 (full reverse to full forward),
// waiting 20 ms (1/50th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.motor(1, power);
delay(20);
}
// Now go back the way we came.
for (power = 127; power >= -127; power --)
{
ST.motor(1, power); // Tip for SyRen users: Typing ST.motor(power) does the same thing as ST.motor(1, power).
delay(20); // Since SyRen doesn't have a motor 2, this alternative can save you typing.
}
}

69
lib/Sabertooth/examples/1.Basics/TankStyleSweep/TankStyleSweep.ino Normal file
View File

@ -0,0 +1,69 @@
// Tank-Style Sweep Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <Sabertooth.h>
// Mixed mode is for tank-style diff-drive robots.
Sabertooth ST(128); // The Sabertooth is on address 128. We'll name its object ST.
// If you've set up your Sabertooth on a different address, of course change
// that here. For how to configure address, etc. see the DIP Switch Wizard for
// Sabertooth - http://www.dimensionengineering.com/datasheets/SabertoothDIPWizard/start.htm
// SyRen - http://www.dimensionengineering.com/datasheets/SyrenDIPWizard/start.htm
// Be sure to select Packetized Serial Mode for use with this library.
//
// This sample uses the mixed mode (diff-drive) commands, which require two motors
// and hence do not work on a SyRen.
//
// In this sample, hardware serial TX connects to S1.
// See the SoftwareSerial example in 3.Advanced for how to use other pins.
void setup()
{
SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
// NOTE: *Not all* Sabertooth controllers need this command.
// It doesn't hurt anything, but V2 controllers use an
// EEPROM setting (changeable with the function setBaudRate) to set
// the baud rate instead of detecting with autobaud.
//
// If you have a 2x12, 2x25 V2, 2x60 or SyRen 50, you can remove
// the autobaud line and save yourself two seconds of startup delay.
ST.drive(0); // The Sabertooth won't act on mixed mode packet serial commands until
ST.turn(0); // it has received power levels for BOTH throttle and turning, since it
// mixes the two together to get diff-drive power levels for both motors.
}
// The SLOW ramp here is turning, and the FAST ramp is throttle.
// If that's the opposite of what you're seeing, swap M2A and M2B.
void loop()
{
int power;
// Don't turn. Ramp from going backwards to going forwards, waiting 20 ms (1/50th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.drive(power);
delay(20);
}
// Now, let's use a power level of 20 (out of 127) forward.
// This way, our turning will have a radius.
ST.drive(20);
// Ramp turning from full left to full right SLOWLY by waiting 50 ms (1/20th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.turn(power);
delay(50);
}
// Now stop turning, and stop driving.
ST.turn(0);
ST.drive(0);
// Wait a bit. This is so you can catch your robot if you want to. :-)
delay(5000);
}

34
lib/Sabertooth/examples/2.Settings/MinVoltage/MinVoltage.ino Normal file
View File

@ -0,0 +1,34 @@
// Set Minimum Voltage Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
// The values in this sample are specifically for the Sabertooth 2x25, and may
// not have the same effect on other models.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
SabertoothTXPinSerial.begin(9600);
ST.autobaud();
// This setting does not persist between power cycles.
// See the Packet Serial section of the documentation for what values to use
// for the minimum voltage command. It may vary between Sabertooth models
// (2x25, 2x60, etc.).
//
// On a Sabertooth 2x25, the value is (Desired Volts - 6) X 5.
// So, in this sample, we'll make the low battery cutoff 12V: (12 - 6) X 5 = 30.
ST.setMinVoltage(30);
}
void loop()
{
ST.motor(1, 50);
delay(5000);
ST.motor(1, -50);
delay(5000);
}

48
lib/Sabertooth/examples/2.Settings/Persistent/BaudRate/BaudRate.ino Normal file
View File

@ -0,0 +1,48 @@
// Set Baud Rate Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
// WARNING: This sample makes changes that will persist between restarts.
// NOTE: The setBaudRate function will only have an effect on V2 controllers (2x12, 2x25 V2, 2x60, SyRen 50).
// Earlier controllers automatically detect the baud rate you choose in Serial.begin
// when you call the autobaud function. Autobaud was replaced in V2 controllers for reliability
// in the event that the Sabertooth lost power.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
// This sample will tell the Sabertooth *at 9600 baud* to *switch to 2400 baud*.
// Keep in mind you must send the command to change the baud rate *at the baud rate
// the Sabertooth is listening at* (factory default is 9600). After that, if it works,
// you will be able to communicate using the new baud rate.
//
// Options are:
// 2400
// 9600
// 19200
// 38400
// 115200 (only supported by some devices such as 2X60 -- check the device's datasheet)
//
// WARNING: The Sabertooth remembers this command between restarts.
// To change your Sabertooth back to its default, you must *be at the baud rate you've
// set the Sabertooth to*, and then call ST.setBaudRate(9600)
SabertoothTXPinSerial.begin(9600);
ST.setBaudRate(2400);
SabertoothTXPinSerial.end();
// OK, we're at 2400. Let's talk to the Sabertooth at that speed.
SabertoothTXPinSerial.begin(2400);
}
void loop()
{
ST.drive(0);
ST.turn(20);
delay(2000);
ST.turn(-20);
delay(2000);
}

33
lib/Sabertooth/examples/2.Settings/Persistent/Deadband/Deadband.ino Normal file
View File

@ -0,0 +1,33 @@
// Set Deadband Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
// WARNING: This sample makes changes that will persist between restarts AND in all modes.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
SabertoothTXPinSerial.begin(9600);
ST.autobaud();
// This makes the deadband from -20 to 20 (of 127).
// If your commands for a motor stay entirely within the deadband for more than
// a second, the motor driver will stop the motor.
// WARNING: The Sabertooth remembers this command between restarts AND in all modes.
// To change your Sabertooth back to its default, call ST.setDeadband(0)
ST.setDeadband(20);
}
void loop()
{
// 50 is greater than 20, so the motor moves.
ST.motor(1, 50);
delay(5000);
// 10 is NOT, so the motor does not move.
ST.motor(1, 10);
delay(5000);
}

41
lib/Sabertooth/examples/2.Settings/Persistent/MaxVoltage/MaxVoltage.ino Normal file
View File

@ -0,0 +1,41 @@
// Set Maximum Voltage Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
// WARNING: This sample makes changes that will persist between restarts AND in all modes.
// The values in this sample are specifically for the Sabertooth 2x25, and may
// not have the same effect on other models.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
SabertoothTXPinSerial.begin(9600);
ST.autobaud();
// See the Packet Serial section of the documentation for what values to use
// for the maximum voltage command. It may vary between Sabertooth models
// (2x25, 2x60, etc.).
//
// On a Sabertooth 2x25, the value is (Desired Volts) X 5.12.
// In this sample, we'll cap the max voltage before the motor driver does
// a hard brake at 14V. For a 12V ATX power supply this might be reasonable --
// at 16V they tend to shut off. Here, if the voltage climbs above
// 14V due to regenerative braking, the Sabertooth will go into hard brake instead.
// While this is occuring, the red Error LED will turn on.
//
// 14 X 5.12 = 71.68, so we'll go with 71, cutting off slightly below 14V.
//
// WARNING: This setting persists between power cycles.
ST.setMaxVoltage(71);
}
void loop()
{
ST.motor(1, 50);
delay(5000);
ST.motor(1, -50);
delay(5000);
}

39
lib/Sabertooth/examples/2.Settings/Persistent/Ramping/Ramping.ino Normal file
View File

@ -0,0 +1,39 @@
// Set Ramping Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
// WARNING: This sample makes changes that will persist between restarts AND in all modes.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
SabertoothTXPinSerial.begin(9600);
ST.autobaud();
// See the Sabertooth 2x60 documentation for information on ramping values.
// There are three ranges: 1-10 (Fast), 11-20 (Slow), and 21-80 (Intermediate).
// The ramping value 14 used here sets a ramp time of 4 seconds for full
// forward-to-full reverse.
//
// 0 turns off ramping. Turning off ramping requires a power cycle.
//
// WARNING: The Sabertooth remembers this command between restarts AND in all modes.
// To change your Sabertooth back to its default, call ST.setRamping(0)
ST.setRamping(14);
}
void loop()
{
// Full forward, both motors.
ST.motor(1, 127);
ST.motor(2, 127);
delay(5000);
// Full reverse
ST.motor(1, -127);
ST.motor(2, -127);
delay(5000);
}

33
lib/Sabertooth/examples/2.Settings/SerialTimeout/SerialTimeout.ino Normal file
View File

@ -0,0 +1,33 @@
// Set Serial Timeout Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <Sabertooth.h>
Sabertooth ST(128);
void setup()
{
SabertoothTXPinSerial.begin(9600);
ST.autobaud();
// setTimeout rounds up to the nearest 100 milliseconds, so this 950 will actually be 1 second.
// A value of 0 disables the serial timeout.
ST.setTimeout(950);
}
void loop()
{
// Set motor 1 to reverse 20 (out of 127), and sleep for 5 seconds.
// Notice how it cuts out after 1 second -- this is the serial timeout in action.
// Since we configured it in setup() for 1 second, 1 second without any new
// commands will cause the motors to stop.
ST.motor(1, -20);
delay(5000);
// Why do this?
// If the S1 wire gets cut for some reason, or if your program crashes,
// the Sabertooth will stop receiving commands from the Arduino.
// With a timeout, your robot will stop. So, it's a safety feature mostly.
}

43
lib/Sabertooth/examples/3.Advanced/SharedLine/SharedLine.ino Normal file
View File

@ -0,0 +1,43 @@
// Shared Line Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <Sabertooth.h>
// Up to 8 Sabertooth/SyRen motor drivers can share the same S1 line.
// This sample uses three: address 128 and 129 on ST1[0] and ST1[2],
// and address 130 on ST2.
Sabertooth ST1[2] = { Sabertooth(128), Sabertooth(129) };
Sabertooth ST2(130);
void setup()
{
SabertoothTXPinSerial.begin(9600);
Sabertooth::autobaud(SabertoothTXPinSerial); // Autobaud is for the whole serial line -- you don't need to do
// it for each individual motor driver. This is the version of
// the autobaud command that is not tied to a particular
// Sabertooth object.
// See the examples in 1.Basics for information on whether you
// need this line at all.
}
void loop()
{
// ST1[0] (address 128) has power 50 (of 127 max) on M1,
// ST1[1] (address 129) has power 60 (of 127 max) on M2, and
// ST2 (address 130) we'll do tank-style and have it drive 20 and turn right 50.
// Do this for 5 seconds.
ST1[0].motor(1, 50);
ST1[1].motor(2, 60);
ST2.drive(20);
ST2.turn(50);
delay(5000);
// And now let's stop for 5 seconds, except address 130 -- we'll let it stop and turn left...
ST1[0].motor(1, 0);
ST1[1].motor(2, 0);
ST2.drive(0);
ST2.turn(-40);
delay(5000);
}

35
lib/Sabertooth/examples/3.Advanced/SoftwareSerial/SoftwareSerial.ino Normal file
View File

@ -0,0 +1,35 @@
// Software Serial Sample for Packet Serial
// Copyright (c) 2012 Dimension Engineering LLC
// See license.txt for license details.
#include <SoftwareSerial.h>
#include <Sabertooth.h>
SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
Sabertooth ST(128, SWSerial); // Address 128, and use SWSerial as the serial port.
void setup()
{
SWSerial.begin(9600);
ST.autobaud();
}
void loop()
{
int power;
// Ramp from -127 to 127 (full reverse to full forward), waiting 20 ms (1/50th of a second) per value.
for (power = -127; power <= 127; power ++)
{
ST.motor(1, power);
delay(20);
}
// Now go back the way we came.
for (power = 127; power >= -127; power --)
{
ST.motor(1, power);
delay(20);
}
}

22
lib/Sabertooth/keywords.txt Normal file
View File

@ -0,0 +1,22 @@
# Syntax Coloring for the Sabertooth Library
# Classes
Sabertooth KEYWORD1
# Sabertooth methods
address KEYWORD2
port KEYWORD2
autobaud KEYWORD2
# Common methods
command KEYWORD2
motor KEYWORD2
drive KEYWORD2
turn KEYWORD2
setMinVoltage KEYWORD2
setMaxVoltage KEYWORD2
setBaudRate KEYWORD2
setDeadband KEYWORD2
setRamping KEYWORD2
setTimeout KEYWORD2
stop KEYWORD2

7
lib/Sabertooth/license.txt Normal file
View File

@ -0,0 +1,7 @@
Arduino Libraries for SyRen/Sabertooth
Copyright (c) 2012-2013 Dimension Engineering LLC
http://www.dimensionengineering.com/arduino
Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above copyright notice and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

142
lib/Sabertooth/src/Sabertooth.cpp Normal file
View File

@ -0,0 +1,142 @@
/*
Arduino Library for SyRen/Sabertooth Packet Serial
Copyright (c) 2012-2013 Dimension Engineering LLC
http://www.dimensionengineering.com/arduino
Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#include "Sabertooth.h"
Sabertooth::Sabertooth(byte address)
: _address(address), _port(SabertoothTXPinSerial)
{
}
Sabertooth::Sabertooth(byte address, SabertoothStream& port)
: _address(address), _port(port)
{
}
void Sabertooth::autobaud(boolean dontWait) const
{
autobaud(port(), dontWait);
}
void Sabertooth::autobaud(SabertoothStream& port, boolean dontWait)
{
if (!dontWait) { delay(1500); }
port.write(0xAA);
#if defined(ARDUINO) && ARDUINO >= 100
port.flush();
#endif
if (!dontWait) { delay(500); }
}
void Sabertooth::command(byte command, byte value) const
{
port().write(address());
port().write(command);
port().write(value);
port().write((address() + command + value) & 0b01111111);
}
void Sabertooth::throttleCommand(byte command, int power) const
{
power = constrain(power, -126, 126);
this->command(command, (byte)abs(power));
}
void Sabertooth::motor(int power) const
{
motor(1, power);
}
void Sabertooth::motor(byte motor, int power) const
{
if (motor < 1 || motor > 2) { return; }
throttleCommand((motor == 2 ? 4 : 0) + (power < 0 ? 1 : 0), power);
}
void Sabertooth::drive(int power) const
{
throttleCommand(power < 0 ? 9 : 8, power);
}
void Sabertooth::turn(int power) const
{
throttleCommand(power < 0 ? 11 : 10, power);
}
void Sabertooth::stop() const
{
motor(1, 0);
motor(2, 0);
}
void Sabertooth::setMinVoltage(byte value) const
{
command(2, (byte)min(value, 120));
}
void Sabertooth::setMaxVoltage(byte value) const
{
command(3, (byte)min(value, 127));
}
void Sabertooth::setBaudRate(long baudRate) const
{
#if defined(ARDUINO) && ARDUINO >= 100
port().flush();
#endif
byte value;
switch (baudRate)
{
case 2400: value = 1; break;
case 9600: default: value = 2; break;
case 19200: value = 3; break;
case 38400: value = 4; break;
case 115200: value = 5; break;
}
command(15, value);
#if defined(ARDUINO) && ARDUINO >= 100
port().flush();
#endif
// (1) flush() does not seem to wait until transmission is complete.
// As a result, a Serial.end() directly after this appears to
// not always transmit completely. So, we manually add a delay.
// (2) Sabertooth takes about 200 ms after setting the baud rate to
// respond to commands again (it restarts).
// So, this 500 ms delay should deal with this.
delay(500);
}
void Sabertooth::setDeadband(byte value) const
{
command(17, (byte)min(value, 127));
}
void Sabertooth::setRamping(byte value) const
{
command(16, (byte)constrain(value, 0, 80));
}
void Sabertooth::setTimeout(int milliseconds) const
{
command(14, (byte)((constrain(milliseconds, 0, 12700) + 99) / 100));
}

179
lib/Sabertooth/src/Sabertooth.h Normal file
View File

@ -0,0 +1,179 @@
/*
Arduino Library for SyRen/Sabertooth Packet Serial
Copyright (c) 2012-2013 Dimension Engineering LLC
http://www.dimensionengineering.com/arduino
Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#ifndef Sabertooth_h
#define Sabertooth_h
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
typedef Stream SabertoothStream;
#else
#include <WProgram.h>
typedef Print SabertoothStream;
#endif
#if defined(USBCON)
#define SabertoothTXPinSerial Serial1 // Arduino Leonardo has TX->1 on Serial1, not Serial.
#else
#define SabertoothTXPinSerial Serial
#endif
#define SyRenTXPinSerial SabertoothTXPinSerial
/*!
\class Sabertooth
\brief Controls a %Sabertooth or %SyRen motor driver running in Packet Serial mode.
*/
class Sabertooth
{
public:
/*!
Initializes a new instance of the Sabertooth class.
The driver address is set to the value given, and the Arduino TX serial port is used.
\param address The driver address.
*/
Sabertooth(byte address);
/*!
Initializes a new instance of the Sabertooth class.
The driver address is set to the value given, and the specified serial port is used.
\param address The driver address.
\param port The port to use.
*/
Sabertooth(byte address, SabertoothStream& port);
public:
/*!
Gets the driver address.
\return The driver address.
*/
inline byte address() const { return _address; }
/*!
Gets the serial port.
\return The serial port.
*/
inline SabertoothStream& port() const { return _port; }
/*!
Sends the autobaud character.
\param dontWait If false, a delay is added to give the driver time to start up.
*/
void autobaud(boolean dontWait = false) const;
/*!
Sends the autobaud character.
\param port The port to use.
\param dontWait If false, a delay is added to give the driver time to start up.
*/
static void autobaud(SabertoothStream& port, boolean dontWait = false);
/*!
Sends a packet serial command to the motor driver.
\param command The number of the command.
\param value The command's value.
*/
void command(byte command, byte value) const;
public:
/*!
Sets the power of motor 1.
\param power The power, between -127 and 127.
*/
void motor(int power) const;
/*!
Sets the power of the specified motor.
\param motor The motor number, 1 or 2.
\param power The power, between -127 and 127.
*/
void motor(byte motor, int power) const;
/*!
Sets the driving power.
\param power The power, between -127 and 127.
*/
void drive(int power) const;
/*!
Sets the turning power.
\param power The power, between -127 and 127.
*/
void turn(int power) const;
/*!
Stops.
*/
void stop() const;
public:
/*!
Sets the minimum voltage.
\param value The voltage. The units of this value are driver-specific and are specified in the Packet Serial chapter of the driver's user manual.
*/
void setMinVoltage(byte value) const;
/*!
Sets the maximum voltage.
Maximum voltage is stored in EEPROM, so changes persist between power cycles.
\param value The voltage. The units of this value are driver-specific and are specified in the Packet Serial chapter of the driver's user manual.
*/
void setMaxVoltage(byte value) const;
/*!
Sets the baud rate.
Baud rate is stored in EEPROM, so changes persist between power cycles.
\param baudRate The baud rate. This can be 2400, 9600, 19200, 38400, or on some drivers 115200.
*/
void setBaudRate(long baudRate) const;
/*!
Sets the deadband.
Deadband is stored in EEPROM, so changes persist between power cycles.
\param value The deadband value.
Motor powers in the range [-deadband, deadband] will be considered in the deadband, and will
not prevent the driver from entering nor cause the driver to leave an idle brake state.
0 resets to the default, which is 3.
*/
void setDeadband(byte value) const;
/*!
Sets the ramping.
Ramping is stored in EEPROM, so changes persist between power cycles.
\param value The ramping value. Consult the user manual for possible values.
*/
void setRamping(byte value) const;
/*!
Sets the serial timeout.
\param milliseconds The maximum time in milliseconds between packets. If this time is exceeded,
the driver will stop the motors. This value is rounded up to the nearest 100 milliseconds.
This library assumes the command value is in units of 100 milliseconds. This is true for
most drivers, but not all. Check the packet serial chapter of the driver's user manual
to make sure.
*/
void setTimeout(int milliseconds) const;
private:
void throttleCommand(byte command, int power) const;
private:
const byte _address;
SabertoothStream& _port;
};
#endif

BIN
lib/Sabertooth/src/SabertoothArduinoLibrary.chm Normal file
View File

Binary file not shown.

5
platformio.ini
View File

@ -20,4 +20,9 @@ lib_deps =
xreef/PCF8574 library@^2.3.6
gbr1/rp2040-encoder-library@^0.1.1
trevorwslee/DumbDisplay Arduino Library@^0.9.841
ftjuh/I2Cwrapper@^0.5.0
waspinator/AccelStepper@^1.64
olikraus/Ucglib@^1.5.2
khoih-prog/RP2040_ISR_Servo@^1.1.2
debug_tool = cmsis-dap
build_flags = -O3

346
src/main.cpp
View File

@ -1,22 +1,25 @@
#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include <AccelStepperI2C.h>
#include "globals.h"
#include <SPI.h>
#include "MCP3XXX.h"
#include "PCF8574.h"
#include "RP2040_ISR_Servo.h"
#include "pio_encoder.h"
#include "dumbdisplay.h"
#include "wifidumbdisplay.h"
//#include "dumbdisplay.h"
//#include "wifidumbdisplay.h"
#include "zserio.h"
#include "SerialUART.h"
#include "Sabertooth.h"
const char* ssid = "TEST";
const char* password = "pink4bubble";
DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
LcdDDLayer *optionsdisplay = NULL;
SevenSegmentRowDDLayer *sevenSeg;
JoystickDDLayer *appjoystick;
//DumbDisplay dumbdisplay(new DDWiFiServerIO(ssid, password), 8192);
//LcdDDLayer *optionsdisplay = NULL;
//SevenSegmentRowDDLayer *sevenSeg;
//JoystickDDLayer *appjoystick;
packet_t pA, pB, safe;
packet_t *astate, *incoming;
@ -27,9 +30,57 @@ byte d[] = { 0x7e, 0x30, 0x6d, 0x79, 0x33, 0x5b, 0x5f, 0x70, 0x7f, 0x7b, 0x77, 0
PCF8574 ioex1(0x20, 20, 21);
PCF8574 ioex2(0x21, 20, 21);
PioEncoder enc1(18); // right
PioEncoder enc2(14); // left
// stepper board slave config
#define MOTOR_X_ENABLE_PIN 8
#define MOTOR_X_STEP_PIN 2
#define MOTOR_X_DIR_PIN 5
#define MOTOR_Y_ENABLE_PIN 8
#define MOTOR_Y_STEP_PIN 3
#define MOTOR_Y_DIR_PIN 6
#define MOTOR_Z_ENABLE_PIN 8
#define MOTOR_Z_STEP_PIN 4
#define MOTOR_Z_DIR_PIN 7
uint8_t i2cAddress = 0x08; // arduino address
I2Cwrapper wrapper(i2cAddress);
AccelStepperI2C stepperX(&wrapper); // Stepper Motor 1
AccelStepperI2C stepperY(&wrapper); // Stepper Motor 2
AccelStepperI2C stepperZ(&wrapper); // Stepper Motor 3
bool stepperXdir = true; // true=CW, false=CCW
bool stepperYdir = true;
bool stepperZdir = true;
#define defMaxSpeed 1000000
#define defAcceleration 1000000
#define stepsToGo 1000 // 200 steps/rev w/o microstepping
Sabertooth swivel[2] = { Sabertooth(128), Sabertooth(129) };
Sabertooth actuators(130);
#define MIN_MICROS 800
#define MAX_MICROS 2450
#define SERVO_PIN_1 5
#define SERVO_PIN_2 6
#define SERVO_PIN_3 7
#define SERVO_PIN_4 9
typedef struct
{
int servoIndex;
uint8_t servoPin;
} ISR_servo_t;
#define NUM_SERVOS 4
ISR_servo_t ISR_servo[NUM_SERVOS] =
{
{ -1, SERVO_PIN_1 }, { -1, SERVO_PIN_2 }, { -1, SERVO_PIN_3 }, { -1, SERVO_PIN_4 }
};
MCP3008 adc;
int count = 0;
int mode = 1;
@ -46,7 +97,21 @@ int acceleration = 1;
bool turbo = false;
int left_cooldown = 0;
int right_cooldown = 0;
int olddisplay = 99999;
int olddisplay = 99999; // guarantee a change when first value comes in
#define FLSTEER 1
#define FRSTEER 2
#define BLSTEER 3
#define BRSTEER 4
#define FLDRIVE 5
#define FRDRIVE 6
#define BLDRIVE 7
#define BRDRIVE 8
#define EXTEND1 9
#define EXTEND2 10
#define LIFT1 11
#define LIFT2 12
#define LIFT3 13
unsigned int getButton(unsigned int num) {
if (num <= 7) {
@ -63,7 +128,7 @@ unsigned int getDPad() {
}
void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
/*void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
if (pLayer == optionsdisplay) {
// clicked the "clear" button
if(turbo) {
@ -76,7 +141,7 @@ void FeedbackHandler(DDLayer* pLayer, DDFeedbackType type, const DDFeedback&) {
}
//delay(100);
}
}
}*/
void osmc_init() {
digitalWrite(ALI1, LOW);
@ -238,29 +303,33 @@ void set_mosfet(bool pin, bool value) {
void set_digit(byte digit, byte value)
{
Wire.beginTransmission(0x38);
/*Wire.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(d[value]);
Wire.endTransmission();
Wire.endTransmission();*/
//Serial.print("Set digit ");
//Serial.print(digit);
//Serial.print(" to ");
//Serial.println(value);
//delay(5000);
return;
}
void set_raw(byte digit, byte value) {
Wire.beginTransmission(0x38);
/*Wire.beginTransmission(0x38);
Wire.write(0x20 + digit);
Wire.write(value);
Wire.endTransmission();
Wire.endTransmission();*/
return;
}
void set_blank() {
Wire.beginTransmission(0x38);
/*Wire.beginTransmission(0x38);
Wire.write(0x20);
Wire.write((byte)0);
Wire.write((byte)0);
Wire.endTransmission();
Wire.endTransmission();*/
return;
}
void set_hex(byte num) {
byte digit1 = num;
@ -274,9 +343,8 @@ void set_hex(byte num) {
}
set_digit(0, digit1);
set_digit(1, digit2);
if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
if(num != olddisplay && millis() % 10 < 1) {
olddisplay = num;
sevenSeg->showHexNumber(num);
}
set_raw(4, 0x00); // clear second dot
}
@ -292,13 +360,54 @@ void set_dec(byte num) {
}
set_digit(0, digit1);
set_digit(1, digit2);
if(num != olddisplay && dumbdisplay.connected() && millis() % 10 < 1) {
if(num != olddisplay && millis() % 10 < 1) {
olddisplay = num;
sevenSeg->showNumber(num);
}
set_raw(4, 0x02); // set second dot
}
void set_motor(byte motor, int speed) {
// 1 - 4 : swivel motors on Sabertooth 1-2
// 5 - 8 : drive motors on Talon SRX
// 9 - 10 : actuators on Sabertooth 3
// 11 - 13 : Steppers on slave board
// speed is -127 to 127
Serial.print("Driving motor ");
Serial.print(motor);
Serial.print(" with speed ");
Serial.println(speed);
if (motor <= 4) {
// swerve controls
swivel[(motor - 1) / 2].motor((motor - 1) % 2, speed);
}
else if (motor <= 8) {
// Talon SRX drive
// Note: can't use the built in arduino-pico Servo.h because it uses the PIO - which is already full
// Can't use PWM because that would slow the PWM frequency to 50hz, kinda need more than that
// So we use hardware interrupt timers instead
//RP2040_ISR_Servos.setPosition(ISR_servo[motor - 5].servoIndex, position);
uint16_t newspeed = map(speed, -127, 127, MIN_MICROS, MAX_MICROS);
RP2040_ISR_Servos.setPulseWidth(ISR_servo[motor - 5].servoIndex, newspeed);
}
else if (motor <= 10) {
// actuator controls
actuators.motor((motor - 9) % 2, speed);
}
// set servos
// speed needs to be a high number to be reasonable
else if (motor == 11) {
stepperX.setSpeed((float)speed);
stepperX.runSpeedState();
}
else if (motor == 12) {
stepperY.setSpeed((float)speed);
stepperY.runSpeedState();
}
else if (motor == 13) {
stepperY.setSpeed((float)speed);
stepperY.runSpeedState();
}
}
void setup() {
WiFi.noLowPowerMode();
@ -355,17 +464,113 @@ void setup() {
Serial.println(" done");
delay(20);
Serial.print("Initializing OSMCs..");
set_hex(0x2);
osmc_init();
//Serial.print("Initializing OSMCs..");
//set_hex(0x2);
//osmc_init();
//Serial.println(" done");
//delay(20);
////delay(2000);
Serial.print("Initializing motor controllers..");
digitalWrite(ALI1, LOW);
digitalWrite(BLI1, LOW);
digitalWrite(AHI1, LOW);
digitalWrite(BHI1, LOW);
digitalWrite(ALI2, LOW);
digitalWrite(BLI2, LOW);
digitalWrite(AHI2, LOW);
digitalWrite(BHI2, LOW);
pinMode(ALI1, OUTPUT);
pinMode(AHI1, OUTPUT);
pinMode(BLI1, OUTPUT);
pinMode(BHI1, OUTPUT);
pinMode(ALI2, OUTPUT);
pinMode(AHI2, OUTPUT);
pinMode(BLI2, OUTPUT);
pinMode(BHI2, OUTPUT);
digitalWrite(22, LOW); // finally, enable output buffers
pinMode(22, OUTPUT);
// enable stepper slave board
if (!wrapper.ping()) {
Serial.println("Arduino stepper driver not found!");
while (true) {}
}
wrapper.reset(); // reset the target device
wrapper.setI2Cdelay(20); // Set small delay for I2C bus communication
// attach() replaces the AccelStepper constructor, so it could also be called like this:
// stepper.attach(AccelStepper::DRIVER, 5, 6);
// stepper.attach(); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5
stepperX.attach(AccelStepper::DRIVER, MOTOR_X_STEP_PIN, MOTOR_X_DIR_PIN); // Stepper Motor 1 - X
stepperY.attach(AccelStepper::DRIVER, MOTOR_Y_STEP_PIN, MOTOR_Y_DIR_PIN); // Stepper Motor 2 - Y
stepperZ.attach(AccelStepper::DRIVER, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN); // Stepper Motor 3 - Z
Serial.print("stepperX.myNum=");
Serial.println(stepperX.myNum);
Serial.print("stepperY.myNum=");
Serial.println(stepperY.myNum);
Serial.print("stepperZ.myNum=");
Serial.println(stepperZ.myNum);
if (stepperX.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperX could not be allocated");
while (true) {}
}
if (stepperY.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperY could not be allocated");
while (true) {}
}
if (stepperZ.myNum < 0) { // Should not happen after a reset
Serial.println("Error: stepperZ could not be allocated");
while (true) {}
}
stepperX.setMaxSpeed(defMaxSpeed);
stepperX.setAcceleration(defAcceleration);
stepperY.setMaxSpeed(defMaxSpeed);
stepperY.setAcceleration(defAcceleration);
stepperZ.setMaxSpeed(defMaxSpeed);
stepperZ.setAcceleration(defAcceleration);
// Sabertooth init
Serial2.setTX(4); // pin 5 of OSMC port 2
Serial2.begin(9600);
Sabertooth::autobaud(Serial2);
// Talon SRX init
for (int index = 0; index < NUM_SERVOS; index++)
{
pinMode(ISR_servo[index].servoPin, OUTPUT);
digitalWrite(ISR_servo[index].servoPin, LOW);
}
for (int index = 0; index < NUM_SERVOS; index++)
{
ISR_servo[index].servoIndex = RP2040_ISR_Servos.setupServo(ISR_servo[index].servoPin, MIN_MICROS, MAX_MICROS);
if (ISR_servo[index].servoIndex != -1)
{
//Serial.print(F("Setup OK Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
RP2040_ISR_Servos.setPosition(ISR_servo[index].servoIndex, 0);
}
else
{
Serial.print(F("Setup Failed Servo index = ")); Serial.println(ISR_servo[index].servoIndex);
}
}
Serial.println(" done");
delay(20);
//delay(2000);
bool ioex1p, ioex2p = false;
Serial.print("Initializing I/O expanders..");
set_hex(0x3);
if(ioex1.begin()) {
delay(200);
Serial.print(" 1");
@ -374,6 +579,7 @@ void setup() {
delay(20);
}
set_hex(0x4);
if(ioex2.begin()) {
delay(20);
Serial.print(" 2");
@ -381,14 +587,13 @@ void setup() {
} else {
delay(20);
}
Serial.println(" done");
delay(20);
Serial.print("Initializing encoders..");
set_hex(0x5);
enc1.begin();
enc2.begin();
//enc1.begin();
//enc2.begin();
Serial.println(" done");
delay(20);
@ -450,28 +655,28 @@ void setup() {
set_offset();
Serial.println("done");
Serial.print("Checking OSMC 1..");
set_hex(0x9);
if (get_voltage(0) > 13) {
Serial.println(" done");
delay(20);
pass++;
} else {
Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
set_hex(0xF9);
delay(500);
}
set_hex(0xA);
Serial.print("Checking OSMC 2..");
if (get_voltage(1) > 13) {
Serial.println(" done");
delay(20);
pass++;
} else {
Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
set_hex(0xFA);
delay(500);
}
//Serial.print("Checking OSMC 1..");
//set_hex(0x9);
//if (get_voltage(0) > 13) {
// Serial.println(" done");
// delay(20);
// pass++;
//} else {
// Serial.println(" WARNING: OSMC 1 battery too low or OSMC not present");
// set_hex(0xF9);
// delay(500);
//}
//set_hex(0xA);
//Serial.print("Checking OSMC 2..");
//if (get_voltage(1) > 13) {
// Serial.println(" done");
// delay(20);
// pass++;
//} else {
// Serial.println(" WARNING: OSMC 2 battery too low or OSMC not present");
// set_hex(0xFA);
// delay(500);
//}
set_hex(0xB);
Serial.print("Checking I/O expander 1..");
@ -504,7 +709,7 @@ void setup() {
Serial.print("Self tests complete: ");
Serial.print(pass);
Serial.println("/6 tests passed.");
Serial.println("/4 tests passed.");
Serial.println("RIB is ready to go. Starting program.");
set_blank();
/*dumbdisplay.recordLayerSetupCommands();
@ -644,8 +849,8 @@ void loop() {
//SerComm.println();
set_hex(avg_speed);
drive_right(right_enabled, right_power);
drive_left(left_enabled, -left_power);
//drive_right(right_enabled, right_power);
//drive_left(left_enabled, -left_power);
SerComm.println(" L: " + String(left_power) + " LT: " + String(target_left_power) + " R: " + String(right_power) + " RT: " + String(target_right_power) + " MEM FREE: "+ String(rp2040.getFreeHeap()));
//if(left_power != target_left_power || right_power != target_right_power)
@ -730,9 +935,42 @@ void loop1() {
delay(25);
loopcount++;
}
//SerComm.println("update");
left_enabled = try_enable_left(left_enabled, get_voltage(1));
right_enabled = try_enable_right(right_enabled, get_voltage(0));
//left_enabled = try_enable_left(left_enabled, get_voltage(1));
//right_enabled = try_enable_right(right_enabled, get_voltage(0));
//digitalWrite(LED_BUILTIN, LOW);
/*if (stepperX.distanceToGo() == 0) { // Give stepper something to do...
if (stepperXdir) {
Serial.println("Driving stepper");
stepperX.moveTo(stepsToGo);
} else {
Serial.println("Driving stepper");
stepperX.moveTo(-stepsToGo);
}
stepperX.runState();
stepperXdir = !stepperXdir;
}
if (stepperY.distanceToGo() == 0) { // Give stepper something to do...
if (stepperYdir)
stepperY.moveTo(stepsToGo);
else
stepperY.moveTo(-stepsToGo);
stepperY.runState();
stepperYdir = !stepperYdir;
}
if (stepperZ.distanceToGo() == 0) { // Give stepper something to do...
if (stepperZdir)
stepperZ.moveTo(stepsToGo);
else
stepperZ.moveTo(-stepsToGo);
stepperZ.runState();
stepperZdir = !stepperZdir;
}*/
for (int x = 1; x < 14; x++) {
set_motor(x, loopcount * 5);
}
delay(25);
}