Step 10: A Very Flattering Filter.

To combine the two sensor readings together in a way that produces the best angle estimate, a special technique called a complementary filter is used. (No, not a complimentary filter, and definitely not a Kalman filter.) The filter is really just a single line of code:

angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;

Okay, so it's a long line. You can also do it with an op-amp or two.

This filter does exactly what is necessary in this scenario: it favors the gyroscope reading for short time durations and the accelerometer average reading for long time durations. Let's break it down:

A is the factor that determines the cutoff time for trusting the gyro and filtering in the accelerometer. It's always between 0 and 1, usually close to 1. In this case, A is defined to be 0.962. This means that at every time step, 96.2% of the new angle measurement comes from the old angle measurement plus the integrated gyro measurement. The remaining 3.8% comes from the accelerometer. This slowly averages in the accelerometer over many time steps.

DT is the time in seconds between program loops, the time step. Here it is defined to be 0.020 and is set by delay(20) at the end of the loop. The code in the loop itself take much less than 20ms, so the delay dominates the time step.

rate is the gyro reading, converted to degrees per second.
accel_raw*A_GAIN is the accelerometer reading, converted to degrees. It is very important that these two be in the same unit base before adding together. (You can't add apples to oranges.)

The time constant of the filter is the duration of time at which the gyroscope reading starts to be filtered out heavily and the accelerometer reading starts to be averaged in heavily. It's actually a continuous process, but the time constant is a single measure of where the balance begins to shift. The time constant is:

tau = DT*(A)/(1-A) = 0.5s

So, for this filter, the gyro is trusted for about 0.5 seconds and the accelerometer start to average in significantly after that. This value can be tweaked by changing A.
<p>Nice work! Thanks for sharing</p>
<p>great and thanx for share...</p>
<p>Nice one...</p>
<p>Awesome work</p>
<p>Great One</p>
Great project
<p>Awesome job.</p>
<p>Wow...Its really awesome</p>
<p>Qu&eacute; buena idea! Cool!!</p>
<p>I'd love one of these around our campus.</p>
<p>Great idea</p>
<p>lovely, creative</p>
amazing.i am going follow your instructions to make one.but i know nothing about programing and electrions.so dont think my questions are simple and stupid.i really dont know.<br>1.Can i use different motor driver?<br>2.Can alter the structures of the car(eg distance between two wheels,size of standing board....)<br>if these changes will effect the program to run smoothly?<br>thanks
answer, please! what is the function of steering pot? How it affects on the controlling of ballancing robot?
The steering pot applies a differential command to the motors, after all the balancing calculations. It adds some amount to one motor and subtracts some amount from the other motor, which causes the platform to turn. The amount it adds and subtracts is set by the steering gain, KS.
<p>Just a note to let you know I have added this to the collection: Cordless Drills Hacking for Other Uses !</p><p>&gt;&gt; <a href="https://www.instructables.com/id/Cordless-Drills-Hacking-for-Other-Uses/" rel="nofollow">https://www.instructables.com/id/Cordless-Drills-Hacking-for-Other-Uses/</a></p><p>Take a look at a bunch of project involving odd uses of drills.</p><p>and for even more drill info</p><p>&gt;&gt; <a href="https://www.instructables.com/id/Cordless-Drills-A-Collection-of-Collections/" rel="nofollow">https://www.instructables.com/id/Cordless-Drills-A-Collection-of-Collections/</a></p>
<p>Regarding Step 10,</p><p>I've seen accelerometers with specs that say 'capable of measuring accelerations with output data rates from 1 Hz to 10 kHz'.</p><p>If the motion will be under 10khz, do you think a gyro is still needed? Or will the readings be so noisy that even when operating under 10khz, the accelerometer data will still need to be filtered with gyro data?</p>
<p>awesome stuff.. thank you</p>
<p>great stuff</p>
<p>You Sir, are a legend.</p>
<p>sure thing</p>
<p>awesome stuff.. cheers</p>
<p>This Segstick really very good?</p><p>I want it too...))</p>
<p>I understand that the point of this build is the balancing part... but I'm interested in everything but that part. </p><p>My son has muscular dystrophy and is in a wheelchair full time. Visiting other peoples houses can be tough because the wheelchair is so big and heavy. I've been looking for a solution for a smaller footprint chair to bring into a home and I think this is it.</p><p>I'd keep the platform/drill/wheels, but skip the stick part and replace that with a joystick. Seems to me like it shouldn't be hard to have a joystick as input and have the arduino calculate the values for the motor drivers. (Not that I have any idea how to do it -- but it seems a lot simpler than making it self-balancing)</p><p>Here's an idea of the chair that I would mount the platform on...</p><p><a href="http://jenmadeit.com/2014/02/14/scooter-to-chair-upgrade/" rel="nofollow">http://jenmadeit.com/2014/02/14/scooter-to-chair-upgrade/</a></p><p>Do you think this is a possibility? I have no soldering or electrical experience. What are the odds that you could help me build it???</p>
Wow this is crazy! I could never build this, but my husband is really handy, so I might have to get him to take a look.
Can you please tell me where I can find the schematic for the controller board. Machine Science is asking for BOM + schematic + CPL. I could not find those. <br> <br>Thanks <br>-r
The controller on this project is different from the one on the &quot;DIY Segway&quot; project, which was based on a Machine Science board. This one is simply and Arduino Nano (manufactured by Gravitech, sold by many distributors) connected to sensors as in Step 7. Unfortunately, it seems that Sparkfun has discontinued the 6DOF Razor IMU in favor of newer, cheaper digital versions. I haven't converted this project over to digital sensors yet, so you'd have to adapt it on your own. <br> <br>Hope that helps!
Thanks Scolton for your reply. Do you think using the newer sensor to be a challenge? I ask this since I have no experience what so ever working with electronic boards and sensors. Was the older sensor an analog one? And does the new one still connect to the same ports as the older one?
How did you calculate the angle from the accelerometer? none of my values from several different methods seem to work very well, or line up with the angle from the gyro.
Steps 9 and 10 have all the details for how the accelerometer and gyro are merged together to create an angle estimate. I also have it in Steps 14 and 15 of my PCB Quadrotor instructable for a different IMU. <br> <br>For the accelerometer, I use a small-angle approximation that says sin(theta) ~ theta, in radians. This is good to within 5% between -30&Acirc;&ordm; and 30&Acirc;&ordm;. So, to get the angle you need to subtract a &quot;zero&quot; value and then scale by the correct constant. <br> <br>If you need to work outside of -30&Acirc;&ordm; to 30&Acirc;&ordm;, the most common method is to take the arctan of two accelerometer axes.
hi scolton.<br>here i got 2 18x25 pololu motor driver but the pins are<br>TIXN<br>ERR<br>RST<br>TX<br>RX<br>VIN<br>GND<br><br>instead of<br>V+<br>5V<br>FF2<br>FF1<br>RESET<br>PWM<br>DIR<br>as shown in your 18x25 motor driver<br><br>so plz tell me what r the PWM &amp; DIR pin in my motor driver.<br>
Seems like you got the 18v25 Motor Controller instead of the 18v25 Motor Driver. Pololu separates &quot;drivers&quot; which have no on-board logic from &quot;controllers&quot; which have an on-board microcontroller. Drivers take a PWM and direction input and directly send it to the motor. Controllers need a different kind of interface. <br> <br>On the controller, you should be able to use the RC interface (pins labeled RC1 and RC2) and the Arduino servo library to command the motors.
Where did you get the heat sinks for the Polulu High-Power 18v25 motor drivers and how did you attach them? <br> <br>A picture would be nice... <br> <br>thanks
They came from a scrap bin, but any old heat sink should work. A processor heat sink might be the right size (but overkill).<br><br>To attach, I used screws from the board into the heat sink. I put a couple of layers of Kapton tape between the board and the sink to insulate it.
Thanks Shane-<br><br>after a little bit of thought, I am thinking of taking the fan off an old PC CPU heat-sink and just suspending it with 4 standoffs above both Pololu controllers but without a heat sink or mechanical contact point. This should be better than nothing I hope...
Another question for you: <br> <br>Your schematic shows A0 - A5 of the arduino connected to the Razor 6D0F but your code only uses A0 for the accelerometer input and A4 for the gyro input. <br> <br>Is it necessary to connect the other Razor 6D0F outputs (A1 - A3 &amp; A5) to the arduino and if so why? <br> <br>(I might want to use those analog inputs for other things...) <br> <br>thanks
No, it's not necessary to connect the other four IMU outputs to the Arduino. I connected them since they were convenient and because I didn't know which way I was going to mount the board. But you only need one accelerometer axis and one rate axis.
Hey Shane,<br><br>I'm working on making a small robot balance, you can see my first attempt in this blog post:<br>http://makerandomstuff.blogspot.com/2011/12/introduction-fail-way-crappy-balancing.html <br>I've tweaked the platform a bit to make things neater but I'm still having trouble balancing.<br><br>The sensors from the Wii Motion Plus (gyros) and Wii Nunchuck (accelerometers) are outputting data over I2C and I'm filtering it to get g_roll, a_roll, and a filtered angle, so I think that part's good.<br><br>When I try to have my robot balance, it either oscillates a lot, or responds too slowly. I think the problem lies in my Kp/Kd values, my motor responsiveness, or my robot's small size (or a combination of 2 or more).<br><br>Any suggestions?
Cool project! It sounds like you're running up against what I call the &quot;robot segway&quot; problem. It's the reason I said in the Instructable that I think building a robot segway is actually harder than building a full-size rideable segway. You're right that the small size makes it harder to control (faster time constant, like balancing a pencil on your finger as opposed to a broom). But there's something else even more fundamental that makes it hard:<br><br>The sensors are inertial, meaning they respond to acceleration. They can't distinguish between standing still and moving with a constant velocity. So, even when tuned properly, the robot segway can't really stand still by itself. It will either oscillate (if the gains are high) or stay near vertical but drift off in one direction until the motors can't keep up, at which point it falls over.<br><br>A human rider can sense that the segway is drifting off in one direction and correct by leaning forwards or backwards to bring it back to zero speed. To mimic this, the controller would need to know the velocity (maybe from wheel encoders) and control it to be zero. This adds extra states to the control system and makes it more complicated. I attempted an autonomous balancing robot with no encoders with moderate success here:<br><br>http://scolton.blogspot.com/2011/05/mini-segstick-my-mid-quals-crisis-robot.html<br><br>Others have definitely done it with success. There are a few links to robot segways in the last step of the Instructable. Another option is to use radio control to guide it as a human rider would, preventing it from building up too much velocity when it should be standing still. Here's an example of that solution in action:<br><br>http://machinescience.wordpress.com/2011/11/29/ps2-module-controls-two-wheeled-balancing-robot/
Drifting problems aside, I still haven't been able to get it to balance like some of the other robot segways I've seen. Staying upright for at least 30 seconds is doable, right?<br><br>I've gotten my hands on a cheap nunchuck clone so I'll be updating the sensor system soon, and maybe making things a bit more solid. <br><br>I also have some motors with quadrature encoders built in (9v LEGO NXT motors) so maybe I'll go somewhere with that. Is there any chance I could take a look at the code you used for your mini-segstick? Maybe I can integrate encoders with that strategy.
Hello Scolton,<br><br>I still can not stabilize my Segway. Also, I had a problem with my IMU and the gyro stopped working, so I had to use another one. The gyro I had at hand is an Epson XV-3500 (http://www.epson-electronics.de/upload/PresidioIndustries/product/QuartzCrystalDevices/Sensors/GyroSensor/XV-3500CB_710020500_data.pdf) whose sensitivity is 0.67 mV / deg / sec. My accelerometer is the ADXL335. I'm supplying 3.3V.<br><br>The calculation bellow is correct?<br><br>Accelerometer (ADXL-335):<br>(57.3 &deg; / 0.3V) * (5V/1024LSB) = 0.932 &deg; / LSB<br><br>Gyroscope (XV-3500):<br>[(1 / s) / 0.00067V] * (5V/1024LSB) = 7.287 (&deg; / s) / LSB (?????)<br><br>The value for the gyroscope seems very high and the Segway completly unstable.<br><br>Kind regards,<br>Marcos
Am I correctly reading the datasheet for the gyro that it has:<br><br>Sensitivity: 0.67mV/(deg/s)<br>Range: +/-100(deg/s)<br><br>and that thus the voltage range is only +/-60.7mV, full scale? <br><br>If so, this sensitivity may not be suitable for use with the Arduino's ADC (at 5V or 3.3V reference). The signal might be just too small to get useful information out of. Even the Sparkfun Razer IMU gyro is cutting it close in terms of the available resolution, given the low angular rates typical on a balancing platform. (Under normal operation, 10-20(deg/s) might be the maximum rate.)
Scolton, first of all thanks for your paper. I think everyone should read, it's really enlightening.<br><br>I think not because this project (http://diysegway.blogspot.com/) uses the same sensor and works very well (really stable). I would like to test the Trevor algoritm. The angle and the angle_rate are the tilt angle of the scooter in radians and its derivative in radians/sec. Is it the same that are in your code (rate and angle)? So, can I use the bellow code together your code? Are in same unit system?<br><br>Thanks for your help, I'm learning a lot!<br><br>balance_torque = 5.0 * (angle - rest_angle) + 0.4 * angle_rate;<br><br>overspeed = max(0, cur_speed - 0.5) ;<br>if (overspeed &gt; 0) { <br> overspeed_integral = min(0.4, overspeed_integral + min(0.2, overspeed+0.05) * dt) ;<br>} <br>else { <br> overspeed_integral = max(0, overspeed_integral - 0.04*dt);<br>} <br>rest_angle = 0.4*overspeed + 0.7*overspeed_integral;<br><br>steer_cmd = 0.07/(0.3+abs(cur_speed)) * steer_knob;<br>cur_speed += 1.2 * balance_torque * dt;<br>left_motor_pwm = balance_torque + cur_speed + steer_cmd;<br>right_motor_pwm = balance_torque + cur_speed - steer_cmd;
I have been having problems making mine stable also but just stumbled across something yesterday in another tutorial about building an arduino segway that I plan to investigate. <br> <br>In that application, they are using a sparkfun razor gyro &amp; accelerometer which is connected to the 3.3v just as it is in Scolton's instructible but what is different is they tie the 3.3v line on the arduino to the Aref pin an then use the following command in the setup section of the code: <br> <br>analogreference(EXTERNAL); <br> <br>Here is the explanation: <br>the normal range for the 10 bit ADC (pins A0-A5) is 0 - 1023, where 0vDC input yields a value of 0 and 5vDC yields a value of 1023. If you plug a 3.3v sensor into an arduino analog input, the maximum value that an arduino can read is approximately 675 or so. (3.3v / 5v *1023), to get full range using a 3.3v device, you must connect the desied reference volage (3,3v) to the analog reference pin (aref) an add a line of code to the end of the setup() function to tell the arduino to use an external analog reference voltage. <br> <br> <br>I haven't tried it yet but this might smotthen things out as mine has been vey sporatic... <br> <br>If you get a chance to try this before I do, please report back waht you find. <br> <br>thanks
What are you talking to me sounds correct, but I think it is not the solution to problems. I think I've seen most of the tutorials found on the internet and I found more stable until today was one of the first to be created: the Trevor's algoritm. He did several tests, both in high and low speed. I'm thinking of rewriting the code from it to a version compatible with the Arduino, but it will not be easy. The code it is large and somewhat complex. Would you be willing to help?<br><br>My MSN is marquinho-a [at] hotmail [dot] com, and my Gtalk is marcospassos.com [at] gmail [dot] com. You can add me and talk more to share experiences.<br><br>Kind regards,<br>Marcos
Hello Scolton,<br><br>I'm using a Sparkfun 5 DOF, OSMC controller and two 450w 24v motors (MY1018z). Although my Segway can balance itself, it can't balance a person (seems to have no force, slowly). I've noticed my Gyro (IDG-500) has <br>2 mV/&deg;/s of sensivity, so my LSB is [(1&ordm;/s)/0.002V]*(5V/1024LSB) = 2.4140625(&ordm;/s)/LSB, right? Anyway, it can't balance as well. <br><br>I also have noticed that over time the platform starts to get steep and considers it as level.<br><br>Do you have any tip?<br><br>Thanks,<br>Marcos

About This Instructable




More by scolton:PCB Quadrotor (Brushless) Seg...stick. 
Add instructable to: