From 239a5e629924e239d16997294ff2eccd302e892e Mon Sep 17 00:00:00 2001
From: Hans Haupt <hans.haupt@dezibot.de>
Date: Mon, 10 Jun 2024 23:10:37 +0200
Subject: [PATCH 1/6] WIP: tiltdetection

---
 src/motionDetection/MotionDetection.cpp | 38 ++++++++++++++-
 src/motionDetection/MotionDetection.h   | 63 +++++++++++++++++++++++--
 2 files changed, 95 insertions(+), 6 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index b675e28..7f858c9 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -1,5 +1,6 @@
 #include "MotionDetection.h"
-
+#include <cmath.h>
+#include <limits.h>
 MotionDetection::MotionDetection(){//:handler(FSPI){
     handler = new SPIClass(FSPI);
 };
@@ -58,6 +59,41 @@ int8_t MotionDetection::getWhoAmI(){
     return readRegister(WHO_AM_I);    
 };
 
+bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){
+
+};
+
+void MotionDetection::calibrateZAxis(uint gforceValue){
+    this->gForceCalib = gforceValue;
+};
+
+Orientation MotionDetection::getTilt(){
+    uint tolerance = 200;
+    IMUResult reading = this->getAcceleration();
+    bool flipped = reading.z < 0;
+    float accelrationStrenght = sqrt(reading.x*reading.x+reading.y*reading.y+reading.z*reading.z);
+    //check if reading is valid
+    if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
+        //total accelration is not gravitational force, error
+        return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
+    }
+
+    //calculates the angle between the two axis, therefore value between 0-90
+    uint yAngle;
+    uint xAngle;
+    if(result.z != 0){
+        yAngle = atan((result.x)/result.z)+0.5;
+        xAngle = atan(float(result.y)/result.z)+0.5;
+    } else {
+        yAngle = 90;
+        xAngle = 90;
+    }
+
+
+    //shift quadrants depending on signum
+
+};
+
 uint8_t MotionDetection::cmdRead(uint8_t reg){
     return (CMD_READ | (reg & ADDR_MASK));
 };
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 6d9bd5a..798b8b1 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -17,6 +17,16 @@ struct IMUResult{
     int16_t y;
     int16_t z;
 };
+enum Axis{
+    xAxis = 0x01,
+    yAxis = 0x02,
+    zAxis = 0x04
+};
+
+struct Orientation{
+    uint xRotation;
+    uint yRotation;
+};
 
 
 class MotionDetection{
@@ -47,7 +57,8 @@ protected:
     static const uint8_t PWR_MGMT0 =         0x1F;
     static const uint8_t WHO_AM_I =          0x75;
 
-    static const uint frequency = 10000000;
+    static const uint frequency = 24000000;
+    static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
@@ -58,6 +69,8 @@ protected:
     int16_t readDoubleRegister(uint8_t lowerReg);
 
     SPIClass * handler = NULL;
+
+    gForceCalib = 2000;
      
     
 public:
@@ -83,21 +96,21 @@ public:
      * 
      * @return IMUResult that contains the new read values
      */
-    IMUResult getAcceleration();
+    IMUResult getAcceleration(void);
 
     /**
      * @brief Triggers a new reading of the gyroscope and reads the values from the imu
      * 
      * @return IMUResult 
      */
-    IMUResult getRotation();
+    IMUResult getRotation(void);
 
     /**
      * @brief Reads the current On Chip temperature of the IMU 
      * 
      * @return normalized temperature in degree Centigrade
      */
-    float getTemperature();
+    float getTemperature(void);
 
     /**
      * @brief Returns the value of reading the whoAmI register
@@ -105,6 +118,46 @@ public:
      * 
      * @return the value of the whoami register of the ICM-42670 
      */
-    int8_t getWhoAmI();
+    int8_t getWhoAmI(void);
+
+    /**
+     * @brief Detects if at the time of calling is shaken. Therefore the sum over all accelerationvalues is calculated 
+     * and checked against threshold. If sum > threshold a shake is detected, else not
+     * 
+     * @param threshold (optional) the level of acceleration that must be reached to detect a shake
+     * @param axis (optional) select which axis should be used for detection. Possible values ar xAxis,yAxis,zAxis
+     * It's possible to combine multiple axis with the bitwise or Operator | 
+     * For Example: to detect x and y axis: axis = xAxis|yAxis
+     * 
+     * @return true if a shake is detected, false else
+    */
+    bool isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis);
+
+    /**
+     * @brief calculates how the robot is tilted. It is set, that when the robot is placed normally on a flat table, the result will be (0,0)
+     * Tilting the robot, so that the front leg is deeper than the other to results in an increasing value, tilting the front leg up will let the value jump to 359 and then decreasing
+     * Tilting the robot to the right (..) will increase the value, tilting it left will jump to 359 and then decreased.
+     * 
+     * Precision is rounded to 1deg steps
+     * 
+     * @attention The results are only valid, if the robot is not moved in any way during the measurment, as the calculation is made by using the accelration values.
+     * If it's detected, that the robot is accelerated while measuring, the method will return max(uint).
+     * Please note that the imu is pretty sensitiv, even walking next to the table may influcene the result. 
+     * 
+    */
+    Orientation getTilt();
+
+
+    /**
+     * can be used to set a custom value for the gforceReading of the zaxis, which will improve the getTiltFunction.
+     * 
+     * @attention this method is not persisten, so the value is not stored when the programm is restarted / the robot is powerd off
+     * 
+     * @param gforceValue the value the IMU returns for the gravitationforce -> to get this value, place the robot on a leveled surface
+     * and read the value getAcceleration().z
+    */
+    void calibrateZAxis(uint gforceValue);
+
+
 };
 #endif //MotionDetection
\ No newline at end of file

From bcd03a6148136aec00bad23cbcb82592d026ccf0 Mon Sep 17 00:00:00 2001
From: hhaupt <hans.haupt@dezibot.de>
Date: Tue, 11 Jun 2024 13:25:04 +0200
Subject: [PATCH 2/6] added tiltdetection

---
 src/motionDetection/MotionDetection.cpp | 90 ++++++++++++++++++++-----
 src/motionDetection/MotionDetection.h   | 34 ++++++++--
 src/multiColorLight/MultiColorLight.cpp |  1 -
 3 files changed, 101 insertions(+), 24 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 7f858c9..fd6be8a 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -1,5 +1,5 @@
 #include "MotionDetection.h"
-#include <cmath.h>
+
 #include <limits.h>
 MotionDetection::MotionDetection(){//:handler(FSPI){
     handler = new SPIClass(FSPI);
@@ -10,13 +10,20 @@ void MotionDetection::begin(void){
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);
      
+     
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0)); 
     digitalWrite(34,LOW);
     // set Accel and Gyroscop to Low Noise
+    handler->transfer(PWR_MGMT0);
     handler->transfer(0x1F);
-    handler->transfer(0x0F);
     //busy Wait for startup 
-    delayMicroseconds(200);
+    delayMicroseconds(250);
+    //set Gyroconfig
+    handler->transfer(0x20);
+    handler->transfer(0x25);
+    //set Gyro Filter
+    handler->transfer(0x23);
+    handler->transfer(0x37);
     digitalWrite(34,HIGH);
     handler->endTransaction();
 };
@@ -31,12 +38,9 @@ void MotionDetection::end(void){
 };
 IMUResult MotionDetection::getAcceleration(){
     IMUResult result;
-    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8;
-    result.x |= readRegister(ACCEL_DATA_X_LOW);
-    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8;
-    result.y |= readRegister(ACCEL_DATA_Y_LOW);
-    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8;
-    result.z |= readRegister(ACCEL_DATA_Z_LOW);
+    result.x = readRegister(ACCEL_DATA_X_HIGH)<<8 | readRegister(ACCEL_DATA_X_LOW);
+    result.y = readRegister(ACCEL_DATA_Y_HIGH)<<8 | readRegister(ACCEL_DATA_Y_LOW);
+    result.z = readRegister(ACCEL_DATA_Z_HIGH)<<8 | readRegister(ACCEL_DATA_Z_LOW);
     return result;
 };
 IMUResult MotionDetection::getRotation(){
@@ -59,7 +63,7 @@ int8_t MotionDetection::getWhoAmI(){
     return readRegister(WHO_AM_I);    
 };
 
-bool MotionDetection::isShaken(uint32_t threshold = defaultShakeThreshold,uint8_t axis = xAxis|yAxis|zAxis){
+bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
 
 };
 
@@ -75,23 +79,73 @@ Orientation MotionDetection::getTilt(){
     //check if reading is valid
     if (abs(accelrationStrenght-this->gForceCalib) > tolerance ){
         //total accelration is not gravitational force, error
-        return Orientation{xRotation = UINT_MAX,yRotation = UINT_MAX};
+        return Orientation{INT_MAX,INT_MAX};
     }
 
     //calculates the angle between the two axis, therefore value between 0-90
-    uint yAngle;
-    uint xAngle;
-    if(result.z != 0){
-        yAngle = atan((result.x)/result.z)+0.5;
-        xAngle = atan(float(result.y)/result.z)+0.5;
+    int yAngle;
+    int xAngle;
+    if(reading.z != 0){
+        yAngle = atan(float(reading.x)/reading.z)*180/3.1415+0.5;
+        xAngle = atan(float(reading.y)/reading.z)*180/3.1415+0.5;
     } else {
         yAngle = 90;
         xAngle = 90;
     }
 
-
     //shift quadrants depending on signum
+        //esp is facing down (normal position)
+    if (reading.z > 0){
+        if(reading.y < 0){
+            xAngle = xAngle+180;
+        } else{
+            xAngle = -1*(180-xAngle);
+        }
 
+        if(reading.x < 0){
+            yAngle = yAngle+180;
+        } else{
+            yAngle = -1*(180-yAngle);
+        }
+        //yAngle = -1*yAngle-90;
+    }
+          
+
+    return Orientation{xAngle,yAngle};
+
+};
+
+Direction MotionDetection::getTiltDirection(uint tolerance){
+    if (this->getAcceleration().z > 0){
+        return Flipped;
+    }
+    Orientation Rot = this->getTilt();
+    Serial.println(Rot.xRotation);
+    Serial.println(Rot.xRotation == INT_MAX);
+    if ((Rot.xRotation == INT_MAX)){
+        return Error;
+    }
+    if(abs(abs(Rot.xRotation)-abs(Rot.yRotation))>tolerance){
+        //determine which axis is more tiltet
+        if (abs(Rot.xRotation)>abs(Rot.yRotation)){
+            //test if dezibot is tilted left or right
+            if (Rot.xRotation > 0){
+                return Right;
+            } else {
+                return Left;
+            }
+        } else {
+            //test if robot is tilted front or back
+            if (Rot.yRotation > 0){
+                return Front;
+            } else {
+                return Back;
+            }
+        }
+    } else {
+        //dezibot is (with tolerance) leveled
+        return Neutral;
+    }
 };
 
 uint8_t MotionDetection::cmdRead(uint8_t reg){
@@ -101,7 +155,7 @@ uint8_t MotionDetection::cmdWrite(uint8_t reg){
     return (CMD_WRITE | (reg & ADDR_MASK));
 };
 
-int8_t MotionDetection::readRegister(uint8_t reg){
+uint8_t MotionDetection::readRegister(uint8_t reg){
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
     uint8_t result;
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 798b8b1..8824c7c 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -24,10 +24,19 @@ enum Axis{
 };
 
 struct Orientation{
-    uint xRotation;
-    uint yRotation;
+    int xRotation;
+    int yRotation;
 };
 
+enum Direction{
+    Front,
+    Left,
+    Right,
+    Back,
+    Neutral,
+    Flipped,
+    Error
+};
 
 class MotionDetection{
 protected:
@@ -57,7 +66,7 @@ protected:
     static const uint8_t PWR_MGMT0 =         0x1F;
     static const uint8_t WHO_AM_I =          0x75;
 
-    static const uint frequency = 24000000;
+    static const uint frequency = 10000000;
     static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
@@ -65,12 +74,12 @@ protected:
     uint8_t cmdRead(uint8_t reg);
     uint8_t cmdWrite(uint8_t reg);
 
-    int8_t readRegister(uint8_t reg);
+    uint8_t readRegister(uint8_t reg);
     int16_t readDoubleRegister(uint8_t lowerReg);
 
     SPIClass * handler = NULL;
 
-    gForceCalib = 2000;
+    uint gForceCalib = 4050;
      
     
 public:
@@ -148,6 +157,21 @@ public:
     Orientation getTilt();
 
 
+    /**
+     * @brief Checks in which direction (Front, Left, Right, Back) the robot is tilted. 
+     * 
+     * @attention Does only work if the robot is not moving
+     *  
+     * @param tolerance (optional, default = 10) how many degrees can the robot be tilted, and still will be considerd as neutral. 
+     * 
+     * 
+     * @return Direction the direction in that the robot is tilted most. Front is onsiderd as the direction of driving.
+     * If robot is not tilted more than the tolerance in any direction, return is Neutral.
+     * If Robot is upside down, return is Flipped.
+     * If Robot is moved, return is Error
+     */
+    Direction getTiltDirection(uint tolerance = 10);
+
     /**
      * can be used to set a custom value for the gforceReading of the zaxis, which will improve the getTiltFunction.
      * 
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index f0235a4..622e4b4 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -60,7 +60,6 @@ void MultiColorLight::blink(uint16_t amount,uint32_t color, leds leds, uint32_t
         MultiColorLight::turnOffLed(leds);
         vTaskDelay(interval);
     }
-
 };
 
 void MultiColorLight::turnOffLed(leds leds){

From 14de5a626e9b2d8aac92adfd2bcb96d9d69fa13a Mon Sep 17 00:00:00 2001
From: Hans Haupt <hans.haupt@dezibot.de>
Date: Tue, 11 Jun 2024 17:05:22 +0200
Subject: [PATCH 3/6] updated documentation of getTilt

---
 src/motionDetection/MotionDetection.cpp | 4 ++--
 src/motionDetection/MotionDetection.h   | 7 ++++---
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index fd6be8a..60b17d7 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -89,8 +89,8 @@ Orientation MotionDetection::getTilt(){
         yAngle = atan(float(reading.x)/reading.z)*180/3.1415+0.5;
         xAngle = atan(float(reading.y)/reading.z)*180/3.1415+0.5;
     } else {
-        yAngle = 90;
-        xAngle = 90;
+        yAngle = 90*(reading.x > 0) - (reading.x < 0);
+        xAngle = 90*(reading.y > 0) - (reading.y < 0);
     }
 
     //shift quadrants depending on signum
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 8824c7c..3e84458 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -144,10 +144,11 @@ public:
 
     /**
      * @brief calculates how the robot is tilted. It is set, that when the robot is placed normally on a flat table, the result will be (0,0)
-     * Tilting the robot, so that the front leg is deeper than the other to results in an increasing value, tilting the front leg up will let the value jump to 359 and then decreasing
-     * Tilting the robot to the right (..) will increase the value, tilting it left will jump to 359 and then decreased.
+     * Tilting the robot, so that the front leg is deeper than the other to results in an increasing degrees, tilting the front leg up will increase negativ degrees
+     * Tilting the robot to the right will increase the degrees until 180° (upside down), tilting it left will result in increasing negativ degrees (-1,-2,...,-180).
+     * On the top there is a jump of the values from 180->-180 and vice versa.
      * 
-     * Precision is rounded to 1deg steps
+     * Precision is rounded to 1 deg steps
      * 
      * @attention The results are only valid, if the robot is not moved in any way during the measurment, as the calculation is made by using the accelration values.
      * If it's detected, that the robot is accelerated while measuring, the method will return max(uint).

From 080563f6cc14fd3f14122f4fad045fd6d9e7feee Mon Sep 17 00:00:00 2001
From: hhaupt <hans.haupt@dezibot.de>
Date: Tue, 11 Jun 2024 20:14:56 +0200
Subject: [PATCH 4/6] added example for tiltdetection

---
 .../tilt_detection/tilt_detection.ino         | 37 +++++++++++++++++++
 1 file changed, 37 insertions(+)
 create mode 100644 example/IMU/Tilt_Detection/tilt_detection/tilt_detection.ino

diff --git a/example/IMU/Tilt_Detection/tilt_detection/tilt_detection.ino b/example/IMU/Tilt_Detection/tilt_detection/tilt_detection.ino
new file mode 100644
index 0000000..c9de988
--- /dev/null
+++ b/example/IMU/Tilt_Detection/tilt_detection/tilt_detection.ino
@@ -0,0 +1,37 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+void setup() {
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  dezibot.begin();
+}
+
+void loop() {
+  switch (dezibot.motionDetection.getTiltDirection()) {
+    case Front:
+      dezibot.multiColorLight.setTopLeds(GREEN);
+      break;
+    case Left:
+      dezibot.multiColorLight.setTopLeds(YELLOW);
+      break;
+    case Right:
+      dezibot.multiColorLight.setTopLeds(TURQUOISE);
+      break;
+    case Back:
+      dezibot.multiColorLight.setTopLeds(BLUE);
+      break;
+    case Flipped:
+      dezibot.multiColorLight.setTopLeds(PINK);
+      break;
+    case Neutral:
+      dezibot.multiColorLight.turnOffLed();
+      break;
+
+    case Error:
+      dezibot.multiColorLight.setTopLeds(RED);
+      break;
+  }
+  delay(100);
+}

From 4a6a58e35fad41985d17d02e7dd77caa0e866cea Mon Sep 17 00:00:00 2001
From: hhaupt <hans.haupt@dezibot.de>
Date: Wed, 12 Jun 2024 00:03:29 +0200
Subject: [PATCH 5/6] Implemented ShakeDetection

---
 .../shake_detection/shake_detection.ino       | 19 +++++++++++++++++++
 src/motionDetection/MotionDetection.cpp       | 19 ++++++++++++++++++-
 src/motionDetection/MotionDetection.h         |  4 ++--
 3 files changed, 39 insertions(+), 3 deletions(-)
 create mode 100644 example/IMU/Shake_Detection/shake_detection/shake_detection.ino

diff --git a/example/IMU/Shake_Detection/shake_detection/shake_detection.ino b/example/IMU/Shake_Detection/shake_detection/shake_detection.ino
new file mode 100644
index 0000000..1050c9a
--- /dev/null
+++ b/example/IMU/Shake_Detection/shake_detection/shake_detection.ino
@@ -0,0 +1,19 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  // put your setup code here, to run once:
+
+dezibot.begin();
+dezibot.multiColorLight.turnOffLed();
+ 
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+if(dezibot.motionDetection.isShaken(1000,zAxis)){
+  dezibot.multiColorLight.setTopLeds(0xFF0000);
+} else if(dezibot.motionDetection.isShaken(1000,xAxis|yAxis)) {
+  dezibot.multiColorLight.setTopLeds(0x00FF00);
+}
+}
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 60b17d7..1250c39 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -64,7 +64,24 @@ int8_t MotionDetection::getWhoAmI(){
 };
 
 bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
-
+    IMUResult measurment1 = this->getAcceleration();
+    delayMicroseconds(10);
+    IMUResult measurment2 = this->getAcceleration();
+    uint count = 0;
+    for(uint i = 0;i<20;i++){
+        measurment1 = this->getAcceleration();
+        delayMicroseconds(10);
+        measurment2 = this->getAcceleration();
+        if(
+            (((axis && xAxis) > 0) && (abs(abs(measurment1.x)-abs(measurment2.x))>threshold)) ||
+            (((axis && yAxis) > 0) && (abs(abs(measurment1.y)-abs(measurment2.y))>threshold)) ||
+            (((axis && zAxis) > 0) && (abs(abs(measurment1.z)-abs(measurment2.z))>threshold))){
+                count++;
+        }
+        delayMicroseconds(15);
+    }
+    Serial.println(count);
+    return (count > 6);
 };
 
 void MotionDetection::calibrateZAxis(uint gforceValue){
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 3e84458..473f743 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -67,7 +67,7 @@ protected:
     static const uint8_t WHO_AM_I =          0x75;
 
     static const uint frequency = 10000000;
-    static const uint16_t defaultShakeThreshold = 500;
+    static const uint16_t defaultShakeThreshold = 1000;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);
@@ -151,7 +151,7 @@ public:
      * Precision is rounded to 1 deg steps
      * 
      * @attention The results are only valid, if the robot is not moved in any way during the measurment, as the calculation is made by using the accelration values.
-     * If it's detected, that the robot is accelerated while measuring, the method will return max(uint).
+     * If it's detected, that the robot is accelerated while measuring, the method will return max(int).
      * Please note that the imu is pretty sensitiv, even walking next to the table may influcene the result. 
      * 
     */

From bc2aef650d16cfb82746cea8bf68e429f84b9844 Mon Sep 17 00:00:00 2001
From: hhaupt <hans.haupt@dezibot.de>
Date: Wed, 12 Jun 2024 13:21:47 +0200
Subject: [PATCH 6/6] adapted default threshold for shakedetection

---
 src/motionDetection/MotionDetection.cpp | 6 ++----
 src/motionDetection/MotionDetection.h   | 2 +-
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 1250c39..b90d2a5 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -64,9 +64,8 @@ int8_t MotionDetection::getWhoAmI(){
 };
 
 bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
-    IMUResult measurment1 = this->getAcceleration();
-    delayMicroseconds(10);
-    IMUResult measurment2 = this->getAcceleration();
+    IMUResult measurment1;
+    IMUResult measurment2;
     uint count = 0;
     for(uint i = 0;i<20;i++){
         measurment1 = this->getAcceleration();
@@ -80,7 +79,6 @@ bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
         }
         delayMicroseconds(15);
     }
-    Serial.println(count);
     return (count > 6);
 };
 
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 473f743..eaa47b7 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -67,7 +67,7 @@ protected:
     static const uint8_t WHO_AM_I =          0x75;
 
     static const uint frequency = 10000000;
-    static const uint16_t defaultShakeThreshold = 1000;
+    static const uint16_t defaultShakeThreshold = 500;
 
     uint16_t cmdRead(uint8_t regHigh,uint8_t regLow);
     uint16_t cmdWrite(uint8_t regHigh,uint8_t regLow);