Things that spin around are cool. Like drills! Those things are awesome. Maybe you want to make your Robovero spin like a drill. Or maybe you just want to make your Robovero spin. Either way, sometimes you want to know how much the Robovero has spun. In this project, I am going to show you how to determine the orientation of the Robovero using it's inertial measurement unit.

 

There are many ways you can rotate the Robovero, but for the sake of simplicity in this tutorial we are going to consider only rotation about one axis as shown below:

 

 

Now let's consider what we have available and how we can use it to determine orientation. A quick look at the Robovero specs show it has a 3DOF accelerometer. We can use the accelerometer to determine the orientation of the Robovero by measuring the direction of gravity. Look at the below diagram:

 

By recording the X and Y acceleration, we can determine the angle between the x axis and the gravity vector. Since gravity is always pointing down (unless you're in Australia). we can then determine the Robovero orientation. In case your trig-fu is weak, the formula looks like this:

 

tan(angle) = Y/X

 

Now of course there are other formulas that you can use, but I choose this one.  I've made a quick GUI application that shows you the angle of the Robovero! I'll give you the code in a sec, but there's a few things I need you to do first.

 

1) Run sudo apt-get install python-opencv

I'm using OpenCV's HighGUI functions to make a GUI. It's not super powerful like Qt or GTK, but I know it well (and by it I mean the C++ version) and it was fast to write the application in.

 

2) Download the below image and stick it in your Robovero folder:

 

That's all. Now to the code. Name this AccelAngle.py

 

 

from robovero.extras import Array, roboveroConfig
from robovero.lpc17xx_i2c import I2C_M_SETUP_Type, I2C_MasterTransferData, \
                            I2C_TRANSFER_OPT_Type
from robovero.lpc17xx_gpio import GPIO_ReadValue
from robovero.LPC17xx import LPC_I2C0
from robovero.lpc_types import Status
import time
import csv
from math import *
import cv

accel_ctrl_reg1 = 0×20
accel_ctrl_reg4 = 0×23
accel_x_low = 0×28

gyro_ctrl_reg1 = 0×20
gyro_ctrl_reg2 = 0×21
gyro_ctrl_reg3 = 0×22
gyro_ctrl_reg4 = 0×23
gyro_ctrl_reg5 = 0×24
gyro_status_reg = 0×27
gyro_x_low = 0×28
gyro_fifo_ctrl_reg = 0x2E

class I2CDevice(object):
  def __init__(self, address):
    self.config = I2C_M_SETUP_Type()
    self.tx_data = Array(2, 1)
    self.rx_data = Array(1, 1)
    self.rx_data6 = Array(6, 1)
    self.config.sl_addr7bit = address
    self.config.tx_data = self.tx_data.ptr
    self.config.retransmissions_max = 3

  def readReg(self, register):
    self.tx_data[0] = register
    self.config.tx_length = 1
    self.config.rx_data = self.rx_data.ptr
    self.config.rx_length = 1
    ret = I2C_MasterTransferData(LPC_I2C0, self.config.ptr,
                                  I2C_TRANSFER_OPT_Type.I2C_TRANSFER_POLLING)
    if ret == Status.ERROR:
      exit("I2C Read Error")   
    return self.rx_data[0]

  def read6Reg(self, register):
    self.tx_data[0] = register | 0b10000000 #MSB must be equal to 1 to read multiple bytes
    self.config.tx_length = 1
    self.config.rx_data = self.rx_data6.ptr
    self.config.rx_length = 6
    ret = I2C_MasterTransferData(LPC_I2C0, self.config.ptr,
                                  I2C_TRANSFER_OPT_Type.I2C_TRANSFER_POLLING)
    if ret == Status.ERROR:
      exit("I2C Read Error")   
    return self.rx_data6

  def writeReg(self, register, value):
    self.tx_data[0] = register
    self.tx_data[1] = value
    self.config.tx_length = 2
    self.config.rx_data = 0
    self.config.rx_length = 0
    ret = I2C_MasterTransferData(LPC_I2C0, self.config.ptr,
                                  I2C_TRANSFER_OPT_Type.I2C_TRANSFER_POLLING)
    if ret == Status.ERROR:
      exit("I2C Write Error")
    if self.readReg(register) != value:
      exit("I2C Verification Error")
    return None

# Initialize pin select registers
roboveroConfig()

# configure accelerometer
accelerometer = I2CDevice(0×18)
accelerometer.writeReg(accel_ctrl_reg1, 0×27)
accelerometer.writeReg(accel_ctrl_reg4, 0×00) #

# configure the gyro
gyro = I2CDevice(0×68)
gyro.writeReg(gyro_ctrl_reg3, 0×08) # enable DRDY
gyro.writeReg(gyro_ctrl_reg4, 0×80) # enable block data read mode
gyro.writeReg(gyro_ctrl_reg1, 0x0F) # normal mode, enable all axes, 250dps

def twosComplement(low_byte, high_byte):
  """Unpack 16-bit twos complement representation of the result.
  """
  return (((low_byte + (high_byte << 8)) + 2**15) % 2**16 – 2**15)

writer = csv.writer(open('eggs.csv', 'wb'), delimiter=' ',quotechar='|', quoting=csv.QUOTE_MINIMAL)

cv.NamedWindow("Window", cv.CV_WINDOW_AUTOSIZE)
bar = cv.LoadImage("Bar.png")
cv.ShowImage("Window", bar)

rotatedBar = cv.LoadImage("Bar.png")
rotationMat = cv.CreateMat(2, 3, cv.CV_32FC1)
center = (bar.width/2,bar.height/2)

while True:
  acceldata = accelerometer.read6Reg(accel_x_low)

  x = twosComplement(acceldata[2], acceldata[3]) / 16384.0
  z = twosComplement(acceldata[4], acceldata[5]) / 16384.0

  angle = -((atan2(z,x) * 180.0 / 3.14159265358979) + 90)

  cv.GetRotationMatrix2D(center,-angle,1,rotationMat)
  cv.WarpAffine(bar,rotatedBar,rotationMat)
  cv.ShowImage("Window", rotatedBar)
  print angle
  var = cv.WaitKey(100)
  if var != -1:
    break

 

 

Give it a try! Your exercise for today? Try and break it!

 

I'll be back with part II