Fixed encoder overflow

2021-09-05 11:11:53 +01:00 · 2021-09-05 11:11:53 +01:00 · 74f0f42d2f
commit 74f0f42d2f
parent 3e7df14da8
1 changed files with 5 additions and 1 deletions
--- a/compLib/Encoder.py
+++ b/compLib/Encoder.py
@ -44,7 +44,11 @@ class Encoder(object):
        if port <= 0 or port > MOTOR_COUNT:
            raise IndexError("Invalid encoder port specified!")

-        return Encoder.read_raw(port) - encoder_start_values[port]
+        diff = Encoder.read_raw(port) - encoder_start_values[port]
+        if diff > 2 ** 31:
+            diff -= 2 ** 32
+
+        return diff

    @staticmethod
    def clear(port: int):