Jody McAdams avatar Jody McAdams committed afbdcac

Some gyro code

Comments (0)

Files changed (1)

Sensors/Gyro/Gyro.ino

+// Code by: Jody McAdams
+// Original code from: http://bildr.org/2011/06/l3g4200d-arduino/
+// License: http://www.opensource.org/licenses/mit-license.php (Go crazy)
+
+#include <Wire.h>
+
+#define kFilteringFactor 0.1f
+#define ENABLE_FILTERING true
+
+#define CTRL_REG1 0x20
+#define CTRL_REG2 0x21
+#define CTRL_REG3 0x22
+#define CTRL_REG4 0x23
+#define CTRL_REG5 0x24
+
+//Sensitivity from data sheet in mdps/digit (millidegrees per second)
+const float SoDR_250 = 8.75f;
+const float SoDR_500 = 17.50f;
+const float SoDR_2000 = 70.0f;
+
+//Digital zero-rate level from data sheet in dps (degrees per second)
+const float DVoff_250 = 10.0f;
+const float DVoff_500 = 15.0f;
+const float DVoff_2000 = 75.0f;
+
+//Final scale factors based on data sheet numbers
+const float scale_250 = SoDR_250/1000.0f;
+const float scale_500 = SoDR_500/1000.0f;
+const float scale_2000 = SoDR_2000/1000.0f;
+
+//Save the scale value that will be used when the program starts
+float scaleFactor = 0.0f;
+
+int L3G4200D_Address = 105; //I2C address of the L3G4200D
+
+float x = 0.0f;
+float y = 0.0f;
+float z = 0.0f;
+
+void setup(){
+
+  Wire.begin();
+  Serial.begin(9600);
+
+  Serial.println("starting up L3G4200D");
+  setupL3G4200D(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec
+
+  delay(1500); //wait for the sensor to be ready 
+}
+
+void loop(){
+   getGyroValues();  // This will update x, y, and z with new values
+   
+  Serial.print("X:");
+  Serial.println(x);
+ 
+  /*Serial.print("Y:");
+  Serial.println(y);
+
+  Serial.print("Z:");
+  Serial.println(z);*/
+
+  delay(100); //Just here to slow down the serial to make it more readable
+}
+
+void getGyroValues(){
+
+  byte xMSB = readRegister(L3G4200D_Address, 0x29);
+  byte xLSB = readRegister(L3G4200D_Address, 0x28);
+  float xVal = scaleFactor * ((xMSB << 8) | xLSB);
+
+  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
+  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
+  float yVal = scaleFactor * ((yMSB << 8) | yLSB);
+
+  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
+  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
+  float zVal = scaleFactor * ((zMSB << 8) | zLSB);
+  
+  #if ENABLE_FILTERING
+    x = FilterInput(x,xVal);
+    y = FilterInput(y,yVal);
+    z = FilterInput(z,zVal);
+  #else
+    x = xVal;
+    y = yVal;
+    z = zVal;
+  #endif
+}
+
+int setupL3G4200D(int scale){
+  //From  Jim Lindblom of Sparkfun's code
+
+  // Enable x, y, z and turn off power down:
+  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
+
+  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
+  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
+
+  // Configure CTRL_REG3 to generate data ready interrupt on INT2
+  // No interrupts used on INT1, if you'd like to configure INT1
+  // or INT2 otherwise, consult the datasheet:
+  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
+
+  // CTRL_REG4 controls the full-scale range, among other things:
+
+  switch(scale)
+  {
+    case 250:
+    {
+      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
+      scaleFactor = scale_250;
+      
+      break;
+    }
+    case 500:
+    {
+      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
+      scaleFactor = scale_500;
+      
+      break;
+    }
+    case 2000:
+    {
+      writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
+      scaleFactor = scale_2000;
+      
+      break;
+    }
+  }  
+
+  // CTRL_REG5 controls high-pass filtering of outputs, use it
+  // if you'd like:
+  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
+}
+
+void writeRegister(int deviceAddress, byte address, byte val) {
+    Wire.beginTransmission(deviceAddress); // start transmission to device 
+    Wire.write(address);       // send register address
+    Wire.write(val);         // send value to write
+    Wire.endTransmission();     // end transmission
+}
+
+int readRegister(int deviceAddress, byte address){
+
+    int v;
+    Wire.beginTransmission(deviceAddress);
+    Wire.write(address); // register to read
+    Wire.endTransmission();
+
+    Wire.requestFrom(deviceAddress, 1); // read a byte
+
+    while(!Wire.available()) {
+        // waiting
+    }
+
+    v = Wire.read();
+    return v;
+}
+
+float FilterInput(float currInput, float newInput)
+{
+  return (newInput * kFilteringFactor) + (currInput * (1.0f - kFilteringFactor));
+}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.