Cbot - Plant Maintenance Robot 1

Preliminary Development Projects

Updated 7/26/15


Incorporating the Triple axis Magnetometer

Once our robot starts to move off of its docking base and on towards its task of watering plants, it will need three major sensors to find its way to the plants and back. The first of which is the current topic of this article, an electronic compass module for direction inside the home, secondly a rolling wheel encoder to determine how far it has actually moved along the floor, and finally a sonar to measure its position off of landmarks to confirm that the robot has indeed reached the various way points in its travel through the house. We have now just finished the compass project with Cbot, and this time we tried something totally new.

In past robots which used a compass module, we used the Devantech compass module, which costs around $70 or more and came precalibrated for the UK, but worked reasonably well for our location in the world. The calibration procedure was a nightmare, and I never could quite get it to work right. The micro controller read it by measuring a pulse width out of the compass and converting to a compass heading. This worked for several robots, but since then a modern new type of sensor has made its way onto the scene and it was my goal to get one up and running to replace the old and now hard to find former compass. The Adafruit HCL...... module contains a Honeywell triple axis magnetometer with the latest micro engineering technology. The great news is that this small board, which is half the size of the Devantech costs only $9 and has all the level conversion hardware built in for direct hook up to your micro controller. Once I got my version of the code working, the device works very well and easily guides the robot within 2 degrees or better through the halls of the house. I spent a lot of time studying the actual magnetometer data sheet and feel this is a robust device to guide our robot and make several other projects as well. Here are some photos of the new device mounted on the robot.

Left: The tiny Magnetometer break out board. The tiny 3/4" square Adafruit brand triple axis magnetometer break out board. The actual magnetometer is the small black square device in the middle.

Left: The compass is mounted in a Plexiglas bubble on a non ferrous wooden stalk about 12 inches over the back of the robot. This keeps interference from the robots motors and metal objects on the ground from interfering with the compass bearings. Wires are run down the stalk and then into the robots micro controller boards. The magnetometer uses the I2C interface for communication. (Read: Eye - Squared - C).

Left: The current state of the robot, with the magnetometer bubble mounted on the rear.
Compass Mathematics diagrams

 This graphic here demonstrates the basic concept of the two bearing angles we will be working with. First, "B" is the Bearing we wish to go. This number is either derived by calculation and trigonometry or in the case of Geobot's task, it is the first angle it sees as straight ahead when its power is turned on.

"CB" is the current bearing, or where the robot is heading right now. You can see that here, we need to turn to the right about 20 degrees to get back on track to the way we want to be going. These two numbers will be sufficient to point our bot toward a target, and actually get us very near our destination.

 Now lets take a closer look at the two angles, and discuss them in a bit further detail. First, we know that the Bearing angle is of course part of a large circle, that goes from 0 degrees at the top (North) through 90 degrees (East), 180 (South), and 270 (West). We then end up completing our circle at the top again at 359....then 360 which is read as 0 degrees once again.

Along with every Bearing angle, there is a Tolerance. This is the small error angle that is considered "close enough" to the angle we wish to go and for the robot if its within this range to consider it to be right on course. This angle varies depending on the terrain, and the accuracy of the drive train. For our robot, with crude tank tread drive and driving over huge rocks and dips, the angle of +/- 2 degrees was chosen. Any tighter tolerance would not have resulted in a more accurate run.

Next we must consider the math of the correction direction to rotate to get back on coarse. This at first sounds pretty straightforward, but in practice required some careful thinking on our part to get it straight!

 The problem is this: If I know my current bearing and the bearing I wish to head, do I turn right or left to make the turn the shortest angular distance possible? We don't want to rotate 359 degrees to the left, when all we had to do is turn 1 degree to the right. Here in this graphic, you can see we are currently going to the upper left at about 300 degrees in direction. To test to the 45 degree Bearing angle and onward to our goal, its a fairly short turn to the right but we must cross the 0 degree point. On the other hand, if we rotate the much longer direction to the left, we simply count down our degrees from 300 to 45 and were done. In our math before we make a decision on which way to turn, we must first calculate the direction and angular amount. The basic formula is:

1. First calculate absolute distance:
Diff = ABS(B-CB)

If Diff < 180 then Diff = Ds
IF Diff > 180 then Ds = 360 - Diff

Shortest Distance = Ds
B = Final Bearing
CB = Current Bearing

Also, since our Tolerance is 2degrees, we know we have to move if Diff > 2.

 But which direction do we rotate? There are four cases here.

Case 1: IF Diff < 180 and IF B > CB then ROTATE RIGHT

Case 2: IF Diff < 180 and IF B < CB then ROTATE LEFT

Case 3: IF Diff > 180 and IF B > CB then ROTATE LEFT

Case 4: IF Diff > 180 and IF B < CB then ROTATE RIGHT

Finally, we must calculate at this time the direction for the return trip. Lets say we head out at a bearing of 45 degrees, and after a thousand feet want to turn around and return. The calculation is:

If B < 180 then opposite direction = B + 180

If B >= 180 then opposite direction = b - 180.

Now lets go through an example of the above graphic to illustrate the math. Here we have the following conditions:

CB = 300 and B = 45.

Diff = 255
Ds = 360 - 255 = 105 = Shortest distance.

Direction to rotate:  B < CB so we rotate RIGHT.

Finally, the Opposite direction you are heading is B+180, or 45 + 180 = 225 degrees.
Code Listing (CCS-C)
//Chris Schur
//Adafruit HMC5883L magnetometer -  16F877A)
//Date:  6/16/15
/*Description of this Program: 
Version 8 - continuous reading version of 7.*/

//I/O Designations ---------------------------------------------------
// RB0:  Status LED output
// RB1:  LCD output
// RB2:  Piezzo Speaker output
// RC3:  INPUT - SCL - to magnetometer
// RC4:  INPUT - SDA - to magnetometer

//Include Files:
#include <16F877A.h>  
#include "math.h"  //required for compass
//Directives and Defines:
#fuses NOPROTECT,HS,NOWDT   //xtal is used
#use delay(crystal=10MHz)   //xtal speed
//set up i2c - data, clock pins are here hardware on 877a.part uses slow spec.  
#use I2C(master, sda=PIN_C4, scl=PIN_C3, slow, FORCE_HW)  //HARDWARE IMPLEMENT
#use fast_io(ALL)          //must define tris below in main when using this
// HMC5883 required Registers
#define W_DATA   0x3C     //Used to perform a Write operation
#define R_DATA   0x3D     //Used to perform a Read operation
#define CON_A    0x00     //Sets up measurement and sampling parameters.
#define CON_B    0x01     //Send continuous MeVARurement mode.
#define MOD_R    0x02     //Read/Write Register, Selects the operating mode. Default = Single meVARurement
#define X_MSB    0x03     //Read Register, Output of X MSB 8-bit value.
#define X_LSB    0x04     //Read Register, Output of X LSB 8-bit value.
#define Z_MSB    0x05     //Read Register, Output of Z MSB 8-bit value.
#define Z_LSB    0x06     //Read Register, Output of Z LSB 8-bit value.
#define Y_MSB    0x07     //Read Register, Output of Y MSB 8-bit value.
#define Y_LSB    0x08     //Read Register, Output of Y LSB 8-bit value.
//for LCD:
#use rs232(baud=9600, xmit=Pin_B1, bits=8, parity=N,stream=SERIALNH)

//Global Variables:
int16 n; //counting var
int8 M_data[6]; //Array - 6 units 8 bit Measured datas (compass)
int16 Xm=0,Ym=0,Zm=0; //16 bit X,Y,Z variables (compass)
float bearing;  //final compass reading.

//Functions/Subroutines, Prototypes:
//for compass only:
void hmc5883_write(int add, int data);  //write to function
int16 hmc5883_read(int add);            //Read from function
void hmc5883_init();                    //Sets up starting conditions
void read_reg();                        //reads compass registers, calc xyz
float calc_heading();                  //calculates returns bearing.
//For New Haven LCD only:
void LCDCLR();
void LCDLN2();
void LCDLN3();
void LCDLN4();

//-- Main Program
void main(void) {
// Set TRIS I/O directions, define analog inputs, compartors:
      set_tris_C(0b11111111);  //set both sda and sclk for inputs (float)
   output_low(Pin_B0);  //status off
   delay_ms(1000);  //LCD warmup time
   fputc(0xFE,SERIALNH);   //Command Prefix
   fputc(0x53,SERIALNH);   //set cursor command
   fputc(8,SERIALNH);   //LCD backlight - 4 is mid 0-8
   //start compass single read: ----------------------------------------------
   hmc5883_init();   //Initialize compass settings. 
   while(true)  {
   read_reg();  //read compass registers, calc xyz
   bearing = calc_heading();  //calculates & returns final compass bearing
   fprintf(SERIALNH,"H= %f  ",bearing);
}  //niam
//********* Functions which have prototypes at top of program ****************
//for compass only:
void hmc5883_write(int add, int data)  {
         i2c_write(W_DATA);      //0x03
         i2c_stop();  }
int16 hmc5883_read(int add)  {
         int retval;
         i2c_write(W_DATA);      //0x03
         i2c_write(R_DATA);      //0x3D
         return retval;  }
void hmc5883_init()  {
         hmc5883_write(MOD_R,  0x00);     //0x02, 0x00
         hmc5883_write(CON_A,  0x10);     //0x00, 0x10
         hmc5883_write(CON_B,  0x20);     //0x01, 0x20
         delay_ms(100);  }
void read_reg()  {   //read compass registers
         //read registers in compass 
   M_data[0]=hmc5883_read(0x04); //Read X (LSB)
   M_data[1]=hmc5883_read(0x03); //Read X (MSB)
   M_data[2]=hmc5883_read(0x08); //Read Y (LSB)
   M_data[3]=hmc5883_read(0x07); //Read Y (MSB)
   M_data[4]=hmc5883_read(0x06); //Read Z (LSB)
   M_data[5]=hmc5883_read(0x05); //Read Z (MSB)
   //Create Word from Highbyte and Lowbyte data:
float calc_heading()  {
   //Calculate using math.h function the bearing.
   float Heading = atan2((signed int16)Ym,(signed int16)Xm)* 180 / pi + 180;
   //correct for this equation yielding values 180 off:
   if (Heading < 180)
      Heading = Heading + 180;
   else if (Heading > 180)
      Heading = Heading - 180;
   else if (Heading == 180)
      Heading = 0;
   return Heading;
//For New Haven LCD only:
//Clears LCD Display:
void LCDCLR()  {
   fputc(0xFE,SERIALNH);   //Command Prefix
   fputc(0x51,SERIALNH);   //Clear screen
//Sets LCD to line 2 start point
void LCDLN2()  {
   fputc(0xFE,SERIALNH);   //Command Prefix
   fputc(0x45,SERIALNH);   //set cursor command
   fputc(0x40,SERIALNH);   //Set cursor to next line, pos 40 = start line 2
//Sets LCD to line 3 start point
void LCDLN3()  {
   fputc(0xFE,SERIALNH);   //Command Prefix
   fputc(0x45,SERIALNH);   //set cursor command
   fputc(0x14,SERIALNH);   //Set cursor to next line, pos 14 = start line 3
//Sets LCD to line 4 start point
void LCDLN4()  {
   fputc(0xFE,SERIALNH);   //Command Prefix
   fputc(0x45,SERIALNH);   //set cursor command
   fputc(0x54,SERIALNH);   //Set cursor to next line, pos 54 = start line 4

Interested in learning more on the development of this robot? Here are previous uploads - LATEST AT THE BOTTOM:

Previous Articles
1. Plant watering sensors
2. Cbot basic concepts and project definition
3.  Docking sensor, battery charging and FSM explained
4.  Development of the water tank, pump and feed mechanism
5.  Development of the reservoir refill system
6.  Incorporating the Tripple axis magnetometer for compass navigation