next topic »

[on:]

Hello Everyone,

I'm trying to calculate angles using the IXZ-500 gyro but I seem to be having issues properly calculating the angles. For example if I rotate the gyro 90* my calculated angle ends up being around 30. It also seems to be calculating different rates in the negative direction opposed to positive, ie: returning to "zero" positions doesn't give a calculated angle of zero. My Code is below, I'm using trapezoid rule integration with a sampling time of 100 ms. Where the angle should be:

angle=angle + (Z_old+Z_new)*.1
                       -------------------
                                 2

Where Z_old and Z_new are angles using the conversion:   z*(5000mv)     (*/s)
                                                                                         ----------   *  --------
                                                                                          1024bit        9.1mv

I'm using the amplified Z-Output connected to analog pin 1 with VREF connected to analog pin 2.

Does anyone have experience doing something similar with gyros? I'm not sure if I'm converting my values properly or the gyro isn't accurate enough.

Thanks!

Code:

#include <LiquidCrystal.h>

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
int z1;
int z2;
int zo;
int zn;
float angle;
int zero_o;
int zero_n;
float anglerate1;
float anglerate2;


void setup()
{
lcd.begin(16, 2);
delay(1000);
zero_o=analogRead(2);
zo=zero_o;
delay(1000);
delay(1000);
}

void loop(){
   zn = analogRead(1);
   zero_n=analogRead(2);
   z1=zero_o-zo;
   z2=zero_n-zn;
   anglerate1=z1*(5000/1024)/9.1;
   anglerate2=z2*(5000/1024)/9.1;
   angle=angle+(anglerate1+anglerate2)*.05;
   lcd.clear();
   lcd.setCursor(0, 0);
   lcd.print("angle:  ");
   lcd.print(angle);
   lcd.setCursor(0, 1);
   lcd.print("Angle Rate: ");
   lcd.print(anglerate1); 
   delay(100);
   zo=zn;
   zero_o=zero_n;}           

Gyro Datasheet: http://invensense.com/mems/gyro/documents/PS-IXZ-0500B-00-03.pdf

[Reply #1 on:]

Note that "(5000/1024)" is an integer divide that will produce the answer "4", not 4.8828125.  Try "(5000.0/1024.0)".

Also note that the LCD output will take a significant amount of time so assuming that the delay(100) is the ONLY delay will put your time way off.  you might want to note the time (millis()) when you take a sample and subtract the previous time to find out how long it has actually been since the previous sample.

[Reply #2 on:]

The code doesn't make sense - why are you using an old sample the next time round?  You just need to sum the zero-corrected output each time round.  Do any scaling afterwards, you don't want conversion quantization noise mixed in with the input to the integration.

You can do better with the zero-cancellation too (and it definitely affects drift performance).  Take say 16 readings to get a zero-point reading in setup(), then you can get a less biased estimate of each sample by multiplying it by 16 and subtracting the zero-point reading.  This should get your drift down (taking a single reading for zero-point means you are at the mercy of the noise in that single sample).  In fact you can do far better at estimating the zero point if you're prepared to spend a few seconds running the integration loop.

[Reply #3 on:]

The code doesn't make sense - why are you using an old sample the next time round?

I think he is using the average of the initial and final values to get a better estimate of the average rotation rate over the time period.

[Reply #4 on:]

No its using a sample in one loop and then adding it again in the next, so it's just being counted 1/2 a sample time late on average - and more to the point introducing quantization error from the scaling before the accumulate step, which can't improve stability to my mind.

[Reply #5 on:]

I'm using a numerical integration method rather than a simple angle =  angle + rotational rate * time. The method I'm using is the trapezoid rule ( http://en.wikipedia.org/wiki/Trapezoidal_rule ) which I'm hoping will improve my accuracy. Although I will try the simpler method to get a comparison to see which is more consistent.

Just to note I don't necessarily need accurate angle calculations, more so consistent ones. If I can get the code to consistently zero out to a set a position -/+5* that should be fine as well. Right now that's my biggest issue.

Thanks for your reply's so far hopefully I can get this working soon! 

[Reply #6 on:]

This is how I would do such a thing:

Code:

const int ZeroPointPin = A0;
const int XAxisRatePin = A1;
const int YAxisRatePin = A2;
const int ZAxisRatePin = A3;

const float MillivoltsPerDegreePerSecond = 9.1;

const int ZeroSmoothing = 16; // Use a power of 2 between 4 and 64

int ZeroPoint;

class Axis {
public:
  Axis(const int pin) {
    dataPin = pin;
    previousValue = 0;
    previousUpdateTime = 0;
    currentAngle = 0;
  };
  void update();
  int angle() {
    return currentAngle;
  };
  void resetAngle() {
    currentAngle = 0;
  };

private:
  int dataPin;
  int previousValue;
  unsigned long previousUpdateTime;
  float currentAngle;
};

void Axis::update(void)
{
  unsigned long updateTime = millis();

  // Get a value multiplied by half the ZeroSmoothing factor.
  int newValue = analogRead(dataPin) * (ZeroSmoothing/2);

  // Add together the last two readings to get the average * the ZeroSmoothing factor.
  // Then subtract the current ZeroPoint which is alraedy * the ZeroSmoothing factor.
  int rateCounts = (previousValue + newValue) - ZeroPoint;

  if (rateCounts != 0)
  { 
    // Convert to millivolts.  Multiply by ADC full scale voltage (5000 mV) and divide by
    // ADC full scale count * ZeroSmoothing
    float rateMillivolts = (rateCounts * 5000L) / (ZeroSmoothing * 1024.0);

    // Convert to degrees per second by dividing by the scale factor.
    float rateDegreesPerSecond = rateMillivolts / MillivoltsPerDegreePerSecond;

    // Convert to degrees by multiplying by the number of seconds
    float degrees = rateDegreesPerSecond * ((updateTime - previousUpdateTime)/1000.0);
    currentAngle += degrees;
  }

  previousUpdateTime = updateTime;
  previousValue = newValue;
}

Axis XAxis(XAxisRatePin);
Axis YAxis(YAxisRatePin);
Axis ZAxis(ZAxisRatePin);

void setup()
{
  Serial.begin(9600);
  ZeroPoint = 0;
  for (int i = 0; i<ZeroSmoothing; i++)
    ZeroPoint += analogRead(ZeroPointPin);
}

void loop(){
  // Update the zero point to protect against zero point drift
  ZeroPoint = ZeroPoint * (ZeroSmoothing-1) + analogRead(ZeroPointPin);

  XAxis.update();
  YAxis.update();
  ZAxis.update();

  Serial.print("Angles: X=");
  Serial.print(XAxis.angle());
  Serial.print(" Y=");
  Serial.print(YAxis.angle());
  Serial.print(" Z=");
  Serial.println(ZAxis.angle());
  delay(100);
}           

[Reply #7 on:]

Thanks for your help. Unfortunately I was getting better results with the code I was using before. Although I'm not a great programmer and I'm not familiar with classes so I haven't attempted to debug the code.

I appreciate the effort, I'll work on understanding what you've got done there and see if I can get it work!