Hi, I am having an issue with my project which is probably related to the NeoSWSerial library. I will explain the setting, what I know is working and how the robot is reacting. I’d be glad if anyone could help me settle this, it’s driving me mad !
So my project is about controlling a 4-wheeled robot from either of two sources: on one side a computer for autonomous navigation purposes (using ROS), and on the other a RC controller, with this controller always having the upper hand for security matters. Here I will be focusing on the RC part where I am having troubles.
So to make this work, I chose to use an Arduino MEGA, who will read the RC signal from its pins. Then it will send the information to 3 Sabertooth 2x25 motor drivers through a single Serial connection, and (yet to be implemented) 2 servo controllers with PWM generation using the Servo library. The ST motor drivers are on Packetized Serial Mode, meaning commands are sent to each motor individually using the ST’s respective addresses (manually set with switches). Each wheel has a big DC motor, and they are controlled by pair by two of the Sabertooths, the last one controlling the brakes.
Anyways, all of this lead me to use the NeoSWSerial library instead of the SoftwareSerial/AltSoftSerial ones, the first being too restrictive and the other conflicting with the Servo library (or maybe is it not the case on the MEGA? I know I had issues with the UNO before, but I had to upgrade it due to a lack of memory).
So far, the RC control (and control from the computer) for the motors have been perfectly working with the UNO. I also confirmed the reading part to be working with the MEGA. For reference, I am using the method described on rcarduino.blogspot.com : interrupts will trigger on every change of the signal’s level received by the pins in order to record the length of the pulse, and thus get the RC value.
In the tests, there are two input channels for controlling the motor drivers (on pin 2 and 3), and I arbitrarily set the output Serial connection on pin 29 (I am not using RX on 28). Regarding the robot behaviour, it does not seem to react at first, but then the front left wheel suddenly starts going at full forward speed maybe 20 to 30s after commanding the robot to go forward. I think it has to do with the library, but I’m not quite sure. If any of you have any idea, or even a solution I’d be very thankful ^^.
Here is the code. I broke it in logical parts, left in order and explained a little bit:
// Includes
#include <EnableInterrupt.h>
#include <Sabertooth.h>
#include <NeoSWSerial.h>
Here are some constants I use. They have been measured specifically for my project and used for translating RC values (1000 to 2000) into values understandable by the Sabertooth (-127 to 127). Note that a negative value means in my case that the motor will go ... forward.
// Constants
// Define input pins
#define ST_ONE 3
#define ST_TWO 2
// Flags indicating a new signal. The number refers to a bit
#define ST_ONE_FLAG 1
#define ST_TWO_FLAG 4
// Neutral value measured for our signals
#define ST_ONE_NEUTRAL 1514
#define ST_TWO_NEUTRAL 1490
// Maximum values we accept as output depending on the robot's speed mode
#define ST_MAX 50
#define ST_MDM 25
#define ST_MIN 10
// Number of possible RC input values for each mode
#define WIDTH_MAX 420
#define WIDTH_MDM 255
#define WIDTH_MIN 115
// Global variables
// Update flags container
volatile uint8_t bUpdateFlagsShared;
// Global variables that will contain the input data
volatile int16_t globalSTOne;
volatile int16_t globalSTTwo;
// Temporary data used for measuring the signal
int32_t STOneStart;
int32_t STTwoStart;
Addresses 128 to 130 refer to the DIP switch configuration of the Sabertooth
// Communications
// RX 28, TX on pin 29
NeoSWSerial SWSerial(28, 29);
// Addresses for the 3 ST, and use SWSerial as the serial port.
Sabertooth ST1(128, SWSerial);
Sabertooth ST2(129, SWSerial);
Sabertooth ST3(130, SWSerial);
void setup()
{
// Arduino <-> PC connection
Serial.begin(9600);
// Sabertooth 1, 2 and 3 <-> Arduino connections
SWSerial.begin(9600);
ST1.autobaud();
ST2.autobaud();
ST3.autobaud();
Serial.println("STConnectionsOk");
// enable interrupts for pins 2 and 3
enableInterrupt(ST_ONE, isrSTOne, CHANGE);
enableInterrupt(ST_TWO, isrSTTwo, CHANGE);
}
For the loop, the first part is about recovering data from the interrupts. Then I use this data and translate it to values that the Sabertooth can read and send them.
I also choose for now to not use the motor's brakes. This is why I send 0 to the ST1.
void loop()
{
// Local variables to avoid errors due to our global variables being overwritten or other bad things likely to happen to global variables.
static int16_t localSTOne;
static int16_t localSTTwo;
// Local copy of our update flags
static uint8_t bUpdateFlags;
// Check shared update flags to see if any channels received any new signal
if(bUpdateFlagsShared)
{
// turn interrupts off quickly while we take local copies of the shared variables
noInterrupts();
// take a local copy of which channels were updated in case we need to use this in the rest of loop
bUpdateFlags = bUpdateFlagsShared;
// Recover the gloabl values locally
if(bUpdateFlags & ST_ONE_FLAG)
{
localSTOne = globalSTOne;
}
if(bUpdateFlags & ST_TWO_FLAG)
{
localSTTwo = globalSTTwo;
}
// clear shared copy of updated flags as we have already taken the updates
bUpdateFlagsShared = 0;
interrupts();
}
// Calculate values to send to the Sabertooth
// Accepts negative values
signed int STTwo = 0;
// Stopped
if (abs(localSTTwo - ST_TWO_NEUTRAL) < 15){
STTwo = 0;
}
// Low Speed
if (abs(localSTTwo - ST_TWO_NEUTRAL) < WIDTH_MIN){
STTwo = (localSTTwo - ST_TWO_NEUTRAL) * ST_MIN / WIDTH_MIN;
}
// Medium Speed
else if (abs(localSTTwo - ST_TWO_NEUTRAL) < WIDTH_MDM){
STTwo = (localSTTwo - ST_TWO_NEUTRAL) * ST_MDM / WIDTH_MDM;
}
// High Speed
else if (abs(localSTTwo - ST_TWO_NEUTRAL) < WIDTH_MAX){
STTwo = (localSTTwo - ST_TWO_NEUTRAL) * ST_MAX / WIDTH_MAX;
}
// Over high speed = problem: stop in case anything bad happens
else {
STTwo = 0;
}
// Send values to the Sabertooth
ST1.motor(1, 0);
ST2.motor(1, STTwo);
ST2.motor(2, STTwo);
ST3.motor(1, STTwo);
ST3.motor(2, STTwo);
// Reset the flag
bUpdateFlags = 0;
}
// simple interrupt service routines
void isrSTOne()
{
if(digitalRead(ST_ONE) == HIGH)
{
// Rising edge
STOneStart = micros();
}
else
{
// Falling edge
globalSTOne = (int16_t)(micros() - STOneStart);
// Indicate that a signal for the Sabertooth 1 has been received
bUpdateFlagsShared |= ST_ONE_FLAG;
}
}
void isrSTTwo()
{
if(digitalRead(ST_TWO) == HIGH)
{
// Rising edge
STTwoStart = micros();
}
else
{
// Falling edge
globalSTTwo = (int16_t)(micros() - STTwoStart);
// Indicate that a signal for the Sabertooth 2 and 3 has been received
bUpdateFlagsShared |= ST_TWO_FLAG;
}
}
You can find the code as an attachment if needed
Thank you for your time !
Ric
mega_RC_control.ino (4.04 KB)
