Gyro Code

#include "mbed.h"


// L3G4200D I2C address
const int GYRO_ADDRESS = 0x68 << 1;
// L3G4200D register addresses
const int WHO_AM_I = 0x0f;
const int CTRL_REG1 = 0x20;
const int CTRL_REG2 = 0x21;
const int CTRL_REG4 = 0x23;
const int CTRL_REG5 = 0x24;
const int OUT_X_L = 0x28;
const int OUT_X_H = 0x29;
const int OUT_Y_L = 0x2a;
const int OUT_Y_H = 0x2b;
const int OUT_Z_L = 0x2c;
const int OUT_Z_H = 0x2d;

Serial pc(USBTX, USBRX);
I2C gyro(p28, p27); //sda, scl

char ctrl1_bits[2] = {0x20,0x0C}; //00001100
char ctrl2_bits[2] = {0x21,0x20}; //--100000
//char ctrl3_bits[2] = {0x22,};
//char ctrl4_bits[2] = {0x23,};
//char ctrl5_bits[2] = {0x24,};

int main() {
    gyro.frequency(400000);
     
    //testing for any errors in communication with the gyro
    char whoami_reg[1] = {WHO_AM_I};
    gyro.write(GYRO_ADDRESS, whoami_reg, 1, true);
    char whoami_data[1] = {0};
    gyro.read(GYRO_ADDRESS, whoami_data, 1, false);
    if (whoami_data[0] == 0xd3){
        pc.printf("Gyro (0x%x) functioning correctly\n\r",whoami_data[0]);    //id should be d3
    }
    else if (whoami_data[0] != 0xd3){
        pc.printf("Gyro Error (0x%x)\n\r",whoami_data[0]);    //id should be d3
    }
    
    //configuring CTRL_REG1
    gyro.write(GYRO_ADDRESS, ctrl1_bits, 2, false);
    
    //configuring CTRL_REG2, turn highpass filter on
    gyro.write(GYRO_ADDRESS, ctrl2_bits, 2, false);
    
    
    while(1){
        //reading gyro's z-axis register
        char outz_l_reg[1] = {OUT_Z_L};
        gyro.write(GYRO_ADDRESS, outz_l_reg, 1, true);
        char outz_l_data[1] = {0};
        gyro.read(GYRO_ADDRESS, outz_l_data, 1, false);  
        
        char outz_h_reg[1] = {OUT_Z_H};
        gyro.write(GYRO_ADDRESS, outz_h_reg, 1, true);
        char outz_h_data[1] = {0};
        gyro.read(GYRO_ADDRESS, outz_h_data, 1, false);  
        
        int OUT_Z = (((int) outz_h_data[0]) << 8 | outz_l_data[0]);
        pc.printf("Z-Axis: %d \n\r", OUT_Z);
        
        wait(1);
    }
}