From 0899deb958867a897b3d16a0f849e112fbdbaa64 Mon Sep 17 00:00:00 2001
From: Konstantin Koslowski <konstantin.koslowski@gmail.com>
Date: Thu, 15 Jul 2021 21:21:07 +0200
Subject: [PATCH] rx: tweaks, add max delta for turns

---
 src/esc.py | 39 ++++++++++++++++++++++++++++++++-------
 1 file changed, 32 insertions(+), 7 deletions(-)

diff --git a/src/esc.py b/src/esc.py
index 4845eff..530d6a3 100644
--- a/src/esc.py
+++ b/src/esc.py
@@ -1,7 +1,9 @@
 from motor import Motor
 from time import sleep
 
-delta = 10
+DELTA = 10
+DELTA_TURN = 5
+DELTA_MAX = 30
 
 class Esc:
     def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2,
@@ -10,22 +12,45 @@ class Esc:
         self.m2 = Motor("m2", pin_m2_ena, pin_m2_in1, pin_m2_in2)
 
     def fwd(self):
-        speed = min((self.m1.speed + self.m2.speed)/2 + delta, 100)
+        speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100)
         self.m1.set_speed(speed)
         self.m2.set_speed(speed)
 
     def rev(self):
-        speed = max((self.m1.speed + self.m2.speed)/2 - delta, -100)
+        speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100)
         self.m1.set_speed(speed)
         self.m2.set_speed(speed)
 
     def left(self):
-        self.m1.dec(delta)
-        self.m2.inc(delta)
+        s1 = self.m1.speed
+        s2 = self.m2.speed
+        avg = (s1 + s2)/2
+        delta = s2 - s1
+
+        if delta <= (DELTA_MAX - 2*DELTA_TURN):
+            self.m1.dec(DELTA_TURN)
+            self.m2.inc(DELTA_TURN)
+        elif delta == DELTA_MAX:
+            print("delta reached, not changing")
+        elif delta > (DELTA_MAX):
+            print("delta exceeded, resetting")
+            self.m1.set_speed(avg - DELTA_TURN)
+            self.m2.set_speed(avg + DELTA_TURN)
 
     def right(self):
-        self.m1.inc(delta)
-        self.m2.dec(delta)
+        s1 = self.m1.speed
+        s2 = self.m2.speed
+        avg = (s1 + s2)/2
+        delta = s1 - s2
+        if delta <= (DELTA_MAX - 2*DELTA_TURN):
+            self.m1.inc(DELTA_TURN)
+            self.m2.dec(DELTA_TURN)
+        elif delta == DELTA_MAX:
+            print("delta reached, not changing")
+        elif delta > (DELTA_MAX):
+            print("delta exceeded, resetting")
+            self.m1.set_speed(avg + DELTA_TURN)
+            self.m2.set_speed(avg - DELTA_TURN)
 
     # stop
     def b1(self):