diff --git a/example/example.ino b/example/example.ino
index 8e2ea8b..bf5898c 100644
--- a/example/example.ino
+++ b/example/example.ino
@@ -1,21 +1,20 @@
 #include "Dezibot.h"
 Dezibot dezibot;
 void setup() {
-  
+
   dezibot.begin();
   dezibot.motionDetection.begin();
   //dezibot.motionDetection.end();
   // put your setup code here, to run once:
   Serial.begin(115200);
-  
 }
 
 void loop() {
   // put your main code here, to run repeatedly:
-  
-//Serial.println(dezibot.motionDetection.getTemperature());
-//Serial.println(dezibot.motionDetection.getAcceleration().z);
-//Serial.println(dezibot.motionDetection.getRotation().x);
-Serial.println(dezibot.motionDetection.getWhoAmI());
-delay(5);
+
+  //Serial.println(dezibot.motionDetection.getTemperature());
+  Serial.println(dezibot.motionDetection.getAcceleration().z);
+  //Serial.println(dezibot.motionDetection.getRotation().x);
+  Serial.println(dezibot.motionDetection.getWhoAmI());
+  delay(5000);
 }
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index c0e2db1..e2c2ca3 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -14,11 +14,10 @@ void MotionDetection::begin(void){
      
     digitalWrite(34,LOW);
         // set Accel and Gyroscop to Low Noise
-       //handler->transfer(0x1F);
-        //handler->transfer(0x0F);
+        handler->transfer(0x1F);
+        handler->transfer(0x0F);
         //busy Wait for startup 
-        //delayMicroseconds(200);
-        handler->transfer(cmdRead(WHO_AM_I));
+        delayMicroseconds(200);
         
     digitalWrite(34,HIGH);
      //handler->endTransaction();*/
@@ -58,26 +57,19 @@ float MotionDetection::getTemperature(){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
     handler -> write(cmdRead(REG_TEMP_HIGH,REG_TEMP_LOW));
-    uint16_t raw_temperatur = handler->transfer16(cmdRead(REG_TEMP_HIGH,REG_TEMP_LOW));
+    int16_t raw_temperatur = handler->transfer16(cmdRead(REG_TEMP_HIGH,REG_TEMP_LOW));
     
     digitalWrite(34,HIGH);
     handler->endTransaction(); 
     return raw_temperatur/128 +25;
 };
 
-uint8_t MotionDetection::getWhoAmI(){
-    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
-    digitalWrite(34,LOW);
-    uint8_t result;
-    //handler->transferBytes(cmd,result,1);
-    result = handler->transfer(cmdRead(WHO_AM_I));
-    result = handler->transfer(0x00);
-     digitalWrite(34,HIGH);
-    handler->endTransaction(); 
-    return result;
-    
+int8_t MotionDetection::getWhoAmI(){
+    return readRegister(WHO_AM_I);    
 };
 
+
+
 uint16_t MotionDetection::cmdRead(uint8_t regHigh,uint8_t regLow){
     return (CMD_READ | (regHigh & ADDR_MASK))<<8 & (CMD_READ | regLow & ADDR_MASK); 
 };
@@ -89,4 +81,19 @@ uint8_t MotionDetection::cmdRead(uint8_t reg){
 };
 uint8_t MotionDetection::cmdWrite(uint8_t reg){
     return (CMD_WRITE | (reg & ADDR_MASK));
-};
\ No newline at end of file
+};
+
+int8_t MotionDetection::readRegister(uint8_t reg){
+    handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
+    digitalWrite(34,LOW);
+    uint8_t result;
+    result = handler->transfer(cmdRead(reg));
+    result = handler->transfer(0x00);
+     digitalWrite(34,HIGH);
+    handler->endTransaction(); 
+    return result;
+};
+
+int16_t MotionDetection::readDoubleRegister(uint8_t lowerReg){
+    return 0;
+}
\ No newline at end of file
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index e0c5a7d..6d9bd5a 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -54,6 +54,9 @@ protected:
     uint8_t cmdRead(uint8_t reg);
     uint8_t cmdWrite(uint8_t reg);
 
+    int8_t readRegister(uint8_t reg);
+    int16_t readDoubleRegister(uint8_t lowerReg);
+
     SPIClass * handler = NULL;
      
     
@@ -96,6 +99,12 @@ public:
      */
     float getTemperature();
 
-    uint8_t getWhoAmI();
+    /**
+     * @brief Returns the value of reading the whoAmI register
+     * When IMU working correctly, value should be 0x67
+     * 
+     * @return the value of the whoami register of the ICM-42670 
+     */
+    int8_t getWhoAmI();
 };
 #endif //MotionDetection
\ No newline at end of file