# NEED HELP: 5DOF

I am aware there is a lot of information out there regarding the sensor, http://www.sparkfun.com/commerce/product_info.php?products_id=9268, but I am afraid I am still lost.

I am able to understand all the equations converting the raw ADC values to degrees/second and degrees for the gyro and accelerometer respectively. However I have used the following code in an attempt to bring the values together.

void find_angles()
{
delta_t = (millis() - prev_t)/1000.0; // Finds the change in time
prev_t = millis(); // Stores current time in previous time variable
est_alpha = prev_alpha + ((analogRead(2) - ss_rollrate_ADC)/gyro_sensitivty_ADC)delta_t; // Alpha Gyro.
alpha = factor
if (alpha > -1 && alpha < 1) {
alpha = 0;
}
prev_alpha = alpha;
//pid();
}

However, the angle is still very much subject to abrupt translations in x, y or z. Which means the gyro is not having enough input into the final angle value. If I weight the angle towards the gyro’s angle, the response is very slow and unresponsive.

I really need help getting the correct angle from this sensor, it is driving me insane.

Sorry I was not more clear with my details. I am doing a self-balancing robot. It is balanced on 2 wheels. Initially I did have all my readings taken together into variables. So I guess I will convert back to my old ways. Inexpensive sensors because I am a poor unistudent.

Not sure if this helps, but I remember on another self balancing project the guy used a kalman filter equation which is used for noisy readings. Search this site for 'self balancing robot' for more info on that if it's applicable.

Inexpensive sensors because I am a poor unistudent.

I've never tried building such a robot, so the following may mean nothing...

First: Wouldn't a hanging pendulum (perhaps mechanically or algorithmically dampened) attached to a potentiometer to sense acceleration and chassis angle be not only the least expensive sensor to use, but also the simplest to code for? Seems like one could tune a PID loop to handle the feedback mechanism...

Second: Do you think the human brain performs such complex calculations in order to keep balance while walking, an arguably (and seemingly) much more complex task than the balancing upon two wheels that your platform must perform?

In summary: Could you be over-complicating the solution?

:)

Thank you everyone so far for replying, however maybe I have under-defined the problem.

another self balancing project the guy used a kalman filter equation which is used for noisy readings

I am aware than many have successfully implemented the Kalman Filter. Sadly, I am not smart enough to understand the mathematics involved.

Then it seems like only one (or maybe two, at most) of the readings are critical to balance.

I know that a 5DOF is overkill, for now. I will need at least 4 degrees once I am finished. However, that is not the point. I am needing help gaining just one angle output. That maybe the pitch or roll... Obviously not the yaw.

First: Wouldn't a hanging pendulum (perhaps mechanically or algorithmically dampened) attached to a potentiometer to sense acceleration and chassis angle be not only the least expensive sensor to use, but also the simplest to code for? Seems like one could tune a PID loop to handle the feedback mechanism...

Second: Do you think the human brain performs such complex calculations in order to keep balance while walking, an arguably (and seemingly) much more complex task than the balancing upon two wheels that your platform must perform?

In summary: Could you be over-complicating the solution?

In brief, the solution is ALWAYS complicated when trying to implement artificial intelligence in a robot. A potentiometer may work, but defeats the purpose of my project. I am using the combination of accelerometers and gyroscopes to acquire a reasonably accurate approximation of a tilting angle.

So the problem redefined:

It is a smaller version of the Segway. It has been done many times, however I am hoping to achieve a self-balancing robot myself. I am stuck on gaining angles. I can gain rather accurate angle readings from the accelerometer, however due to the noisy signals that are also susceptible to translations they will not suffice on their own. The idea of using the gyroscope is a little harder. I still need help. Then the last stage is combining these two. Generally, for the smarter people, a Kalman filter would do. However I am looking for maybe help with a complementary filter. Unless someone has the patience to explain a kalman.

Kindest Regards!

Here is the thing. Integrating the X-gyro you get Pitch, integrating the y-gyro you get Roll. Could be why you are getting noise readings.
Kalman is overkill… CF works great for simple project like these. How much vibrations/noise are you expecting (Frequency) ?

Also, you can use atan2 to calculate the angles more accurately for the accelerometer part. atan2(Xg,Zg)*180/Pi would give you more accuracy. And a you can make a complete turn, +180 to -180. A bit overkill here

If your thing will only tilt in between ± 30 degrees , you can use the small angle approximation to calculate your angle. sinx=x if x is small ( x <= 30*Pi/180, i believe ).

Say you get x to be 0.34g, then your angle is approximately 0.34*180/PI. It depends on how accurate you want to be. The approximation is more accurate when the angles are small.

Thanks heaps BalkanLion1.0, my accelerometer readings are good though. I have an accuracy of approximately 1 degree for a range of -+50degrees. What did you mean by,

Here is the thing. Integrating the X-gyro you get Pitch, integrating the y-gyro you get Roll. Could be why you are getting noise readings. Kalman is overkill... CF works great for simple project like these. How much vibrations/noise are you expecting (Frequency) ?

? I need the values attributed by translations in the XY plane minimised. The gyroscope reading will do this. As for the noise from gyro readings. Why would integrating pitch rate and roll rate create noise? Lastly, what is a basic algorithm for the CF?

Thanks

The noise is because you might be using the wrong Gyro to correct you accelerometer readings, so basically you are just getting the noisy acceleromter. How are you calculating Roll?

For roll if you are using the X accelerometer, you should be integrating the Y-Gyro (Y-Roll pin in your 5dof) . For pitch, Y accelerometer and X Gyro ( X-roll pin) .

The CF is basicially what you have there.

Roll = a* ( Roll + YRotation*dt) + b*( asin(XAccelerometerData) )

a=tau/(dt+tau), b=1-a tau=1/(cut off frequency in rad/s)

YRotation is the rotation around the Y-axis( Y-gyro reading)

That is exactly what I am doing except I randomly chose a and b. Thank you for supplying the formulas. A question though, I am struggling to get dt. A very slimmed down version of my code is:

``````int roll_rate_offset = 504;
double Aref = 4.77; // Volts
double gyromultiplier = (Aref/1024)/.0092;
double ang_convert = 180.0/PI;
float roll_rate;
double alpha;
float t, prev_t, dt;

void setup() {
Serial.begin(9600);
prev_t = millis();
alpha = 0;
}

void loop() {
t = millis();
dt = (t - prev_t)/1000.0; // Change in time in seconds
prev_t = t; // new t becomes old t
roll_rate = (float)(roll_rate_ADC - roll_rate_offset)*gyromultiplier*ang_convert;
alpha = alpha + roll_rate*dt;
Serial.println(dt);
}
``````

Obviously that code has no accelerometer readings, but I am really struggling to get an accurate dt

Some specifics about my physical set up include, sensitivity is 9.2mV/degree/s, x-roll pin connected to analogue pin 0, using a Arduino Nano 328p.

Where am I going wrong?

I will post my code i did when i first started using that same IMU, almost a year ago. It might help you a lot. The floating math doesn’t take too much time now, but you should avoid it. It works good for basic stuff, measuring Roll and Pitch. It does have some error, but it will get you started. Ignore the zGyro, its from another sensor board.

``````#include <math.h>

#define xAcc 0
#define yAcc 1
#define zAcc 2
#define xGyro 5
#define yGyro 4
#define zGyro 8

#define vRef 2.784
#define sen 0.330
#define gSen 0.002
#define gyroZero 1.350

int x0,y0,z1,gx0,gy0,gz0; //x0,y0,z0 is x,y,z axis zero G  ADC value gx0,gy0 no rotation ADC values

float xAngle,yAngle,xAtan,yAtan,zAngle,dxAngle,dyAngle =0;

unsigned long time,dt = 0;
void setup()
{
analogReference(EXTERNAL);
Serial.begin(57600);
delay(500);
getZeros();
}
void getZeros()
{
}
void loop()
{
//******************** CF filter******************************************************************
time=millis();

xAngle = 0.9615 * (xAngle + dxAngle*dt*0.001) + 0.0385*xAtan;
yAngle =0.9615 * (yAngle + dyAngle*dt*0.001) + (0.0385 *yAtan);
//0.9615 is a and its given by a=tau/(tau-dt), 0.0385=1-a

// dt is 1-2 ms
//************************************************************************************************

Serial.print(xAngle);
Serial.print(",");
Serial.print(yAngle);
Serial.print(",");
Serial.println(dt);
dt=millis()-time;
}
``````

I think it’s self explanatory , but if you got any questions please ask.
Here dt is not very accurate, but the change is small so doesn’t really effect the accuracy too much. This program is just starting point. Hope it helps.

I would also suggest that you do all your readings together and then everything else.

Why can't you do all of this in integer arithmetic?

At the risk of seeming completely dimwitted how do I do this?

I think its better to do it so that human understands it first. Then ones you are sure your program runs 100% then you go on about optimizing. Just get it to work. Then if it is not fast enough, then optimize.

At the moment, your code BalkanLion1.0 is outputting the following.

0.00,0.00,3 0.00,0.00,2 0.00,0.00,3 0.00,0.00,3 0.00,0.00,3 0.00,0.00,3 0.00,0.00,3 0.00,0.00,3 0.00,0.00,3 0.00,0.00,4 0.00,0.00,3 0.00,0.00,3

I have changed the pins, but still getting this!

Nothing happens when you tilt the thing? Did you change the reference voltage? I saw in your code its different than mine.

Sadly, yes. I am interested how did you get your ref voltage? Is it what you connected the reference voltage to?

Since the 5DOF works on 3.3v, you are going to need aref to be around 3.3 volts.

If you're using an external reference voltage (applied to the AREF pin), you must set the analog reference to EXTERNAL before calling analogRead(). Otherwise, you will short together the active reference voltage (internally generated) and the AREF pin, possibly damaging the microcontroller on your Arduino board.

Alternatively, you can connect the external reference voltage to the AREF pin through a 5K resistor, allowing you to switch between external and internal reference voltages. Note that the resistor will alter the voltage that gets used as the reference because there is an internal 32K resistor on the AREF pin. The two act as a voltage divider, so, for example, 2.5V applied through the resistor will yield 2.5 * 32 / (32 + 5) = ~2.2V at the AREF pin.

What i did was just connect a 5k resistor from my 3.3V that powers the 5dof.

3.3V o----------[5k resistor] -----------------AREF pin on arduino

Great bit of coding Balkan

Just a couple of questions...

1) If I only wanted to say poll the data once a second rather than consistently as it is done presently what would be the best route? Ive tried putting a delay(1000) in the code and it sure does delay it 1s but the readings are delayed, ie it takes about 20s for the full 20degree pitch/roll to register LOL

2) Are the readings accurate for degree of pitch and roll with respect to a 0 axis? Without a calibrated 10 or 20 degree slope Im not sure if the unit is 100% accurate. Im sure it is but I would like to put it on a slope to confirm the readings are sound.

Thanks!