Writing a Bluetooth Packet based #Arduino sketch for the Versalino Control board

Okay so this one took a while to develop, I was up until after about 1 AM last night trying to figure it out. That was even after Bobby and I planned out as much as we could think of on the Robot Arena which happily, being made of whiteboards, makes for an amazing engineering table on top of it’s intended purpose of robot battle.

I released the Versalino Control board a few weeks ago, but until now there was no sketch to us it to control the Versalino Rove. In fact, all of the VirtuabotixPacket articles I have written in the last couple of days were to help support the creation of this control system in the first place.

Now that we have the Versalino Rove set up and ready to receive packet data we have to figure out a way to convert the raw analog data from the Joystick on the Versalino Control into usable control signals on the Versalino Rove.

The first step is of course to get the data off the port, and format it so it fits that data output we expect to see on the rover which is a number between -255 (full reverse) and 255 (full forward). This we can easily accomplish by using the Arduino Map function as follows:

xRead = analogRead(xPin);
yRead = analogRead(yPin);
xRead = (int)map(xRead,0,1024,-255,255); //x is not inverted on the right
yRead = (int)map(yRead,0,1024,255,-255); //y is inverted if the joystick is on the right

Basically what we did above was read our analog data, and remap it from 0 through 1024 to -255 through 255. Okay so far so good, but now that we know how our ranges we have to translate that data into usable right and left motor speeds. 

The first step we took was to figure out what our output would look like on our extremes by mapping out the Y+ and Y- against the X+ and X- as shown above. Basically what the above mapping shows is that we want to do a super fast turn when y = 0, and we don’t want to turn at all (just backward and forward) when x = 0.

That sounds all fine and dandy when you first say it, but in practice is very difficult to actually get your joystick into a true 0 position. For this reason we will be using a technique called dead zoning which allows us to designate a larger region of our positioning as equivalent to zero.

The concept of a dead zone may already be familiar to you from other gaming applications, but in this application I have included a deadzoneX and a deadzoneY to allow even more flexibility on the feel and configuration of the controls.

Okay so now we have a basic idea of what we need to do on the extremes, but what will we do in the other regions of control where neither axis is in a dead zone? The answer for the most part is illustrated in the diagram that Bobby and I put together the other night. It is worth mentioning that many tweaks were required from the original set of equations once I actually had the Versalino Rove under it’s control, but we will go over the details for those in a bit.

Basically the idea above is that we are going to split our controls into quadrants, and have separate equations that run our controls when at least one stick is in the dead zone. When you try to decipher the diagrams above be sure that you remember when making a right turn your left wheel is the one that moves faster thus turning you about the right wheel, and vice versa.

#include

//for Bus B of the versalino for the Versalino Control board
//Change if you switch buses.
VersalinoBUS myBus = BUSB;

//xPin is for A2 Pin, and yPin is for the A3 Pin on the versalino, x and y axis respectively
//These are the readounts for the joystick
byte xPin = myBus.AN2;
byte yPin = myBus.AN1;

//for center area of joystick, so that it will stop the control if not moved
byte deadzoneX = 90;//dead zone on both + and - sides of X
byte deadzoneY = 40;//dead zone on both + and - sides of Y
double sensativity = 0.30;//this changes the turn sensativity for both etreme and normal turning

//initialize the read of the joystick to 0
int xRead = 0;
int yRead = 0;
int xLast = 0;//this is used later to keep packet traffic down
int yLast = 0;//same as above

Okay now that we have that all cleared up, let’s talk about what we need to put into the header of our sketch. The first part should be obvious if you have ever programmed on the Versalino, but for those of you who haven’t the Versalino library makes it easier to use the BUS system for developing your Arduino code in a way that is bus independent. This is also the reason for the myBus variable being declared, I set up my big sketches this way so I can use a reference to the attached bus later, and only have to change it in one place if I switch buses down the line.

Once we have that much set up we declare our pins for X and Y axis, and set up our dead zone variables and sensitivity so we can tweak them easily later. The numbers I have set were what felt comfortable to me after a few hours of tweaking, but I recommend playing with them if you don’t like the way it feels.

Increasing the dead zone on the Y makes it tougher to do slow turns, but gives you faster access to the ability to turn on a dime. Increasing the dead zone on the X axis makes it a lot easier to get your bot to go straight forward, and backward, but it limits your ability to make controlled turns. Tweaking them to a nice balance is obviously ideal, but I have found that most people disagree with where that balance lies.

void setup()
{
Serial.begin(9600);
pinMode(myBus.D1, INPUT);
digitalWrite(myBus.D1, HIGH);
}

Okay so now we have our basic variables set up, and we got our serial port up and running to send Serial Packets over our BT2S slave. This is where I have to admit that I made the Versalino Control boards silk screen a little confusing, but what is labeled D2 is actually myBus.D1 (I believe my logic was that on bus A that is pin 2 on the Arduino, but don’t quote me on it). Anyways we set D1 to an input on our bus, and the digitalWrite HIGH makes the internal pull-up resistor turn on.

void loop()
{
if(!digitalRead(myBus.D1))
{
Serial.print("||AT000");
delay(250);
}


Okay so we start our loop, and the first thing we do is check to see if the Virtuabotix Joystick button is pressed. If it is then we send the packet to toggle our rovers mode and wait 250 milliseconds for debounce before proceeding.

xRead = analogRead(xPin);
yRead = analogRead(yPin);
xRead = (int)map(xRead,0,1024,-255,255); //x is not inverted on the right
yRead = (int)map(yRead,0,1024,255,-255); //y is inverted if the joystick is on the right

if(xRead <= deadzoneX && xRead >= -deadzoneX)
xRead = 0;

if(yRead <= deadzoneY && yRead >= -deadzoneY)
yRead = 0;

if( xRead != xLast || yRead != yLast) {
xLast = xRead;
yLast = yRead;
int rSpeed =0;
int lSpeed =0;

if(yRead > 0 && xRead > 0)
{
lSpeed = yRead;
rSpeed = xRead*sensativity;
}

if(yRead > 0 && xRead < 0) { lSpeed = -xRead*sensativity; rSpeed = yRead; }if(yRead < 0 && xRead < 0) { lSpeed = -xRead*sensativity; rSpeed = yRead; }if(yRead < 0 && xRead > 0)
{
lSpeed = yRead;
rSpeed = xRead*sensativity;
}

if(yRead != 0 && xRead == 0)
{
lSpeed = yRead;
rSpeed = yRead;
}

if(yRead == 0 && xRead != 0)
{
if(xRead < 0) { lSpeed = xRead * sensativity*1.5; rSpeed = -xRead; } if(xRead > 0)
{
lSpeed = xRead;
rSpeed = -xRead * sensativity*1.5;
}
}

So there is a lot of stuff going on above, but the key take away is as follows: We are reading in our output and scaling it (just like we did at the beginning of this article). We then map our dead zones and check to see if either value has changed.

Once we confirm that values have changed we map our right and left outputs according to the equations we figured out on the white board (s is our sensitivity variable) for both extremes and our quadrants.

char tbs[6];//formatting vector
delay(50);//this helps keep us from bombarding with too many packets
sprintf(tbs, "||R%04d",(int)rSpeed );//formatting the output
Serial.print(tbs);//send command
delay(50);//space our messages out a bit
sprintf(tbs, "||L%04d",(int)lSpeed );
Serial.print(tbs);//send command
}
}

Finally we have our data prepped enough that we can format them into tidy little VirtuabotixPacket strings and send them over the BT2S Bluetooth to Serial Slave to our Versalino Rove where it will be happily processed.