Balancing Robot, Gyro/Accel, Complementary Filter Sanity Check

Hi All,

My name is Brian, I’m new to the forum. I’m wrapping up my Mechanical Engineering Degree in the fall and, as a senior project, I’m trying to duplicate some of the great ball balancing robot success like that of Masaaki Kumagai at Tohoku Gakuin University. Perhaps a tall order but I dove in a few months back with a mechanical prototype and I’m starting to write code to control it. I’m new to embedded microcontrollers and programming in general but I’m learning fast and thought that if I posted my work here maybe some folks with more experience could look it over and feed back. I’m mostly wondering about blatant efficiency killers, if I’m doing a decent job defining variables logically, and if I’m implementing the complementary filter correctly.

I’m using an Arduino Mega 2560, a chipkit Pmod Gyro and Accellerometer (using the 3.3vdc supply) and 2 easy driver 4.4s with Soyo steppers from The steppers are a little weak but they’ll be good enough for a proof of concept. More importantly I’ve attached my code, you can see I’m using the Love Electronics library for the Accelerometer and an L3G4200D library for the Gyro that I think started at Pololu. I use snippets of code from each of their example files to pull raw data from the sensors in the beginning of the program.

The program starts, takes 5 samples from each axis over 1 second, and averages them and creates a variable for calibration (The intent is to start the robot on a stable, level surface). The calibration value is subtracted from each axis to zero out the offset of the sensor at startup and/or account for any tilt in the assembly (the value is actually added to the Z axis of the accelerometer to make it read a clean 1.0g when vertical). After the calibration value is generated I start a loop of 100 measurements that sample the accelerometer, apply the cal values, invert the Y axis to match to the Gyro and calculate an angle of pitch using all 3 axis using a set of formulas from The output of those formulas should be in radians/second. I then sample the gyroscope (which I believe outputs degrees/second) and apply a conversion factor to the values to get them into radians/second also. Lastly I use a the millis() function to measure the loop time and use that as my sample rate in a complementary filter modeled after the MIT article ( Sprinkled in there are some lines to print values into the serial bus for troubleshooting, I think this is really what drives the loop time up as changing the baud rate of the serial bus has a huge impact (240ms to 64ms by changing form 9600 to 19200). What’s weird is that the output of my filter reads 0 on both the X and Y axis when level and goes for 1.0 to -1.0 as I rotate it ± 90 degrees respectively. I would think that I’d be getting either .5 to -.5 or ±1/2 PI since the units are radians. I’m not sure if I have an error or if I don’t have the right expectations. The values ‘seem’ to make sense and don’t vary radically when I shake the bot.

I have some other questions but I’ve made this complicated enough. Thanks in advance for any help or comments!


Calculate_Angle.ino (7.72 KB)

There are some things right off that make your code hard to read. First, all capital letter name are, by convention, reserved for constants, and constants never appear on the left of an equal statement, except where they are first valued.

Second, the random indenting is heard to follow. Use Tools + Auto Format to correct that. I'd also prefer to see each { on a new line.

As for the performance killers, delay() is right there at the top of the list, and you have a lot of them.

Serial.print() is probably #2 on the list, and you have plenty of them.

That for loop is going to take forever to execute. The comment notwithstanding, it will iterate 100 times, taking well over 2 seconds per iteration - that's over 3 minutes.

Your robot is going to have fallen off the ball and been flopping around on the ground for an eternity before you get around to noticing that it is off kilter.

And, unless I just missed it, I don't see you actually doing anything with all the data you collect (oh, so slowly).


Thanks so much for taking the time to reply. I wasn’t aware of the uppercase/lower place convention. I’ve been reading and writing G Code for so long I think anything that isn’t in ALL CAPITALS looks strange to me!

That auto format function is a great tip. Thanks again.

That last delay of 2000ms is actually in there by mistake, I meant to remove or comment it out before posting. I put that delay in, and adjusted it around as needed, to help me slow sound the samples and watch behavior. The other 5 delays only run in the first time through as a poor man’s calibration so I think the actual loop time will be under 70ms even with the comments and serial write. Ten loops/second may prove to be too slow still.

I guess there is no way to make the comments less detrimental aside from limiting/eliminating characters or speeding up the baud rate? Is there a command that tells the arduino when the computer is connected so maybe I can use an “if” statement to bypass them when there is no serial monitor? Could it be faster to print to an LCD?.

I haven’t even started to code the actual motor control yet, still trying to get my head around the algorithms that define each wheels acceleration. I just want a little confidence in this part before it’s underlying mistakes have a chance to muddle up the motor troubleshooting.

Thanks again! Anyone else?