Sunday, November 6, 2011

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!

2 comments:

  1. Can you please share the C/C++ source code for this device with Tx & Rx lines short into one single data line using one 74HC126 chip controlled by PD6 & PD7 pins of the ATmega328.

    ReplyDelete
  2. I have "IMU Digital Combo Board - 6 DoF ITG3200/ADXL345".

    http://www.sparkfun.com/products/10121

    I have made an other breakout board with ATmega328 and one 74HC126 IC, with an intention to use the IMU in Dynamixel bus along with the servos.

    Honestly i'm bad at coding, can you help modifying your existing code for my project?

    ReplyDelete