#include "I2Cdev.h"
#include <Wire.h>
#define gyro_Addr 0x69
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
#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
int16_t gx, gy, gz;
uint8_t buffer[6];
void setup(){
Serial.begin(9600);
Wire.begin();
init_gyro();
}
void loop(){
read_gyro(&gx, &gy, &gz);
Serial.print(gx); Serial.print(" ");
Serial.print(gy); Serial.print(" ");
Serial.println(gz);
delay(100);
}
void init_gyro(){
I2Cdev::writeByte(gyro_Addr, CTRL_REG1, 0x00); //Nollställer sensorn
I2Cdev::writeByte(gyro_Addr, CTRL_REG1, 0x0F); //Aktiverar alla axlar(x,y & z)
I2Cdev::writeByte(gyro_Addr, CTRL_REG2, 0x00); //Sätter "Normal mode"
I2Cdev::writeByte(gyro_Addr, CTRL_REG3, 0x00); //Avaktiverar interrupts och FIFO
I2Cdev::writeByte(gyro_Addr, CTRL_REG4, 0x20); //Sätter DPS(degree per sec) = 2 000
I2Cdev::writeByte(gyro_Addr, CTRL_REG5, 0x10); //Aktiverar högpassfilter
}
void read_gyro(int16_t* gx, int16_t* gy, int16_t* gz){
I2Cdev::readBytes(gyro_Addr, OUT_X_L, 6, buffer);
*gx = (((int16_t)buffer[1]) << 8 ) | buffer[0];
*gy = (((int16_t)buffer[3]) << 8 ) | buffer[2];
*gz = (((int16_t)buffer[5]) << 8 ) | buffer[4];
}
edit: Fixed "storage_buffer" to just "storage"
When I use this code to read the values from X, Y and Z axis I get the same values on all of them, anyone know the reason for this ?