Revision 0:66fe7a32bd59, committed 2014-11-25

Comitter:

Spilly

Date:

Tue Nov 25 15:27:47 2014 +0000

Commit message:

First release

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D.h	Tue Nov 25 15:27:47 2014 +0000
@@ -0,0 +1,257 @@
+#include "mbed.h"
+
+/*************************
+    L3G4200D Registers
+*************************/
+#define WHO_AM_I 0x0F
+#define CTRL_REG1 0x20
+#define CTRL_REG2 0x21
+#define CTRL_REG3 0x22
+#define CTRL_REG4 0x23
+#define CTRL_REG5 0x24
+#define REFERENCE 0x25
+#define OUT_TEMP 0x26
+#define STATUS_REG 0x27
+#define OUT_X_L 0x28
+#define OUT_X_H 0x29
+#define OUT_Y_L 0x2A
+#define OUT_Y_H 0x2B
+#define OUT_Z_L 0x2C
+#define OUT_Z_H 0x2D
+#define FIFO_CTRL_REG 0x2E
+#define FIFO_SRC_REG 0x2F
+#define INT1_CFG 0x30
+#define INT1_SRC 0x31
+#define INT1_TSH_XH 0x32
+#define INT1_TSH_XL 0x33
+#define INT1_TSH_YH 0x34
+#define INT1_TSH_YL 0x35
+#define INT1_TSH_ZH 0x36
+#define INT1_TSH_ZL 0x37
+#define INT1_DURATION 0x38
+#define xHigh 2  //axis x 0b00000010
+#define yHigh 8  //axis y 0b00001000
+#define zHigh 32 //axis z 0b00100000
+#define GYRO_CALIBRATION_COUNT  64       //how many samples to take for calibration
+#define GYRO_SAMPLE_PERIOD  .001f       //differnece of ODR period and time required to read registers
+#define GYRO_SAMPLE_COUNT       1       //number of samples to average
+
+DigitalOut chipSelect(PTD4);
+DigitalIn INT1(D0);
+Serial pc(USBTX, USBRX);
+
+
+SPI L3G4200D(PTD6, PTD7, PTD5);
+
+// 16-bit two's comp gyro readings 
+int16_t gyro[3] = {0,0,0}, gyroBias[3] = {0,0,0};
+
+int readRegister(int address)
+{
+    int toRead;
+    
+    address |= 0x80;  // This tells the L3G4200D we're reading;
+    
+    chipSelect.write(0);
+    L3G4200D.write(address);
+    toRead = L3G4200D.write(0x00);
+    chipSelect.write(1);
+    
+    return toRead;
+}
+
+void writeRegister(int address, int data)
+{
+    address &= 0x7F;  // This to tell the L3G4200D we're writing
+    
+    chipSelect.write(0);
+    L3G4200D.write(address);
+    L3G4200D.write(data);
+    chipSelect.write(1);
+}
+
+
+void setRef()
+{
+    readRegister(REFERENCE);
+    writeRegister(INT1_CFG, 0x6A); //Enable XH, YH and ZH interrupt generation
+                                     //Interrupt latched
+    //Gyro.writeReg(L3G_INT1_CFG, 0x80);
+}
+
+void getGyroValues()
+{
+    gyro[0] = (readRegister(0x29)&0xFF)<<8;
+    gyro[0] |= (readRegister(0x28)&0xFF);
+    //gyro[0] = gyro[0] - gyroBias[0];
+    
+    gyro[1] = (readRegister(0x2B)&0xFF)<<8;
+    gyro[1] |= (readRegister(0x2A)&0xFF);
+    //gyro[1] = gyro[1] - gyroBias[1];
+    
+    gyro[2] = (readRegister(0x2D)&0xFF)<<8;
+    gyro[2] |= (readRegister(0x2C)&0xFF);
+    //gyro[2] = gyro[2] - gyroBias[2];
+}
+
+void getGyroBias()
+{
+    float accumulator[3] = {0,0,0}, tempStore[3];
+    int sampleCount  = 0;
+    
+    //Summation of 64 readings
+    while (sampleCount < GYRO_CALIBRATION_COUNT) 
+    {
+        //Make sure the accelerometer has had enough time
+        //to take a new sample.
+        wait(GYRO_SAMPLE_PERIOD);
+        
+        //get accelerometer data
+        getGyroValues();
+        
+        for(int i = 0; i < 3; i++)
+        {  
+            //add current sample to previous samples
+            accumulator[i] += tempStore[i];
+        }
+        
+        sampleCount++;
+    }
+    for(int i = 0; i < 3; i++)
+    {  
+        //divide by number of samples
+        tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;   
+        gyroBias[i] = (int16_t)tempStore[i];   
+    } 
+}
+
+void getGyroAvg()
+{
+    float accumulator[3] = {0,0,0}, tempStore[3];
+    int sampleCount  = 0;
+    
+    //Summation of 64 readings
+    while (sampleCount < GYRO_SAMPLE_COUNT) 
+    {
+        //Make sure the accelerometer has had enough time
+        //to take a new sample.
+        wait(GYRO_SAMPLE_PERIOD);
+        
+        //get accelerometer data
+        getGyroValues();
+        
+        for(int i = 0; i < 3; i++)
+        {  
+            //add current sample to previous samples
+            accumulator[i] += tempStore[i];
+        }
+        
+        sampleCount++;
+    }
+    for(int i = 0; i < 3; i++)
+    {  
+        //divide by number of samples
+        tempStore[i] = accumulator[i] / GYRO_CALIBRATION_COUNT;   
+        gyro[i] = (int16_t)tempStore[i];   
+    } 
+}
+
+void setupL3G4200D()
+{
+    L3G4200D.format(8, 3);          //Set SPI mode to CPOL = 1 and CPHA = 1  
+                                    //8-bit transmissions
+                                    
+    L3G4200D.frequency(10000000);   //10MHz SPI frequency
+    
+    chipSelect.write(1);  
+    wait(.1);
+    
+    chipSelect.write(0);
+    L3G4200D.write(0);  //Work around for clock not being set high when initiating SPI
+    chipSelect.write(1);
+  
+    //writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 800 Hz
+    writeRegister(CTRL_REG1, 0xCF); //normal power mode, all axes enabled, 100 Hz
+    
+    writeRegister(CTRL_REG2, 0x00); //High-pass filter disabled
+    writeRegister(CTRL_REG3, 0x80); //Interrupt driven to INT1 pad  
+    writeRegister(CTRL_REG4, 0x00); //250 dps full scale
+    writeRegister(CTRL_REG5, 0x05); //Data in DataReg and FIFO are high-pass filtered
+                                      //High-pass-filtered data are used for interrupt
+                                      //generation
+    
+    writeRegister(INT1_TSH_XH, 0x00); //X HIGH threshold 
+    writeRegister(INT1_TSH_XL, 0x60); //X LOW threshold
+    writeRegister(INT1_TSH_YH, 0x00); //Y HIGH threshold
+    writeRegister(INT1_TSH_YL, 0x60); //Y LOW threshold
+    writeRegister(INT1_TSH_ZH, 0x00); //Z HIGH threshold
+    writeRegister(INT1_TSH_ZL, 0x60); //Z LOW threshold
+    
+    writeRegister(INT1_DURATION, 0x01); //Duration = 10ms
+    
+        
+    setRef();           //Device must remain stationary while setting the gyro reference level
+    
+    //getGyroBias();
+}
+
+void checkGyro()
+{
+    int interrSource = readRegister(INT1_SRC);
+    int compare = interrSource&xHigh;
+    
+    //X-Axis
+    if(compare == 2)
+    {
+        //getGyroAvg();
+        getGyroValues();
+        pc.printf("X: %i\n", gyro[0]);     
+        
+        compare = interrSource&yHigh;
+        //Y-Axis
+        if(compare == 8)
+        {
+            pc.printf("Y: %i\n", gyro[1]); 
+        }
+        compare = interrSource&zHigh;
+        //Z-Axis
+        if(compare == 32)
+        {
+            pc.printf("Z: %i\n", gyro[2]);
+        }
+    }
+    
+    else
+    {
+        
+        compare = interrSource&yHigh;
+        //Y-Axis
+        if(compare == 8)
+        {
+     
+            //getGyroAvg();
+            getGyroValues();   
+            pc.printf("Y: %i\n", gyro[1]); 
+            
+            //Z-Axis
+            compare = interrSource&zHigh;
+            if(compare == 32)
+            {
+                pc.printf("Z: %i\n", gyro[2]);
+            }
+        
+        }
+        else
+        { 
+            //Z-Axis
+            compare = interrSource&zHigh;
+            if(compare == 32)
+            {
+                //getGyroAvg();
+                getGyroValues();   
+            
+                pc.printf("Z: %i\n", gyro[2]);
+            }
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 25 15:27:47 2014 +0000
@@ -0,0 +1,56 @@
+/***********************************************************************************************/
+//Ryan Spillman 
+//11/23/2014
+//
+//This is a small piece of a Dead Reckoning robotics project
+//
+//High pass filtered, bias corrected, L3G4200D SPI driver
+//
+//In a later revision, will integrate DPS to approximate position
+//
+//FRDM-K64F Connections
+//MOSI, MISO, SCK,  CS,   INT1
+//PTD6, PTD7, PTD5, PTD4, PTC16
+/***********************************************************************************************/
+
+#include "L3G4200D.h"
+
+//Serial pc(USBTX, USBRX);
+
+
+
+//Prototype function
+void checkGyro();
+
+int main()
+{
+    pc.baud(115200);
+    
+    wait(1);
+    
+    setupL3G4200D();  // Configure L3G4200
+    
+    wait(1);
+    
+    setRef();
+    //getGyroBias();      //Device must be stationary while calculating bias
+    
+    while(1)
+    {
+        //Wait until INT1 pin goes active before reading gyro
+        while(!INT1);
+        /*
+        getGyroValues();
+    
+        for(int i = 0; i < 3; i++)
+        {
+            pc.printf("%i\t", gyro[i]);
+        }
+        
+        
+        pc.printf("\n");
+        */
+        checkGyro();
+        
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Nov 25 15:27:47 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file