9DOF IMU Development For Dynamixel Based Robots - Part 1

I recently purchased the 9 Degrees of Freedom - Razor IMU from Sparkfun Electronics. 

My intentions with this device is to use it as a platform to start the development of an IMU that communicates on the Dynamixel AX (and/or RX /EX) buss for easy use in Dynamixel based robots.

For starters I wanted to make sure the unit was working as expected with its pre programmed firmware. I wrote a little wxpython app that displays the values of the IMU's three sensors in a nice little GUI.

Nice little GUI with nice output.

Here is the python code that makes it go:

#!/usr/bin/python

import serial
import threading
import math
import wx
   
class IMU_GUI(wx.Frame):

 TIMER_ID = wx.NewId()
 
 def __init__(self):
  wx.Frame.__init__(self, None, -1, "9DOF IMU Testing", style=wx.DEFAULT_FRAME_STYLE)
  
  self.ax = wx.StaticText(self, -1, "ax: ?", size=(100,20))
  self.ay = wx.StaticText(self, -1, "ay: ?", size=(100,20))
  self.az = wx.StaticText(self, -1, "az: ?", size=(100,20))
  
  self.p = wx.StaticText(self, -1, "p: ?", size=(100,20))
  self.r = wx.StaticText(self, -1, "r: ?", size=(100,20))
  self.t = wx.StaticText(self, -1, "t: ?", size=(100,20))
  
  self.gx = wx.StaticText(self, -1, "gx: ?", size=(100,20))
  self.gy = wx.StaticText(self, -1, "gy: ?", size=(100,20))
  self.gz = wx.StaticText(self, -1, "gz: ?", size=(100,20))
  
  self.mx = wx.StaticText(self, -1, "mx: ?", size=(100,20))
  self.my = wx.StaticText(self, -1, "my: ?", size=(100,20))
  self.mz = wx.StaticText(self, -1, "mz: ?", size=(100,20))
  
  self.serial = serial.Serial("/dev/ttyUSB0", 57600)
  
  self.data = []
  
  self.thread = threading.Thread(target=self.listen)
  self.thread.start()
  
  self.timer = wx.Timer(self, self.TIMER_ID)
  self.timer.Start(10)
  wx.EVT_TIMER(self, self.TIMER_ID, self.onTimer)
  
  sizer = wx.GridBagSizer(0, 0)
  
  sizer.Add(self.ax, pos=(0,0), flag=wx.ALL, border=5)
  sizer.Add(self.ay, pos=(0,1), flag=wx.ALL, border=5)
  sizer.Add(self.az, pos=(0,2), flag=wx.ALL, border=5)
  
  sizer.Add(self.gx, pos=(1,0), flag=wx.ALL, border=5)
  sizer.Add(self.gy, pos=(1,1), flag=wx.ALL, border=5)
  sizer.Add(self.gz, pos=(1,2), flag=wx.ALL, border=5)
  
  sizer.Add(self.mx, pos=(2,0), flag=wx.ALL, border=5)
  sizer.Add(self.my, pos=(2,1), flag=wx.ALL, border=5)
  sizer.Add(self.mz, pos=(2,2), flag=wx.ALL, border=5)
  
  sizer.Add(self.p, pos=(3,0), flag=wx.ALL, border=5)
  sizer.Add(self.r, pos=(3,1), flag=wx.ALL, border=5)
  sizer.Add(self.t, pos=(3,2), flag=wx.ALL, border=5)
  
  self.SetSizerAndFit(sizer)
  
  self.Show(True)
  
  self.serial.write("4")
  
 def onTimer(self, event):
 
  try:
 
   if len(self.data) != 9:
    return

   self.ax.SetLabel("ax: " + self.data[0][1:])
   self.ay.SetLabel("ay: " + self.data[1])
   self.az.SetLabel("az: " + self.data[2])
   self.gx.SetLabel("gx: " + self.data[3])
   self.gy.SetLabel("gy: " + self.data[4])
   self.gz.SetLabel("gz: " + self.data[5])
   self.mx.SetLabel("mx: " + self.data[6])
   self.my.SetLabel("my: " + self.data[7])
   self.mz.SetLabel("mz: " + self.data[8][:-1])
  
   ax = int(self.data[0][1:])
   ay = int(self.data[1])
   az = int(self.data[2])
  
   #print ax, ay, az
  
   p = math.atan2(ax,math.sqrt(math.pow(ay,2)+math.pow(az,2))) * (180/3.14)
   r = math.atan2(ay,math.sqrt(math.pow(ax,2)+math.pow(az,2))) * (180/3.14)
   t = math.atan2(math.sqrt(math.pow(ax,2)+math.pow(ay,2)),az) * (180/3.14)
  
   #print p, r, t
  
   self.p.SetLabel("p: " + str(p)[:10])
   self.r.SetLabel("r: " + str(r)[:10])
   self.t.SetLabel("t: " + str(t)[:10])
   
  except:
   pass
  
 def listen(self): 
  
  while True:
   if self.serial.inWaiting() != 0:
    line = ""
    done = False
    while not done:
     c = self.serial.read()
     #print ord(c)
     if ord(c) == 13 or ord(c) == 10:
      done = True
     else:
      line += c 
    #print line
    
    data = line.split(",")
    if len(data) == 9:
     self.data = data
    
if __name__ == "__main__":
 app = wx.App(0)
 frame = IMU_GUI()
 app.MainLoop()

More to come!