Some XLoBorg code to get you started

Here’s code to create a graphical compass for your XLoBorg from PiBorg. It’s a work in progress, working fine for 2-dimensional headings (ie: keep the Pi on a flat, level surface). It’s got the code in for 3-dimensional headings using the accelerometer as well as the magnetometer to enable the Pi to be tilted but this is *ahem* not quite working yet (my maths is a bit screwy it seems). I’ll post an update when it’s working good ’n proper.

I’ve included support for Declination which is pretty important to get an accurate compass heading. Just define your location(s) as constants near the top and edit the declinationAngle = BRISTOL / 1000 to equal your location and hey presto.

I also found a very interesting PDF from Honeywell all about how these combined accelerometer + magnetometer compasses work. Definitely worth a read.

# Calculate and generate a graphical display of the compass heading
# calculated from the output of XLoBorg from www.piborg.com

# Author: Colin Deady, 2012.
# Released under Creative Commons Attribution Share Alike,
http://creativecommons.org/licenses/by-sa/2.5/

# Credit to Bryan Oakley for the MyApp class code that generates the clock face:
#
http://stackoverflow.com/questions/6161816/tkinter-how-to-make-tkinter-to-refresh-and-delete-last-lines
# Released under Creative Commons Attribution Share Alike (
http://stackexchange.com/legal, http://creativecommons.org/licenses/by-sa/2.5/)

# WARNING: THIS IS NOT TO BE USED FOR ANYTHING IMPORTANT!
# THIS IS PURELY PROOF OF CONCEPT FOR FUN.
# NO liability is accepted WHATSOEVER for inaccurate data generated by this program.
# If you use it to navigate the Pacific / Sahara / other place of your choice and
# end up falling over the edge of the world and the last thing you see are some giant
# elephants then that is your get, not mine.

import Tkinter as tk; import time
from math import cos,sin,pi, atan, atan2, floor, asin
import sys

# Load the XLoBorg library
import XLoBorg

# Tell the library to disable diagnostic printouts
#XLoBorg.printFunction = XLoBorg.NoPrint

# Start the XLoBorg module (sets up devices)
XLoBorg.Init()

# define a target direction that we can use to rotate the Pi towards
# this could be built upon to be the target angle that a robot steers towards
targetDirection = 120

# Declination - very important!
# http://en.wikipedia.org/wiki/Declination
# http://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library
# http://www.magnetic-declination.com/
BRISTOL = -2.2
declinationAngle = BRISTOL/1000

# create a "clock face" upon which we can render the compass heading of XLoBorg
class MyApp(tk.Tk):
def __init__(self, *args, **kwargs):
tk.Tk.__init__(self, *args, **kwargs)

self.size=300

self.title("Compass")
self.w = tk.Canvas(self, width=320, height=320, bg="#111", relief= "sunken", border=10)
self.w.pack()

self.w.create_oval(10,10,330,330, fill="cyan", tags="compassbg1")
self.w.create_oval(70,70,270,270, fill="blue", tags="compassbg2")
self.w.create_line(0,0,0,0, fill="red", width="3", tags="compass")
self.w.create_line(0,0,0,0, fill="yellow", width="3", tag="target")

legendbg = "#fff"
uzr1 = tk.Label(self, text="N", bg=legendbg )
uzr1.place(x=160, y=12)
uzr2 = tk.Label(self, text="S", bg=legendbg )
uzr2.place(x=160, y=311)
uzr3 = tk.Label(self, text="E", bg=legendbg )
uzr3.place(x=317, y=160)
uzr4 = tk.Label(self, text="W", bg=legendbg )
uzr4.place(x=12, y=162)

e = tk.Button(self,text="Quit", command=self.Quit)
e.pack()

self.update_compass()

def update_compass(self):

# Read and render the raw compass readings, correcting by our manual calibration values
compass = XLoBorg.ReadCompassRaw()
print str(compass[0])+","+str(compass[1])+","+str(compass[2])
mX = compass[0] - -248
mY = compass[1] - 370
mZ = compass[2] - 1384
# print "mX:"+str(mX)+",mY:"+str(mY)

# Read the raw accelerometer readings
accel = XLoBorg.ReadAccelerometer()
print str(accel[0])+","+str(accel[1])+","+str(accel[2])
aX = accel[0]
aY = accel[1]
aZ = accel[2]

# some notes on tilt compensation
# http://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial

# =======================================================================================
# The following code does NOT yet work... it should but my maths is a bit screwy
# When I manage to fix it then we'll have a compass that copes with being tilted, huzzah!
rollRadians = asin(aY)
pitchRadians = asin(aX)
# up to 40 degree tilt max:
# http://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial
cosRoll = cos(rollRadians)
sinRoll = sin(rollRadians)
cosPitch = cos(pitchRadians);
sinPitch = sin(pitchRadians);

vX = mX * cosPitch + mZ * sinPitch;
vY = mX * sinRoll * sinPitch + mY * cosRoll - mZ * sinRoll * cosPitch;
# =======================================================================================

# get the heading in radians
heading = atan2(vY,vX)

# correct for declination
heading +=declinationAngle

# Correct negative values
if (heading < 0):
heading = heading + (2 * pi)

# Check for wrap due to declination and compensate
if(heading > 2*pi):
heading -= 2*pi

# convert to degrees
heading = heading * 180/pi;

# get the base degrees
heading = floor(heading)
print heading

angle = heading*pi*2/360
ox = 165
oy = 165
x = ox + self.size*sin(angle)*0.45
y = oy - self.size*cos(angle)*0.45
self.w.coords("compass", (ox,oy,x,y))

angleTarget = targetDirection*pi*2/360
xt = ox + self.size*sin(angleTarget)*0.45
yt = oy - self.size*cos(angleTarget)*0.45
self.w.coords("target", (ox,oy, xt, yt))

gapAngle = abs(targetDirection - heading) # we want the angle without the sign
# if the gap angle is more than 180 degrees away then it is closer than we think
if gapAngle > 180 :
gapAngle = 360-gapAngle
if gapAngle < 0 :
print "Target is " + str(gapAngle) + " degrees anticlockwise"
elif gapAngle > 0 :
print "Target is " + str(gapAngle) + " degrees clockwise"
else:
print "Target acquired!"

self.after(500, self.update_compass)

def Quit(self):
self.after(700,self.destroy())

app = MyApp()
app.mainloop()

Comments