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] 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. 
      * 
     */