Gyro Integration and Trig?

Hi, two part question for everyone. I'm currently in the middle of a balancing robot build and I'm stuck on the programing. I'm using a IMU Combo Board - 2 Degrees of Freedom - ADXL203/ADXRS401 from sparkfun. All references I've seen tell me that to get the angular position from the angular rate that I need to integrate the rate. Basically....I have no idea how to do that. Can anyone write some quick example code that I could refer from? I'm aware of the drift aspects associated with a rate gyro but I can't even begin to solve that problem until I can actually get a position measurement.

Okay, second question. I would like to use the x acceleration measurement from the accelerometer to help better calculate motor torque. The problem is when the robot is at an angle the X direction of the accelerometer is pointing at a downward or upward angle. Is there any way to do trig functions to find the component of acceleration in the direction of robot motion on Arduino? A tan function and an atan function would be great.


When they say to "integrate the rate", that is just a fancy way of saying "add up all the changes". Let's say you know your starting angle is S. If after M microseconds you check your gyro and it says you are changing at a rate of R degrees/second then you update S by adding R times M/1000000. (that constant is converting the microseconds to seconds to match the units of R.) Then you keep doing this, hopefully fast enough to catch all the little bumps and valleys in your R.

As you say, S will slowly drift off and your robot will fall over. You can probably use your accelerometers to also compute an angle in certain circumstances and use that to correct S when possible.

Floating point math and trig take a large portion of the available memory. Fortunately, you don't really want to know the tangent or arctangent. You want to know what the component of a vector in the direction of another vector is. You want to learn about "dot products" which just use a little multiplication and addition and work very nicely with fixed point numbers. I point you to a quick tutorial on 3D math (but maybe follow the 2d link at the top to get started) and I haven't been through it, but looks very thorough and I plan to read it myself before I build a motor controller.

If you decide you must do trig... then make yourself a constant array of 256 bytes that are the first 1/4 cycle of a sine wave scaled from 0 to 255. With that and a little bit of flipping you can compute the sine and cosine. You can do linear interpolation for better resolution if needed and also scan through it using a binary search algorithm for your inverse sine and cosine functions. Somewhere in the forums I posted one, though none of the supporting functions, just the array of bytes.

Hey Jims. Thanks so much for that info I just want to make sure I've got it right. This is probably a dumb question but how do I know m? If I set a delayMicroseconds(1000) in my loop does that mean the program will grab a rate value 1000 times a second? Or is there something more?

Reading high resolution time is tricky on an Arduino. You can use millis() if you only need to be accurate to something near milliseconds. I’d go with that, you probably won’t be making more than 100 updates per second anyway.

To get more accurate you need to look at the timer registers and the millis() value and try to figure it out from there, but that gets tricky. The ATmega8 manual talks at great length about the timer registers. Remember in the Arduino they are being used for the PWM outputs, so if you change their clock rates or cycle times you will mess up the PWMs.