#include <SPI.h>
#include <stdio.h>

const byte READ = 0b11111100;     
const byte WRITE = 0b00000010;

//Gyron registerit
#define REVID 0x01
#define CTRL 0x02
#define I2C_DIS 0x10
#define MODE_80 0x06
#define RESET 0x80
#define STATUS 0x02

// CSB pinnit ja gyron data ready pin
const int GyroDataReadyPin = 6;
const int GyroChipSelectPin = 48;

// Gyron muuttujat
unsigned short gyro_msb_ydata;
unsigned short gyro_lsb_ydata;
short gyro_ydata;

unsigned short gyro_msb_xdata;
unsigned short gyro_lsb_xdata;
short gyro_xdata;

unsigned short gyro_msb_zdata;
unsigned short gyro_lsb_zdata;
short gyro_zdata;

void setup() {
  Serial.begin(115200);
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV64);
  pinMode(GyroDataReadyPin, INPUT);
  pinMode(GyroChipSelectPin, OUTPUT);
  GyroWriteRegister(CTRL, RESET);
  delay(100);
  GyroWriteRegister(CTRL,0x16);//, I2C_DIS | MODE_80);
  delay(100);
}

void loop()
{  
 
  // Gyron akseleiden luku
  if (digitalRead(GyroDataReadyPin) == HIGH)
    {
          // Gyro x-data
          gyro_msb_xdata=GyroReadRegister(0x0D, 1);
          gyro_lsb_xdata=GyroReadRegister(0x0C, 1);       
          gyro_xdata=(gyro_msb_xdata<<8) | gyro_lsb_xdata;
          
          // Gyro y-data          
          gyro_msb_ydata=GyroReadRegister(0x0F, 1);     
          gyro_lsb_ydata=GyroReadRegister(0x0E, 1);    
          gyro_ydata=(gyro_msb_ydata<<8) | gyro_lsb_ydata;
          
          // Gyro z-data
          gyro_msb_zdata=GyroReadRegister(0x11, 1);
          gyro_lsb_zdata=GyroReadRegister(0x10, 1);
          gyro_zdata=(gyro_msb_zdata<<8) | gyro_lsb_zdata;
       
    }
// tulostetaan arvot sarjaväylään
Serial.print("GYRO: ");
Serial.print(gyro_xdata, DEC);
Serial.print(" ");
Serial.print(gyro_ydata, DEC);
Serial.print(" ");
Serial.println(gyro_zdata, DEC);
}

//gyro read
unsigned int GyroReadRegister(byte thisRegister, int bytesToRead ) {
  byte inByte = 0;           
  unsigned int result = 0;   

  thisRegister = thisRegister << 2;
  byte dataToSend = thisRegister & READ;
  digitalWrite(GyroChipSelectPin, LOW);
  SPI.transfer(dataToSend);
  result = SPI.transfer(0x00);
  bytesToRead--;
  if (bytesToRead > 0)
  {    
    result = result << 8;
    inByte = SPI.transfer(0x00);  
    result = result | inByte;  
    bytesToRead--;
  }
  digitalWrite(GyroChipSelectPin, HIGH);
  delay(5);
  return(result);
}

//gyro write
void GyroWriteRegister(byte thisRegister, byte thisValue)
{  
  thisRegister = thisRegister << 2;
  byte dataToSend = thisRegister | WRITE;
  digitalWrite(GyroChipSelectPin, LOW);
  SPI.transfer(dataToSend);
  SPI.transfer(thisValue);  
  digitalWrite(GyroChipSelectPin, HIGH);
  delay(20);
}
  • No labels
You must log in to comment.