From 577a3ab67b1f0202a7d12dd5396017a80fee8a19 Mon Sep 17 00:00:00 2001
From: Peter Hinch <peter@hinch.me.uk>
Date: Thu, 14 Oct 2021 19:01:28 +0100
Subject: [PATCH] Encoders: add changes suggested by IhorNehrutsa.

---
 encoders/ENCODERS.md         |  2 +-
 encoders/encoder.py          |  6 ++++--
 encoders/encoder_portable.py |  2 +-
 encoders/encoder_timed.py    | 23 +++++++++++++++--------
 4 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/encoders/ENCODERS.md b/encoders/ENCODERS.md
index 097bb0e..9c92606 100644
--- a/encoders/ENCODERS.md
+++ b/encoders/ENCODERS.md
@@ -61,7 +61,7 @@ class Encoder:
 
     def position(self, value=None):
         if value is not None:
-            self._pos = value // self.scale
+            self._pos = round(value / self.scale)
         return self._pos * self.scale
 ```
 If the direction is incorrect, transpose the X and Y pins in the constructor
diff --git a/encoders/encoder.py b/encoders/encoder.py
index 9f8da70..a1208bb 100644
--- a/encoders/encoder.py
+++ b/encoders/encoder.py
@@ -24,8 +24,10 @@ class Encoder:
         self.forward = self.pin_x.value() ^ self.pin_y.value() ^ self.reverse ^ 1
         self._pos += 1 if self.forward else -1
 
-    def position(self):
-        return self._pos*self.scale
+    def position(self, value=None):
+        if value is not None:
+            self._pos = round(value / self.scale)
+        return self._pos * self.scale
 
     def reset(self):
         self._pos = 0
diff --git a/encoders/encoder_portable.py b/encoders/encoder_portable.py
index 525ca42..09c4396 100644
--- a/encoders/encoder_portable.py
+++ b/encoders/encoder_portable.py
@@ -32,5 +32,5 @@ class Encoder:
 
     def position(self, value=None):
         if value is not None:
-            self._pos = value // self.scale
+            self._pos = round(value / self.scale)  # # Improvement provided by @IhorNehrutsa 
         return self._pos * self.scale
diff --git a/encoders/encoder_timed.py b/encoders/encoder_timed.py
index 8095ae3..8665dbe 100644
--- a/encoders/encoder_timed.py
+++ b/encoders/encoder_timed.py
@@ -2,6 +2,7 @@
 
 # Copyright (c) 2016-2021 Peter Hinch
 # Released under the MIT License (MIT) - see LICENSE file
+# Improvements provided by IhorNehrutsa 
 
 import utime
 from machine import Pin, disable_irq, enable_irq
@@ -9,8 +10,8 @@ from machine import Pin, disable_irq, enable_irq
 class EncoderTimed:
     def __init__(self, pin_x, pin_y, scale=1):
         self.scale = scale  # Optionally scale encoder rate to distance/angle
-        self.tprev = 0
-        self.tlast = 0
+        self.tprev = -1
+        self.tlast = -1
         self.forward = True
         self.pin_x = pin_x
         self.pin_y = pin_y
@@ -39,14 +40,20 @@ class EncoderTimed:
         tlast = self.tlast  # Cache current values
         tprev = self.tprev
         enable_irq(state)
-        if utime.ticks_diff(utime.ticks_us(), tlast) > 2_000_000: # It's stopped
-            result = 0.0
-        else:
-            result = 1000000.0/(utime.ticks_diff(tlast, tprev))
-        result *= self.scale
+        if self.tprev == -1:  # No valid times yet
+            return 0.0
+        dt = utime.ticks_diff(utime.ticks_us(), tlast)
+        if dt > 2_000_000: # Stopped
+            return 0.0
+        dt = utime.ticks_diff(tlast, tprev)
+        if dt == 0:  # Could happen on future rapid hardware
+            return 0.0
+        result = self.scale * 1_000_000.0/dt
         return result if self.forward else -result
 
-    def position(self):
+    def position(self, value=None):
+        if value is not None:
+            self._pos = round(value / self.scale)
         return self._pos * self.scale
 
     def reset(self):