Hi,
I want to control the tilting of a robot by using L3G4200D gyroscope and Arduino Mega 2560 R3 microcontroller. I got an example code at http://bildr.org/2011/06/l3g4200d-arduino/.. I am attaching the output i've got..
X: -125 Y: -826 Z: -11
X: -84 Y: -798 Z: -10
.......................................
in this code, y is in degrees???
i want variation from 0 to 360 degrees...
The gyro outputs in degrees per second rate of change of rotation in raw ADC counts. The raw data needs to be scaled. Consult the data sheet for the scale factor (page 10, sensitivity) based on the selected range. For example, with the FS range set to ±500 degrees per second the sensitivity is 17.5 milli degrees per second per LSB. So take the raw output and multiply by 0.0175 to get degrees per second. To get position (current tilt) you need to integrate the rate. Gyros drift over time so may not be real accurate. An accelerometer may be a better tilt sensor.
Hi,
Thank you for your reply. I am new to Arduino programming. Can you please give me some hint how to calculate position(current tilt). I've got some hint from a source..
// omega = d(theta)/dt
omegadt = dtheta
theta1 - theta0 = omega(t1-t0)
theta1 = theta0 + omega*(t1-t0)
we get the value of omega by [ sensor value (in dps) * 0.0175]
but we dont know theta0, time(t1 and t0).. how to declare these variables??
You do not need to convert the gyro output from deg./sec to deg in order to balance a robot. You can design a controller that accepts the raw gyro output in deg/sec. But if you do not already know this you may be in over your head. Here is some pseudo code that shows how it can be done...
int PHI[] = {0,0}
int SIGMA[]={0,0,0}
while ( balancing )
{
PHI[2] = ReadGyro( ) ; // read the gyro, tilt angle change per time, deg/sec
// This line implements the controller as a digital filter
SIGMA[2] = -0.369389 PHI[0] + 0.389283 PHI[1] - 1.05127 SIGMA[0] + 2.05148 SIGMA[1] ;
// shift the registers in preparation for the next cycle
SIGMA[0] = SIGMA[1] ;
SIGMA[1] = SIGMA[2] ;
PHI[0] = PHI[1] ;
SetMotorSpeed( SIGMA[2] );
Wait( 10 milliseconds);
}
You can get an overview at my website. The slideshow there shows how the design can be done for a Lego NXT but the same analysis can be done for any platform including the Arduino. I am now doing an Arduino design that should be ready in a few weeks.
http://nxtotherway.webs.com/
Hi,
I found this program for Gyro.
#include <Wire.h>
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#define CTRL_REG6 0x25
int gyroI2CAddr=105;
int gyroRaw[3];
double gyroDPS[3];
float heading[3]={0.0f};
int gyroZeroRate[3];
int gyroThreshold[3];
#define NUM_GYRO_SAMPLES 100
#define GYRO_SIGMA_MULTIPLE 1
float dpsPerDigit=.0175f;
void setup() {
Serial.begin(9600);
Wire.begin();
setupGyro();
calibrateGyro();
}
void loop() {
updateGyroValues();
updateHeadings();
//testCalibration();
printDPS();
Serial.print(" --> ");
printHeadings();
Serial.println();
}
void printDPS()
{
Serial.print("DPS X: ");
Serial.print(gyroDPS[0]);
Serial.print(" Y: ");
Serial.print(gyroDPS[1]);
Serial.print(" Z: ");
Serial.print(gyroDPS[2]);
}
void printHeadings()
{
Serial.print("Heading X: ");
Serial.print(heading[0]);
Serial.print(" Y: ");
Serial.print(heading[1]);
Serial.print(" Z: ");
Serial.print(heading[2]);
}
void updateHeadings()
{
float deltaT=getDeltaTMicros();
for (int j=0;j<3;j++)
heading[j] -= (gyroDPS[j]*deltaT)/1000000.0f;
}
unsigned long getDeltaTMicros()
{
static unsigned long lastTime=0;
unsigned long currentTime=micros();
unsigned long deltaT=currentTime-lastTime;
if (deltaT < 0.0)
deltaT=currentTime+(4294967295-lastTime);
lastTime=currentTime;
return deltaT;
}
void testCalibration()
{
calibrateGyro();
for (int j=0;j<3;j++)
{
Serial.print(gyroZeroRate[j]);
Serial.print(" ");
Serial.print(gyroThreshold[j]);
Serial.print(" ");
}
Serial.println();
return;
}
void setupGyro()
{
gyroWriteI2C(CTRL_REG1, 0x1F);
gyroWriteI2C(CTRL_REG3, 0x08);
setGyroSensitivity500();
delay(100);
}
void calibrateGyro()
{
long int gyroSums[3]={0};
long int gyroSigma[3]={0};
for (int i=0;i<NUM_GYRO_SAMPLES;i++)
{
updateGyroValues();
for (int j=0;j<3;j++)
{
gyroSums[j]+=gyroRaw[j];
gyroSigma[j]+=gyroRaw[j]*gyroRaw[j];
}
}
for (int j=0;j<3;j++)
{
int averageRate=gyroSums[j]/NUM_GYRO_SAMPLES;
gyroZeroRate[j]=averageRate;
gyroThreshold[j]=sqrt((double(gyroSigma[j]) / NUM_GYRO_SAMPLES) - (averageRate * averageRate)) * GYRO_SIGMA_MULTIPLE;
}
}
void updateGyroValues() {
while (!(gyroReadI2C(0x27) & B00001000)){}
int reg=0x28;
for (int j=0;j<3;j++)
{
gyroRaw[j]=(gyroReadI2C(reg) | (gyroReadI2C(reg+1)<<8));
reg+=2;
}
int deltaGyro[3];
for (int j=0;j<3;j++)
{
deltaGyro[j]=gyroRaw[j]-gyroZeroRate[j];
if (abs(deltaGyro[j]) < gyroThreshold[j])
deltaGyro[j]=0;
gyroDPS[j]= dpsPerDigit * deltaGyro[j];
}
}
void setGyroSensitivity500(void)
{
dpsPerDigit=.0175f;
gyroWriteI2C(CTRL_REG4, 0x90);
}
int gyroReadI2C (byte regAddr)
{
Wire.beginTransmission(gyroI2CAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.requestFrom(gyroI2CAddr, 1);
while(!Wire.available()) {};
return (Wire.read());
}
int gyroWriteI2C( byte regAddr, byte val)
{
Wire.beginTransmission(gyroI2CAddr);
Wire.write(regAddr);
Wire.write(val);
Wire.endTransmission();
}
I have uploaded this program to my Arduino Mega 2560R3 microcontroller. At the starting the Headings(x,y,z) are at 0.00 .... but Heading Y is getting incremented every time.. it does not come to zero when i place it at the starting position and the Heading X is showing fluctuations.
Connection details:
Vin = 5 volt
Gnd= Ground
Vio= 3.3 volt
SCL= 21
SDA=20
SDO=3.3 volt
CS= not connected
DRD= not connected
INT1- not connected
Please give me some hints to overcome this problem
Rate gyros (like yours) often have constant offsets and so the angle drifts with time. They need to be calibrated to remove the offset, and periodically reset.
You cannot build an absolute tilt angle detector with just a rate gyro, you need another reference, like an accelerometer, to reset the gyro.
Hi,
I have bought "MMA7361 dual line accelerometer". Can you please give me some hint how to reset the gyroscope(L3G4200D gyro) using the accelerometer? I am using the above mentioned code for reading the gyro values. What modifications i have to make in the program? Please help me out. thanks in advance.
Look up complimentary filter. It is simpler than a Kalman filter for determining orientation (tilt). There is lots of info on implementing the filter and descriptions of how it works. Basically you use integrated rate gyro data for the short term and accel data for the longer term to reset the gyro and compensate for drift.