HTWay – A Segway type robot

Introducing the HTWay, a Segway type self-balancing robot. This robot uses the HiTechnic Gyro Sensor as well as the HiTechnic IRReceiver. The Gyro sensor is used to maintain balance while the IR Receiver allows it to be controlled with a LEGO Power Functions remote.

Update(Nov 8, 2010):The NXC version was updated to fix a bug that caused the program to crash when the optimization level was set beyond 1.Update(July 28, 2010):The NXT-G Program in the download above has been updated to use the new 2.0 Gyro sensor block available for download from the Downloads page. If you are using the original 1.0 Gyro sensor block, you must update it to the new 2.0 block for use with this program.

Both the NXC and NXT-G programs work essentially the same way. The programs take advantage of floating point math that is new with LEGO Mindstorms NXT 2.0. The original 1.0 NXT only supported integer math. While using floating point math is not essential when programming a Segway type robot, it makes the program much easier to understand and to work with.

Update for NXT-G: For the NXT-G program, make sure you also download and install the Gyro and IR Receiver blocks from the LEGO Mindstorms software. You can find the them on the Mindstorms NXT-G Blocks downloads page.

If you usually program in NXT-G with LEGO Mindstorms 1.0 or 1.1, then this is a great opportunity to give NXC a try. If you install the development environment BricxCC, you will get NXC as part of the package. For the HTWay you will also need LEGO firmware 1.26 or higher. I recommend LEGO Firmware 1.29. This firmware is fully compatible with the earlier version and will still work with your LEGO Mindstorms 1.0 and 1.1 software. You can even use BricxCC to download the firmware to the NXT; from the Tools menu, select Download Firmware. You will also need to make the NXC compiler target the 2.0 firmware to take advantage of the floating point math. You can do that by going to Edit->Preferences, then click on the Compiler tab and then the NBC/NXC sub-tab. Now check the option for “NXT 2.0 compatible firmware”.

Update for NXC: In order to take advantage of the floating point math support you will also need to download and install the latest test release of BricxCC. After you download the zip file of the test release, copy the contents over your existing BricxCC installation. This will most likely be c:\Program Files\BricxCC.

When you run the program, the first thing it will do is let you select the wheel size that you are using. There are three options: Small (NXT 2.0), Medium (NXT 1.0), and Large (RCX). Use the arrow keys to choose and the Orange Enter button to select.

The robot will now need to get an initial gyro offset. You can think of this as a Gyro Sensor calibration. In order to get a good gyro offset, the robot will need to be copletely still. If you hold the robot in your hand it will wait until you you put the robot down, and it is not moving, before it will get the gyro offset and go on. HTWay will now start beeping indicating that you have five seconds to get the robot vertical and as balanced as possible. At the end of the long beep, let go.

The robot should now be balancing and is ready to be driven with the LEGO PF remote. You can control it like you would a tank. Both levers forward and the robot drives forward. Both lever back and it reverses. One lever only and it will turn around one stopped wheel and both levers in opposite directions and it will turn in place.

Background

This is a classic problem in control theory. Prior to the Segway Personal Transporter, it was more commonly known as the inverted pendulum problem. Normally when you think of a pendulum, like on a clock, it hangs down below the pivot point where it is stable; the inverted pendulum is one where the center of gravity is above the pivot point in a position that is inherently unstable. To keep it up, the pivot point has to move to catch it when it begins to fall. That is in essence the same problem as making a segway robot balance.

With LEGO robotics, this challenge has been taken up many times. First came Steve Hassenplug’s LegWay using the RCX and two EOPD sensors. These RCX EOPD sensor were early HiTechnic products that functioned essentially the same as the current NXT EOPD sensors. These sensor were used to tell the distance to the floor. If the robot was leaning forward, then the sensors would be closer to the floor so the RCX could tell that it was leaning forward. If it leaned back, then the sensors would be further away from the surface. This was a fantastic creation and Steve even got a place on cable TV fame with this amazing robot. Not only could it balance exceptionally well, it could line track and even spin in circles to impress the crowds.

With the NXT, Philippe E. Hurbain built the NXTWay using the NXT Light Sensor in a way similar to Steve Hassenplug’s LegWay. Under controlled conditions, the LEGO Light sensor can also be used to tell distance. As long as lighting and surface are consistent, the robot can tell if it is leaning forwards or backwards based on the light sensor value. Given the poor resolution of the light sensor, this was an impressive accomplishment.

The first gyro sensor based NXT Segway robot came from Ryo Watanabe at Waseda University in Japan. The original HiTechnic YouTube video of a balancing robot was in fact Ryo Watanabe’s very impressive robot that was using, at the time, a prototype HiTechnic Gyro Sensor. Ryo did an amazing job of describing the physics and his solution that were very valuable in the creation of the current HTWay model.

More recently Dave Parker at www.nxtprograms.com created an amazing NXT 2.0 one kit Segway with rider robot using the LEGO Color sensor in a similar way to Philo’s NXTWay. Dave Parker came up with the original idea of using the third motor to offset the balance and to it use to make the robot go forwards or backwards, in a way similar to the real Segway PT which is also controlled by the human rider leaning forwards or backwards. Very cool!

Laurens Valk has also recently published a Segway type robot that uses the HiTechnic Gyro Sensor. He calls his creation the AnyWay. Like the HTWay, his program is also written in NXT-G. Laurens’s project inspired some ideas that were used in the creation of the HTWay including the idea of letting the user choose wheel size with the button interface in the beginning of the program.

How it works

First of all, you don’t have to understand exactly how it works to make this robot. You can build it and put the NXC or NXT-G program on it and have fun with it even if you don’t fully get the math that makes it balance. Both the NXC and NXT-G programs are written such that the control code is separate from the balance code. If you want to use other sensors, such as the Ultrasonic or Light sensor, in addition to the Gyro sensor that is essential for balancing, you can do that. All you need to do is have your own control code which can in turn drive the robot by changing two global variables, motorControlDrive and motorControlSteer in NXC and controlDrive and controlSteer in NXT-G. Both of these variables are in degrees per second. The steering control is based on the desired difference in the motor encoders.

Above you will find the download links for both NXC and NXT-G programs. These programs have been written so that they work as much as possible the same way. Below I give some code snippets from the NXC program. If you are a NXT-G programmer, try to follow along. You can also look at the actual NXT-G code in the LEGO Mindstorms software and follow along there. Since some of the math equations get quite big in NXT-G, it may be easier to understand the NXC program.

In order to balance, the robot has a control loop which takes four pieces of information into account and then decides how much motor power is needed to stay upright. In a simplified form, the NXC code for the main balance loop looks like this:

Note that the real NXC program is a little more complicated because it takes a few more things into account such as driving, steering and the fact that the motor power needs to be limited to a +/- 100 range.
In this code fragment you can see that every time through the balance loop, four pieces of data are obtained: gyroSpeed, gyroAngle, motorSpeed, and motorPos. These are the state variables that describe what is currently happening to the robot.

gyroSpeed

is from the Gyro Sensor and is the angular velocity of the robot. If the robot is in the middle of falling forward, then this value will be positive. The units are approximately in degrees per second.

gyroAngle

is the angle of the robot. This value is positive if the robot is leaning forwards, and negative if leaning backwards. (This is not exactly true as will be explained later) The units for gyroAngle is degrees.

motorPos

is the position of the robot in degrees of the motors. For the HTWay, this is actually the sum of the two motor encoders. This is the term that keeps the robot in a particular place.

motorSpeed

is the speed of the motors. This number is also in degrees/second and is also based on the sum of the two motor encoders. This term is what keeps the robot from excessively oscillating back and forth and effectivly slows it down.

To calculate the power these variables are multiplied by respective constants in a four term linear equation, the result being the power needed for the robot to stay balanced. The trick to making this all work, is finding the right constants.

To give you an idea of the role of each term in the balance equation, I will talk about each term one at a time with an attempt to isolate what each one does. First of all, imagaine that the robot is perfectly balanced and exactly at the desired target position. In that case, all four of the variables will be zero. In other words, the robot is perfecty vertical so the gyroAngle is zero, the robot is not falling forwards or backwards, the robot is not moving and it is exactly in the desired position. Since all four variables are zero, the result of the power equation is also zero.

So what if the all the terms are zero except that the robot is leaning forwards, for example the gyroAngle is 5 degrees. What to do? Well if the robot is leaning forwards, then it is necessary to drive forwards to attempt to catch the robot. That is is the role of the KGYROANGLE constant. When multiplied with the gyroAngle, it will give the power needed to drive forwards to catch the fall to regain balance.

Again, imagine all the terms are zero except that this time the gyroSpeed is positive, perhaps it is 10 degrees/second. So the robot is upright and not moving, but it has somehow gotten itself into a situation where it is falling forwards. In a way you can think of this as a head’s up that things are going astray. Even though it is upright now, it is about to be leaning forwards. This term lets the robot respond even before it falls forward. It also plays a role when the robot is leaning but is on its way to becoming upright, in that case this term will prevent the gyroAngle term from making the robot respond when in fact its okay.

So what about the two motor terms? Well if the robot is perfectly upright and not falling but the robot is 100 degrees further forward then desired, in that case motorPos will be 100. Remember, motorPos in the HTWay program is the sum of the two motor encoders. The actual distance of how far forward it is will also depend on the wheel size. While it would be nice to just drive back to the zero position, that doesn’t work. If you attempt to just drive backwards back to the zero position, then the robot would actually fall forwards. The solution is actually to drive forwards, because in order to go backwards, you first have to get the robot to fall backwards. To do that, you drive forwards!

The motorSpeed term works in sort of the the same way as motorPos. If the robot is in the middle of driving forwards, then first of all you need at least enough power to maintain the speed, and thus the balance, and then you even need a little more to get the robot to lean backwards to make it slow down.

In the NXC program, the four constants are set to these values near the top of the program:

A note about wheel size making it go
In the HTWay program you can select the wheel size you are using with a three button interface. What this does is it sets a global variable called ratioWheel to either 0.8, 1.0, or 1.4 for respectively the small NXT 2.0 wheels, the medium sized NXT 1.0 wheels and or the large RCX wheels. So what does this actually do? Well the actual balance equation is a little different then what was shown above, here is the full expression used in the program:

It turns out that the wheel size only needs to play a role with the two gyro sensor terms and not the motor terms. The reason that wheel size is important is because bigger wheels need less power to compensate for being out of balance. Since bigger wheels move further, given a certain amount of input, you need less of it to achieve the same amount of movement. So why not take the wheel size into account on the motor terms? The reason is that these terms are effectively self-relative. If the robot is, for example, one inch too far forwards, then this will be represented by a higher motorPos value for small wheels than for big wheels. Effectively, for the small wheeled robot this will result in a higher influence on the motor power than what will happen for the same distance using the bigger wheels.

In the code above you will also see that the motorControlDrive also plays a role in the balance equation, this is not actually what drives the robot. This term is used to help get the robot going as well as to slow it down whenever the motorControlDrive term changes. When you start driving the robot, this term will cause the robot to first drive backwards a little to quickly get going, then when you stop it will give the robot a little extra boost to help it lean back to slow down. The actual driving comes from this line right above the power balance equation:

motorPos -= motorControlDrive * tInterval;

Every time through the loop, the motorPos variable is adjusted proportionally to the global variable motorControlDrive. motorControlDrive is in degrees/second so by multiplying it by the interval time, we are adjusting the motorPos by the amount that the robot should move each time through the control loop. This moves the target position along which is what makes the robot drive.

Integration
If you are a young robot builder, you have probably never heard of integration. If you are older, you probably wish you never had. It turns out that integration is really useful and essential for this project. And actually not that hard to understand.

The problem is that the Gyro Sensor does not give you an actual angle. You can’t just read the Gyro Sensor and tell if the robot is leaning forwards or backwards. All you can tell is the angular velocity, in other words, how fast it is falling. So if you know the angular velocity, how can you get an angle? Well that’s what integration does.

Integration is simply the act of adding an infinite series of values over time. Let’s say that at a given time you know the robot angle, we keep that in a variable called gyroAngle. Each time through the loop we get from the Gyro Sensor the angular velocity in degrees/second. So if we know the interval of the loop, then we can update our gyroAngle by the amount that we know the angle has changed. Since our loop is running at about 100 times per second, the interval time is 0.01 seconds. To update the gyroAngle, we simply add the gyroSpeed times the interval time (0.01 seconds) into the gyroAngle to get a new value. That’s it!

This function also takes care of one more piece of housekeeping that we should discuss: the gyro offset. Because the technology of the gyro sensor element, the raw sensor value is likely to be non-zero even when the sensor has no actual angular velocity. Since we need a zero value when the sensor is not turning, we need to maintain a gyro offset value that we can use to adjust the sensor value to get an accurate angular velocity.

To maintain a gyro offset the program does two things. Before the robot starts to balance it gets an initial gyro offset by averaging 100 samples of the sensor value while the robot is lying on the ground. But this value will just be the initial gyro offset. While the robot is driving we will also need to constantly adjust this offset value in order to keep the value from drifting with time (and as a result the whole robot will drift in position if not corrected). In the HTWay program, this is done by maintaining a long term average known as Exponential Moving Average. Since the long term average of the angular velocity, assuming the robot is balancing, should be zero, the long term average of the sensor value can be used as the gyro offset.

In the code above, EMAOFFSET is very small, 0.0005, so even if the robot is moving back and forth while balancing, it will not have a large immediate effect on the gOffset value. Only if the offset is off over a significant period of time will the offset value get noticeable changed. This way of using an exponentially moving average is also known as a low-pass filter.

gyroAngle and motorPos… not exactly zero
When the robot starts balancing, the program assumes the robot is vertical. Well, even if you are really good when you let go of the robot, this may not be true. You are likely to be off by a degree or two. Also, even though the program maintains a gyro offset and is constantly integrating the angular velocity to maintain gyroAngle, this may not be perfect and the gyroAngle may still drift a bit with time. Sounds like a disaster, no? It turns out to not be a problem. The reality is that when you are not driving and the robot is gently oscillating back and forth maintaining balance, the gyroAngle and motorPos can be proportionally away from zero. The reason it works is because the two terms have reached an equilibrium where one offsets the other. For example, the robot may be nicely balanced around a gyroAngle value of 1 degree, to compensate, motorPos may be around -107 degrees. When these two values are multiplied by their respective constants in the balance equation, they will cancel each other out.

References

Ryo Watanabe did an excellent job of explaining both the physics of the problem and the linear equation that makes a solution possible on the NXT. Here is the overview.

Another good website with information about LEGO Segways programming is www.techbricks.nl.

Acknowledgments

I would like to thank Laurens Valk and Xander Soldaat who helped me with this project. Xander reviewed the building instruction and tested the robot. Laurens and I discussed various aspects of the program which influenced some features such as how the gyro offset is maintained as well as the feature of allowing the user to select the wheel size at the beginning of the program.

This entry was posted
on Friday, June 11th, 2010 at 4:30 pm and is filed under Gyro Sensor.
You can follow any responses to this entry through the RSS 2.0 feed.
You can leave a response, or trackback from your own site.

97 Responses to “HTWay – A Segway type robot”

Hi Gus,
I have bought a gyro sensor to build your “magic” htway robot, and I’d like to program it with LabView. It’s a little bit difficult for me.
Could you send me the LV version of the hway to help me ?
Thanks for all your work !

The controller takes four things into account to balance the robot: the gyro rate (the rate of the fall in deg/sec), the angle (the integral of the rate), the motor speed, and the motor position. These four terms, along with some additional processing to handle driving and steering, are then use to set a proportional power amount to the motors.

Hi sorry, for the slow response. Things have been pretty busy lately.
You can use the acceleration sensor along with trig functions to get a tilt angle. The problem with that is that it only works when the sensor is stationary. If you move the sensor around then you also get acceleration due to movement and then you can’t reliably get an angle. In the worst case, a sensor mounted on a falling robot will essentially be falling due to gravity. In this case it will not detect anything is wrong until it hits the ground. You need a gyro sensor to make a balancing robot. I have never seen it done with an acceleration sensor.

Gus
I too purchased a gyro sensor and IR receiver from your company. I’d like to make a “Segway” LEGO robot, but I’m doing my programming with RobotC.
I was at the Robotics Academy and asked the experts about this but such a project they couldn’t provide much help.
Can you point me in the right direction? Do you have a RobotC version of your program?
Thanks

I don’t have a RobotC version but I know that RobotC versions exist somewhere on the net. I suggest that you take the NXC version that is also available from this blog post and convert it to RobotC. These languages are a little different but this should be doable.

Hi,
I`m looking for the exact mathematical model of this lego segway, can sombady help me? I found several models, in state space representation,
but those models output is the torch not the speed and the input is velocity, acceleration of wheel, angular velocity, angular acceleration of pendulum
and not the position, velocity of the wheel and angle, angular velocity of the pendulum.
Thank you in advance.

Hi there. Ive been struggling to modify the nxc code for this HTWay. Even unmodified code doesnt work. I run the downloaded file on BricxCC, compile and download just fine.. but then, When I ran the program on NXT, after the beeps, it says File Error!. Please advice. I’ve followed the instructions carefully. Latest test release, check nxt 2.0 checkbox and all that stuff. Thanks

I suspect that you don’t have the IRReceiver connected like the program assumes. If you don’t have the IRReceiver sensor, then comment out the code that initializes the sensor and attempts to read it. The robot should balance and stay in place if you remove this code. If you want to make the robot drive and turn, then you can replace this task with your own code that sets the same global variables used by the control task to drive and steer the robot.

Send me an email to support at hitechnic dot com and I will find the LabVIEW version I have of the HTWay. I have a LV 2009 version but I still need to make a new LV for LEGO Mindstorms version of the program.

No, the “Tilt” sensor is an Accelerometer. The Accelerometer can measure tilt because gravity is perceived as acceleration but this only works if the sensor is not moving. Since the HTWay is constantly in motion, this sensor can not be used to maintain balance. For the HTWay, you need the Gyro Sensor. The Gyro sensor gives a value based on angular velocity but is not otherwise affected by movement of the robot.

What is advantage of gyro sensor vs eopd? i am just starting a LegWay project and it would seem to me easier to translate the change in distance, as sensed by EOPD, into a speed/power level for motor then to deal with the gyro sensor. I am using an older RCX kit (with the HiTech NXT-EOPD)…will this work? (I have converson cable) Do I need two EOPDs (ala Hassenplug)? Should forget this and just go gyro? What are your thoughts? Thanks.

Verdonko, first of all, the NXT EOPD and Gyro sensors will not work with the older LEGO RCX, even with a conversion cable. The cable allows the RCX sensors and motors to be used with the NXT but not the other way around. (Actually the NXT motors will work).
An NXT LegWay type robot that uses the EOPD sensors could be build similar to what Hassenplug did with the RCX. The problem with the EOPD sensor is that it will be sensitive to the nature of the surface that it is running on. If you try to drive the robot on different color surfaces or something like carpet then it will either not work or require re-calibration to that surface. Some surfaces will not work at all such as a dark mat which absorbs almost all the light. It will also not handle slopes since being balanced requires a different angle to the surface when you are on a slope.

Hi,
I also try to build a self-balacing robot like the HTWay. I’m using LabView 2009. But I have some problems with the angle(drifting) and how it works with the motor position. Can you help me with a LabView version of the HTway?

I’ve built your HTWay carefully and it seems to work, up to some extent.
It balances, it turns, but as long as I push both PF remote levers in the same direction (forward or backwards), the robot starts moving forward or backwards but almost immediately it looses the balance and falls to the front or to the back respectively. Nothing like you demonstrate on your video.
I’m using NXT firmware 1.31, the correct wheel sizes, etc.
I experience the same problem, both with NXT-G and NXC.
My NXC Compiler is targeting 2.0 firmware and optimizations levels is 2.

Do you have any idea, about what might be going wrong with my robot?
Many thanks, in advance.

Reading trough all comments, it turned out that I was using also my rechargeable battery, at too low level. 7,4V despite the bar indicator was still above half way.
After fully charging (now 8,3V) it works perfectly!

Hi,
I have builded this HTWay with the Gyro sensor, but now I dont know how to continue to program it with LabVIEW (all my tests and codes doesn’t was concluant). Is there a fuctional (*.vi) example and documentation/theory alivable ?
Thanks

Fred, Sorry, I had the LV version of the HTWay up on the site and then it disappeared with some website server updates. I have made it available again and you can find it on the HiTechnic LabVIEW downloads page. Here is the direct link to the download: http://www.hitechnic.com/file.php?f=262-HTWayLV2009.zip

I suggest that you do some simple experiments to determine the center of mass. All you have to do is let the robot hang freely various points on the robot. From every point that you let the robot hang, the center of mass will always be directly underneath.

I have just uploaded a LabVIEW 2010 version of the HTWay program. This program supports both remote controlled via the LEGO PF Remote (and HiTechnic IRReceiver) as well as bluetooth controlled from another NXT. You can find the download here:

controldrive is the speed setting for the robot is degrees/second (measured by the sum of the two motors.) controlsteer is the rate of turning of the robot (the difference of the motors), also in degrees per second. To line track you want to set the controldrive to a fixed amount (I would suggest starting slow, like 100 or 200 deg/second. Then in the control loop you want to look at the light sensor value, subtract a mid point constant (the value half way between white and black) and then multiply that by a constant to scale it up to something that will get it in the range of about +- 200, then set that to the controlsteer. The robot should now steer toward the edge of the line. Actually, you should start with a controldrive speed of 0 and just get the robot to steer toward the line in place first. Once that works, make the robot move along the edge of the line.

The sensor should be slightly in front of the robot. You will need to experiment on the best placement. If too far forward then the forward/backward movement of the robot will results in a lot of value variation. If too close to the center line of the robot then not enough look ahead to track the line.

If you have purchase the IRReceiver sensor, as well as the Gyro sensor, then the 8879 remote should work. This remote gives variable power to the left and right more so may be a little more difficult to operate the robot than the simpler 8885 remote which simple has two levers (my personal favorite). Have you built the HTWay with both the IR Receiver and the Gyro sensors and not able to make it work? Are you getting an error? Have you downloaded and installed the IRReceiver and Gyro Sensor blocks? If you can’t get this to work I suggest you contact me using the support form on the HiTechnic website.

You can find LV versions of the HTWay on the LabVIEW downloads page. This version is not a line follower but you can create that by replacing the control loop with your own code that uses the light sensor to determine the steering setting. I suggest you start with a very slow drive speed so you can work out the proper steering code to make the robot stay on the line.

i did the same thing as u said. but my robot wont follow the line until it felt down. any advice with that problem ? my line follower code starts with getting the midpoint from the sensor, set that value greater than the value that the sensor is reading, and then create a case structure: if true turn right, false turn left. everything in a white loop.
thank you

Hi Gus,
I have bought a gyro sensor and the IR sensor to build your htway robot, and I’d like to program it with NXC. But I think, taht your programm has a bug!
When I download your NXC Programm it starts but the wheel selection is not running. I hear some sound ticks and the NXT hang up.
The NXT-G Programm worked very well, but I want to do it with the NXC – programm and the Brixc.
Thank you for your help

First of all, I’m not sure what the problem is but I seem to recall there may be an issue caused by the optimization level uses by the NXC compiler. Using the Edit > Preferences dialog, change the optimization level used under Compiler > NBC/NXC to level 1. That may do the trick.

Leave a Reply

Lightbricks, Hitechnic are registered trademarks of Dataport Systems, Inc.
All product specifications and prices are subject to change at any time.
LEGO, MINDSTORMS, Robotics Invention System, RCX are trademarks of the LEGO Group.