In part I, we used strictly the accelerometer to determine orientation. However, this makes the device sensitive to shaking. Any acceleration Robovero experiences will interfere with the gravity reading.

 

Part II used the gyroscope to determine the angle. For those too impatient to find out what the problem with using strictly the gyroscope was, the answer is drift. Because the gyroscope method adds up changes in angle over time, it also adds up error in the gyroscope measurements. If the script is run for long enough, the error will become noticeably long.

 

So let's just do a quick compare and contrast of these two sensors. The accelerometer experiences a fair deal of noise when it is shaked, but it doesn't drift. On the other hand, the gyroscope is not as effected by shaking, but over long periods of time, the measured value will deviate from the true value.

 

 

Suppose we have a good guess of a previous angle. If we know what speed the Robovero is rotating at, we can multiply it by the time it has been spinning and add it to our previous known angle to get an estimate of our current angle. That's exactly what our second script did. That method suffers from drift, but it gives us a good estimate of our current orientation. However, supposed we "pulled" that angle closer to the angle determined by the accelerometer. Because the accelerometer doesn't drift over time, this is the same as pulling our reading closer to it's true value.

 

 

This is a simple sensor fusion aims in which we get the best of both worlds. By combining the reading from the accelerometer and gyroscope, we can reduce the effects of drift and noise. I will illustrate a simple sensor fusion algorithm by taking a weighted average. However, there exist other methods such as Kalman filtering.

 

Run this code:


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

u = 0.15

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)
angle = 0
while True:
  acceldata = accelerometer.read6Reg(accel_x_low)
  gyrodata = gyro.read6Reg(gyro_x_low)

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

  gyroAngle = 0.1 * twosComplement(gyrodata[2], gyrodata[3]) / 1055.0
  y = twosComplement(gyrodata[2], gyrodata[3]) /1055.0
  w = 0.01 * y * 180 / 3.1415926535

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

  angle = ((1.0-u)*(angle + w)) + (u*accelAngle)

  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

 

You will notice at the beginning of the script there is a variable declared u. This value is between 0 and 1 and determines which device the algorithm favours more. If u = 1, the script will only use the accelerometer. If u = 0, only the gyroscope is used. Feel free to tinker with the value.