pbLua Gyro

Introduction

This tutorial describes how to use the HiTechnic Gyro Sensor with pbLua. The examples in this section will explain how to read the sensor as well as how to filter the measurements for effective use in robot models that need to determine the rate of turn and absolute heading in a particular axis. We’ll finish with an example of a robot that can balance on two wheels.

If you really can’t wait, scroll down to the bottom of this page to see the video of the balancing NXT…

You can buy the Gyro Sensor directly from directly from HiTechnic or from the LEGO S@H Website.

As a reminder, there is an excellent online version of Programming in Lua that is a complete description of Lua along with many programming examples, and an even better idea is to purchase the latest Programming in Lua book .

By the end of this tutorial, you’ll be able to use the Gyro sensor to build a self-balancing robot. We’ll be building on the knowledge gained from the tutorials on:

Contents

Sensor Basics

Before using the Gyro sensor with the Input Port API you should definitely review the material in the Basic Analog Sensors tutorial if you haven’t already seen it.

You’ll need to know the port number the sensor will be plugged into, what type the sensor is, and what direction the sensor port pins must be set to. These things are described in detail for each sensor type in the next sections.

The NXT has 4 sensor ports along the bottom face of the housing. They are numbered from 1 to 4. The sensor type is set using the nxt.InputSetType() function. Each sensor port has two I/O pins which can be independently controlled using the nxt.InputSetDir() and nxt.InputSetState() functions. The sensor port is read using the nxt.InputGetState() function which returns the raw A/D value, and the current state of the two I/O pins.

The raw A/D value will range between 0 and 1023 – which is a range of 10 bits.

Setting Up The Gyro Sensor

The Gyro Sensor is an analog sensor, which means that it produces an analog reading that can be read frequently by the NXT. This reading is updated only once every 3 msec, or about 333 Hz, so there’s no point in reading it much faster than that. The I2C based sensors such as the colour or 3-axis accelerometer can return multiple values, but you can only sample them at between 100 and 150 Hz.

The ability to read the gyro sensor frequently makes it suitable for reacting quickly to sensor rotation, which is going to be helpful when we’re balancing an NXT on two wheels.

The gyro sensor measures the rate at which is is turning which is often expressed in degrees per second. The faster it’s turning, the more degrees per second and therefore the higher (or lower) the sensor reading.

The sensor has two modes of operation. The first is less sensitive (Mode 0) which returns a value of about 1 count per degree per second of revolution. The maximum turn rate that can be measured is +/- 360 degrees per second, or about 1 revolution per second.

The second mode of operation is high sensitivity, which gives about 4 counts per degree per second of revolution. In this mode, you can only measure +/- 180 degrees per second or about 0.5 revolutions per second.

The next sections of this note build an application in pieces. You’ll need to load them all in sequence as we move through the tutorial, and they’re available in the pbLua distribution as well. Use the USB rather than the Bluetooth console because it’s faster – it takes more time to send a line of output to the Bluetooth console.

What Information Does the Rate Gyro Give Us?

For the typical applications of a rate gyro, you’re interested in the answer to one of these increasingly difficult questions:

  1. Am I turning?
  2. In which direction am I turning?
  3. At what rate and in which direction am I turning?
  4. How far, at what rate, and in which direction am I turning?

The answers to all of these questions require some knowledge of the zero point of the sensor. The first three these questions can be answered with simple arithmetic, just subtract the zero point from the current reading. The last question can be answered with some slightly more complicated math which we’ll go into later.

So let’s move along and answer the first three questions…

Determining the Gyro Zero Point

One of the things that we need to do to make use of the readings from the gyro sensor is figure out the zero point. When the sensor is held in the stationary position, it returns a value of about 620. This is a little more than half of the full range of output for the sensor port, which can range from 0 to 1024.

The sensor “warms up” after about 10 seconds of operation and the readings begin to stabilize. At rest, the sensor I have returns a value that oscillates between 616 and 617. To determine how fast, and in what direction we’re turning, we’ll need to subtract this zero point from each reading.

Here’s a sample program that prints the the raw value from the gyro sensor along with a timestamp about once every 4 msec: 

-- Initialize the Gyro sensor on a port

function GyroInit(port,active)
  nxt.InputSetType(port,0)

  if 0 == (active or 0) then nxt.InputSetState(port,0,0)
                        else nxt.InputSetState(port,1,0)
  end

  nxt.InputSetDir(port,1,1)
end

-- Read the Gyro sensor no faster than every 4 msec and
-- print the timestamp and result. Stop when we press the
-- orange button

function GyroTest(port, timeout)
  timeout = timeout or 4

  local sampletick = 0
  local tick, new

  repeat
    tick = nxt.TimerRead() 
    if tick-sampletick >= timeout then
      new = nxt.InputGetStatus(port)
      print( tick .. " | " .. new )
      sampletick = tick
    end
  until( 8 == nxt.ButtonRead() )
  	
  repeat
  -- spin here until the button is released!
  until( 0 == nxt.ButtonRead() )
  	
end

-- Inititialize the gyro on port 1 to less sensitive mode and print
-- the samples

GyroInit(1,0)
GyroTest(1)

And here’s some sample output:

2279974 617
2279978 617
2279983 617
2279994 617
2279999 617
2280003 617
2280007 620
2280011 614
2280017 616
2280030 617
2280034 617
2280038 617
2280042 615
2280048 617
2280053 616
2280067 617
2280071 617
2280075 617
2280079 617
2280083 617

It’s pretty clear that the zero value should be close to 617, but there’s a potential problem later if we naively choose that as our zero point.

In our 20 samples from the first example, the value 617 shows up 75% of the time. Values greater than 617 show up once (620) and values less than 617 show up 4 times. The actual average for these samples is 617.8, so if we were to subtract 617 we’d end up tending to slightly positive results because we have not subtracted enough.

This isn’t too bad if we just want to know how fast we’re turning, but when we want to know how far we’ve turned, we’ll need to add all our samples over time. Using 617 as a zero means we’ll end up accumulating slightly more positive values, which means that our reading of how far we’ve turned will slowly drift even if we’re stationary.

Before we get too worried about this, we need to understand that the rate gyro will always drift a bit. After all, we’re talking about a $50 device, not a military grade sensor.

So what can we do to minimize the drift? Read on and find out.

Improving the Gyro Zero Point

One thing we could do is use a floating point number for the zero point and the average, but that is much slower than using fixed point numbers. And when we get around to integrating the rate to determine position, we’ll end up doing a lot of arithmetic operations, so floats will definitely be out.

A useful technique for getting a more accurate zero point is to simply keep the sum of many more samples than we need. For this test, we’ll use the weighted average of 128 samples for a zero point. The general case is shown in the figure below:

A naive approach based on this figure would be to keep a circular array of 128 elements and to periodically sum them, but of course this would be very slow and use up a lot of memory.

Another approach would be to calculate the total once, then subtract the value indexed by next and add the new value when it’s available. Either way, you need a big array, and that’s going to use a lot of memory.

The technique I’m using is really a sort of weighted average, and it produces a good estimate of the average. The main trick is to artificially add more bits of precision to our samples by multiplying our gyro samples by a constant, which I’ve chosen to be 64. Then we’ll “weight” the average by 128 so that noise in the samples does not affect the average too much.

What do I mean by weighting? Let’s do a little thought experiment to see how this works. Let’s say we have to figure out the level of a certain bacteria in a stream of running water. We want to find the average level of bacteria and ignore small changes in the level that occur from minute to minute.

Let’s also assume we have a beaker with 1 litre (1000 ml) of pure water in it. Think of this beaker as our averaging function. Here’s the neat part. We’re going to take 25 ml out of our averaging beaker and replace it with 25 ml of stream water.

At first, the ammount of bacteria in the averaging beaker will be very small compared with the bacteria in the stream. But as we take more and more samples, you can see that eventually the level of bacteria in the averaging beaker is very close to the long term average of bacteria in the stream.

Since the sample we’re replacing in the averaging beaker is 40 times smaller than the total volume, a sample that suddenly has twice the bacteria will not affect the average very much.

In the same way, if the output of our gyro sensor is a bit noisy, then this function will smooth out the noise and make our zero point much less sensitive.

Here’s the code that calculates (and periodicaly prints) the running average of the samples from the gyro sensor. In fact, we’ll be printing both the scaled integer and the equivalent floating point value so that you can see when the average stabilizes.

The gyro sensor is very sensitive, so hold it absolutely still when you determine the zero point. 

-- Read the Gyro sensor no faster than every 4 msec and calculate the
-- running average. Each sample is scaled by 64 and the average is
-- weighted by a factor of 128.

function GyroZero(port, scale, weight)
  local sampletick = 0
  local printtick  = 0
  local avg = 0
  local fweight = nxt.float( scale*weight )

  local tick, new
    
  repeat
    local tick = nxt.TimerRead() 

    -- update the average no faster than every 4 msec
    if tick-sampletick >= 4 then
      new = nxt.InputGetStatus(port)*scale
      avg = avg - (avg/weight) + new
      sampletick = tick
    end

    -- print the average no faster than every 256 msec
    if tick-printtick >= 256 then
      print( tick, avg, avg/fweight )
      printtick = tick
    end

  until( 8 == nxt.ButtonRead() )

  repeat
  -- spin here until the button is released!
  until( 0 == nxt.ButtonRead() )
  	
  return( avg )
end

-- Inititialize the gyro in port 1 to less sensitive mode and print
-- the running average until the orange button is pressed

GyroInit(1,0)
=GyroZero(1,64,128)

And here’s some sample output:

> =GyroZero(1,64,128)
130430  39488   4.820312
130686  1845131 225.235717
130942  3031650 370.074462
131198  3758822 458.840576
131454  4230305 516.394653
131710  4532917 553.334594
131966  4718822 576.028076
132222  4837786 590.550048
132478  4917269 600.252563
132734  4964312 605.995117
132990  4995417 609.792114
133246  5015514 612.245361
133502  5027208 613.672851
133758  5034958 614.618896
134014  5039344 615.154296
134270  5042426 615.530517
134526  5044153 615.741333
134782  5045634 615.922119
135038  5046739 616.057006
135294  5046977 616.086059
135550  5047888 616.197265
135806  5048128 616.226562
136062  5047896 616.198242
136318  5048005 616.211547
136574  5048054 616.217529
136830  5048699 616.296264
137086  5048668 616.292480

You can see that the zero point is stabilizes fairly quickly once the averaging algorithm starts. You can speed it up by “seeding” the zero point with a number that is closer to the typical output, like 620.

If I’m Turning, How Fast Am I Turning?

Now that we’ve got the zero point figured out, let’s use it to indicate how fast we’re turning. All we have to do is modify the GyroTest() routine a bit and subtract the zero point. We’ll pretty up the output a bit so that it’s useful for basic testing of the Gyro sensor operation…

-- Read the Gyro sensor no faster than every 32 msec and subtract the scaled
-- zero point returned by a previous call to GyroZero().

function GyroRead(port,scale,weight,zero)
  local sampletick = 0
  local fweight = nxt.float( scale*weight )
  
  local tick, new

  repeat
	tick = nxt.TimerRead() 

	-- take a new sample no faster than every 32 msec
	if tick-sampletick >= 32 then
	  new = nxt.InputGetStatus(port)*scale*weight - zero
      print( tick, new, new/fweight )
	  sampletick = tick
	end
	
  until( 8 == nxt.ButtonRead() )
  	
  repeat
  -- spin here until the button is released!
  until( 0 == nxt.ButtonRead() )
  	
end

-- Inititialize the gyro in port 1 to less sensitive mode and print
-- the running average until the orange button is pressed, then print
-- the zero compensated output until the orange button is pressed
--
-- Notice how the output of one function is part of the input to
-- the next one...

GyroInit(1,0)
GyroRead(1,64,128, GyroZero(1,64,128) )

The nice thing about this set of functions is that the result of the GyroZero() function is used as input to the rate GyroRead() function.

The next step is, of course, to figure out how far we’ve turned…

Calculating Angular Displacement with a Rate Gyro

The rate gyro sensor is used to tell us how fast we’re turning, not how far we’ve turned. Is it possible to use the rate information to figure out how far we’ve turned? The answer is yes!

Let’s do another little thought experiment – this time with a car that has a broken odometer. All you have is a speedometer, but you still need to know how far you’ve gone.

Every 15 to 60 seconds (the spacing does not really matter) you should call out the current speed and someone else writes down the exact time and the speed. Basically, you’re breaking down the trip into little segments of known duration and known speed at the beginning and end of each segment.

To figure out how far you’ve gone, all you need to do is a bit of math at each interval. You know the speed at the beginning and end of each interval, and you know how long the interval is, so just multiply the speed by the length of the interval and you know how far you’ve gone.

But wait, do you take the speed at the beginning or the end of the interval? If your speed is steady, it does not matter very much. One way to get a better result is to average the speed at the beginning and end of the interval and use that.

Here’s some sample code that determines the sum of the gyro readings. When you run the first part of the code, the NXT calculates the zero point. so keep the NXT absolutely still. Press the orange button to lock in the zero point and then press the gray button to zero the sum. Watch how the sum is printed once per second.

-- Read the Gyro sensor no faster than every 16 msec and subtract the scaled
-- zero point returned by a previous call to GyroZero(). Keep a running total
-- of the sum which is the current angular displacement

function GyroSum(port,scale,weight,zero,period)
     local tick = nxt.TimerRead()
     local printtick,sampletick = tick,tick
     local old, new, sum = 0,0,0
     local fweight = nxt.float( scale*weight*1000 )
	 
	 repeat
	   tick = nxt.TimerRead() 
   
	   -- take a new sample no faster than every period msec
	   if tick-sampletick >= period then
		 new = nxt.InputGetStatus(port)*scale*weight - zero
		 sum = sum + ((new+old)/2)*(tick-sampletick)
		 
		 old = new
		 sampletick = tick
	--     print( tick, new, diff, sum, sum/(scale*weight*1000), speed  )
	   end
	   
	   -- print the sum no faster than every 1000 msec
	   if tick-printtick >= 1000 then
		 print( sum, sum/fweight )
		 printtick = tick
       end

	   if 1 == nxt.ButtonRead() then
		 sum = 0  
	   end
	 
	 until( 8 == nxt.ButtonRead() )
   		   
	 repeat
	 -- spin here until the button is released!
	 until( 0 == nxt.ButtonRead() )
end

-- Inititialize the gyro in port 1 to less sensitive mode and print
-- the running average until the orange button is pressed, then print
-- the zero compensated output and total angular displacement until
-- the orange button is pressed
--
-- Notice how the output of one function is part of the input to
-- the next one...

GyroInit(1,0)
GyroSum(1,64,128, GyroZero(1,64,128), 32 )

And here’s some sample output:

-- Here's the output of the program after the zero point stabilizes. I'm turning
-- the sensor 90 degrees clockwise in a fixture and the indicated angle is -91
-- degrees. When I return the sensor to the original position the indicated
-- angle is back to the original -1 degrees

4981144      0.608049
3260429      0.398001
1979037      0.241581
3651252      0.445709
101220116   12.355970
652550725   79.657073
764644166   93.340354
707941286   86.418617
15886016     1.939210
-894571     -0.109200
510008       0.062256
1992112      0.243177

Experiment with the sample period to see where the tradeoffs are. Too many samples and you’ll accumulate errors quickly. Too few samples and you won’t get good results when the gyro sensor changes turn rates.

Balancing – The Ultimate Challenge!

Now that we have a way to figure out how fast we’re turning, and how far we’ve turned, we can do something pretty amazing – balance on two wheels.

Before we get too excited, this experiment only works fairly well. We’re bounded by the accumulated error in the gyro sensor readings. Eventually the drift angle causes the robot to drift forwards of backwards and then it just runs until it tips.

That being said, it’s still pretty cool. Here’s a video of the robot:

Conclusion

Hopefully this tutorial has given you some additional tricks in your programming toolbag. Review the bits on scaling and weighted averages for getting more precision out of integer math – it’s well worth the
extra time.

Permalink • No Comments »