Add sabertooth lib, start enabling all motor drive
This commit is contained in:
53
lib/Sabertooth/examples/1.Basics/Sweep/Sweep.ino
Normal file
53
lib/Sabertooth/examples/1.Basics/Sweep/Sweep.ino
Normal 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.
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user