NavBot: Version 1

Screen Shot 2014-03-16 at 4.22.07 PM

Finally have a first working version of the NavBot for differential robots using wheel encoders.

The project is at in the NAvBot_v1 subfolder and is setup for the Arduino IDE (V1.05) but you could incorporate it into any C++ based environment (that supports floating point math) as most of the code is device independent and all hardware/platform specific logic has been abstracted out.

I show in this post how to get NavBot up and running on you own robot. All that is required is that your robot have differential steering via two independent motors and equipped with rotary encoders, preferably high-resolution ones, and a push button to start and stop movement.

Step 1: Set up the Project

I’m assuming that most people are using Arduino based bots and the Arduino IDE, and might even be new to all of this so I’ll be as explicit as possible.

If you are an experienced or intermediate developer then you will know how to access the git repository and incorporate the code into your particular development environment, so just glance over the next few steps and adapt them to your particular setup.

For everyone else, grab a snapshot of the project by clicking the “Download ZIP” button on the main NavBot project page or this button here:

Screen Shot 2014-05-12 at 12.17.50 PM

Extract and/or copy the whole project into the Arduino sketch folder. If you are not sure where the sketch folder is launch the Arduino IDE and access the Preference’s dialog to find out.

Relaunch the IDE and select the “Open” command. You should see “NavBot” in the list of sketches and under that “NavBot_v1”.

Screen Shot 2014-05-12 at 12.16.45 PM

Select “NavBot_v1” to load the project.

You should see tabs for the following files:

  • NavBot_v1
  • BlankBot.h
  • BlackBot.h
  • WallieBot.h
  • ZumoBot.h
  • Navigator.cpp + .h
  • Pilot.cpp + .h

Screen Shot 2014-05-09 at 9.04.34 PM

Step 2: Rename BlankBot.h to MyBot.h

Rename or copy BlankBot.h to MyBot.h.

You can do this from the IDE by selecting the BlankBot.h tab and then using the tab action “Rename”.

Screen Shot 2014-05-12 at 12.20.10 PM

Step 3: Add Your Bot’s Code to MyBot.h

To get NavBot to function on your robot we need code to control the bot’s motors and read the wheel encoders, as well as configure some settings.

There are three functions that must be implemented:

init_bot() gets called at the end of the setup() function in the main sketch. Use this function to handle any initialization needed for your bot. You can make changes to the Pilot and Navigator at this point too.

motor_handler() is called by the Pilot to control the power levels of the motors. The power level ranges from -1024 (full reverse) to 1024 (full forward). I’ve added code to show how to scale these levels to values that a motor controller might typically use. I also added SWAP_MOTORS, LMOTOR_DIR and RMOTOR_DIR defines to allow easy software correction for any miss-wiring of the motors. [At some point I might move that functionality directly to the Pilot code.]

ticks_handler() is also called by the Pilot to get the current left and right tick counts for he wheels. It is important that the function only returns the number of ticks since the last time it was called, not the total ticks since the bot started.

At the top of the file there are numerous defines that are required by the main sketch:

WHEELBASE, WHEEL_DIAMETER and TICKS_PER_REV should be self explanitory. Set them to the correct values for your bot.

For the wheel base measure the distance between the the wheels from the center of each tires’ contact surface.

BUTTON_PIN defines which I/O pin has a button attached. You must have a button otherwise it is nearly impossible to control the NavBot and perform all the calibration tests required.

All the other defines can be left at their current values. We’ll revisit them further on.

You can refer to the BlackBot.h, ZumoBot.h and WallieBot.h files for examples of working code.

Step 4: Adding MyBot.h to the Main Sketch

Now that we have the code for your bot in MyBot.h we need to get the main sketch to load it.

Open NavBot_v1.ino and you will notice the following code near the top of the file:

// Include Bot Specific Code

// the arduino ide does not seem to allow
// nested includes so you must first include 
// any header files  needed by the bot's code

//#include <ZumoMotors.h>
//#include "ZumoBot.h"

//#include <Wire.h>
//#include <SoftwareSerial.h>
//#include <PololuQik.h>
//#include "WallieBot.h"

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "BlackBot.h"

//#inlcude <...your needed header files...>
//#include "MyBot.h"

Comment out the the current include statements and uncomment “#include “MyBot.h”. Also include any libraries your bot code needs before the MyBot.h include.

This will seem like a strange way to add the robot specific code to the project. Typically we would put the code into its own .cpp or .ino file and include any libraries needed in those file directly. However, in this case we want to have code for multiple robots in the project but only select one of them to compile at a time. If we use .cpp or .ino files for the robot specific code then they will all get compiled together which we don’t want. Instead we do the hack of putting the code in a .h file and only including the code for the robot we wish to build for.

One side effect of this approach is that the Arduino IDE does not check .h files for what libraries to include in the build, hence the reason we have to include the libraries for our robot code in the main sketch.

You should now be able to compile and download the sketch to your robot.

Step 5: Testing the Encoders

The first thing we need to ensure is that the encoders are working correctly.

To do this go to the top of the main sketch and look for the following code:

// Config Logic

#define CFG_TEST_ENCODERS     1     // print encoder ticks
#define CFG_TEST_MOTORS       0     // verify motor wiring
#define CFG_SQUARE_TEST       0
#define CFG_CALIBRATE_MOVE    0     // straight line movement
#define CFG_CALIBRATE_TURNS   0     // turning only test

Set CFG_TEST_ENCODERS to 1 and the other CFG_XXX settings to 0, compile and download the sketch to the bot then open the serial monitor. Once you see “NavBot V1!” appear start manually turning the wheels. You should see a stream of output showing the left and right tick counts for each wheel:

Screen Shot 2014-03-16 at 10.20.38 PM

Verify that the encoders are assigned to the correct wheel and that forward revolutions increase the count and backward revolutions decrease the count. Check that there are no encoder errors reported too.

Also verify the tick count per revolution. I usually do ten revolutions of each wheel and then divide the total count by ten. Remember to update TICKS_PER_REV in MyBot.h with the new value.

Once you are satisfied that the encoders are working correctly and that you know the precise ticks per revolution, move on to the next step.

Step 6: Verify Motor Wiring

For me I usually find that I’ve not paid enough attention to how the motors get wired up during the build and when I do realize it’s usually too late and I’d have to dismantle the bot to fix the problem. So instead I fix the problem in software.

To verify your motors are wired up correctly set the CFG_TEST_MOTORS define to 1 and CFG_TEST_ENCODERS to 0.

// Config Logic

#define CFG_TEST_ENCODERS     0     // print encoder ticks
#define CFG_TEST_MOTORS       1     // verify motor wiring
#define CFG_SQUARE_TEST       0
#define CFG_CALIBRATE_MOVE    0     // straight line movement
#define CFG_CALIBRATE_TURNS   0     // turning only test

Download the new code, open the serial monitor and, while holding the bot off the ground, press the start button.

The code will first turn the right motor for 2 seconds at half speed in a forward direction, i.e. the direction that will move your robot forwards. It will then turn the left motor forwards for 2 seconds. This pattern will repeat indefinitely.

In the serial output window you should see the following, assuming you left in the Serial print code from the template:

Screen Shot 2014-03-17 at 5.39.12 PM

If the wheels do not turn the correct way use SWAP_MOTORS, LMOTOR_DIR and RMOTOR_DIR to fix any problems.

Setting LMOTOR_DIR to -1L will reverse the direction of the left motor. RMOTOR_DIR affects the right motor direction. Set SWAP_MOTORS to 1 if you find that your left motor turns first.

Step 7: Square Test

At this point NavBot should be functional. We can test it by making it execute the square test.

Go to NavBot_v1.ino and set CFG_SQUARE_TEST to 1 and all the other CFG_XXX settings to 0:

// Config Logic

#define CFG_TEST_ENCODERS     0     // print encoder ticks
#define CFG_TEST_MOTORS       0     // verify motor wiring
#define CFG_SQUARE_TEST       1
#define CFG_CALIBRATE_MOVE    0     // straight line movement
#define CFG_CALIBRATE_TURNS   0     // turning only test

Further down the file set SQUARE_SIZE to an appropriate value:

#define SQUARE_SIZE       800   // size of square test sides in mm

And finally find squarePath and set it to fullSquare:

int16_t *squarePath = fullSquare;

You can see the sequence of movements and turns the bot will perform by examining the fullSquare array:

int16_t fullSquare[] = 

It goes clockwise around a square of size SQUARE_SIZE mm and then back again counter clockwise.

Compile and download the code to the bot. Now when you press the start button the robot will execute the fullSquare sequence. Place the robot on a flat even surface so that it has enough room to move, mark its starting position with a piece of tape and press the start button to see how it performs.

Step 8: Fixing Common Issues

The chances that the bot has executed two perfect squares and ended back at its start point will be next to zero. Typically the bot will not move in straight lines and its turns will most likely not be 90°. Its path about the square will look more like this:

Screen Shot 2014-05-22 at 12.13.35 PM

This is actually normal behavior and has to do with the true size of the wheels and wheelbase compared to what we have measured. These anomalies can be corrected for using the UMBmark Test. I have created a separate post that shows how to perform this test: NavBot: Calibration

You might also experience two other anomalies. The robot may zig-zag or weave while moving, and/or the motor speed seems to oscillate unduly. This will require tuning of the Pilot’s PID controllers. Fix these errors before doing any of the other corrections. Again I have created a separate post on how to tune the PID controllers: NavBot: Tuning the PID Controllers

Another issue you might face has to do with the resolution of the wheel encoders. If the encoders have a low resolution then your robot might perform multiple “positioning” maneuvers at each waypoint. This is due to the Pilot trying to be more accurate with positioning than the wheel encoders allow. The fix here is to increase the target radius of the pilot. Do this by adding the following line to your init_bot() function:


void init_bot()
    // do all bot initialization here
    pilot.SetTargetRadius( nvMM(20));

The default target radius is 10 mm so increase it till your robot stops trying to position itself at waypoints. However, first make sure you do not have PID controller issues as that could also cause this behavior.

Step 9: Making Your Own Paths

Once you have the NavBot tuned and calibrated it should now be able to maneuver accurately between waypoints, or at least as accurately as your wheel encoders allow.

You can also make your own path sequences. To do this set CFG_SQUARE_TEST to 0 again.

// Config Logic

#define CFG_TEST_ENCODERS     0     // print encoder ticks
#define CFG_TEST_MOTORS       0     // verify motor wiring
#define CFG_SQUARE_TEST       0
#define CFG_CALIBRATE_MOVE    0     // straight line movement
#define CFG_CALIBRATE_TURNS   0     // turning only test

Now look for the section called “Your Paths” in the main sketch:

// Your Paths

int16_t my_path[] =
  PTH_MOVE, 600, PTH_TURN, 180,
  PTH_MOVE, 600, PTH_TURN, 180,

int16_t *run_sequence = my_path;

Edit the my_path array to your heart’s content.

You could also develop your own waypoint system or simply incorporate the Navigator and Pilot into your own application.

Future Plans

I plan to make an app that can control the NavBot via the serial link over bluetooth. That will probable be for Version 2 of NavBot. Version 3 will hopefully be where I add compass, gyroscope and accelerometer support.

My ultimate goal is to create a navbot for Sparkfun’s AVC competition in 2015.


One comment

  1. José Jácome · · Reply

    Hi, I found very useful your code, however I can’t implement it to my robot yet, my robots need to use the function pilot.MoveTo(pos) but I get an erratic result of the wheels, for example I set the pos of the robot (250,400) and the Rightwheel is turning clockwise however the LeftWheel is turning in counterclockwise! Any advice is aprecciated!
    Note: I’m using a low resolution encoder made with a coloured disk and a QRD1114

Comments welcome

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )


Connecting to %s