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 robotshop.com. 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 Hobbytronics.co.uk. 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 (web.mit.edu/scolton/www/filter.pdf). 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!
-Brian
Calculate_Angle.ino (7.72 KB)