From 943589d998df2e2127573ce6f093a6103edbd09f Mon Sep 17 00:00:00 2001
From: Finlay Davidson <finlay.davidson@coderclass.nl>
Date: Fri, 12 Nov 2021 00:48:04 +0100
Subject: [PATCH] Improve and simplify algorithm

---
 src/components/motion/MotionController.cpp | 19 ++++++++-----------
 1 file changed, 8 insertions(+), 11 deletions(-)

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index de1199cc..2593d33a 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -67,17 +67,14 @@ int32_t MotionController::currentShakeSpeed() {
 }
 
 bool MotionController::ShouldSleep(bool isSleeping) {
-  if (not isSleeping) {
-    if (y <= 0) {
-      lastYForSleep = 0;
-      return false;
-    }
-    if (y - 230 > lastYForSleep) {
-      lastYForSleep = y;
-      return true;
-    }
-  }
-  return false;
+  bool ret = false;
+
+  if (y >= lastYForSleep + 192 && !isSleeping)
+    ret = true;
+
+  lastYForSleep = (y > 320) ? y : 320;
+
+  return ret;
 }
 
 void MotionController::IsSensorOk(bool isOk) {