rewrite to mimic the behavior of regular rc remotes

src/esc.py

@@ -1,86 +1,76 @@
 from motor import Motor
 from time import sleep
 
-DELTA = 10
+## Parameters
-DELTA_TURN = 5
+# SPEED_MAX = 100
-DELTA_TURN_MAX = 60
+# SPEED_MIN = -100
+SPEED_MAX = 70
+SPEED_MIN = -70
+DELTA_ACC = 10
+DELTA_DEC = 10
+FACTOR_TRN = 0.3
+FACTOR_ROT = 0.5
 
 class Esc:
     def __init__(self, pin_m1_ena, pin_m1_in1, pin_m1_in2,
                  pin_m2_ena, pin_m2_in1, pin_m2_in2):
         self.m1 = Motor("m1", pin_m1_ena, pin_m1_in1, pin_m1_in2)
         self.m2 = Motor("m2", pin_m2_ena, pin_m2_in1, pin_m2_in2)
+        self.m1_speed = 0
+        self.m2_speed = 0
 
-    ## regular movement
-    def accel(self):
-        speed = min((self.m1.speed + self.m2.speed)/2 + DELTA, 100)
-        self.m1.set_speed(speed)
-        self.m2.set_speed(speed)
-
-    def decel(self):
-        speed = max((self.m1.speed + self.m2.speed)/2 - DELTA, -100)
-        self.m1.set_speed(speed)
-        self.m2.set_speed(speed)
-
-    # m1: left, m2: right
     def left(self):
-        s1 = self.m1.speed
+        if self.m2_speed > 0:
-        s2 = self.m2.speed
+            self.m1_speed = self.m2_speed * FACTOR_TRN
-        avg = (s1 + s2)/2
+        elif self.m2_speed < 0:
-        delta = s2 - s1
+            self.m1_speed = self.m2_speed * FACTOR_TRN
-
-        if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
-            self.m1.dec(DELTA_TURN)
-            self.m2.inc(DELTA_TURN)
-        elif delta > (DELTA_TURN_MAX):
-            # print("delta exceeded, resetting")
-            self.m1.set_speed(avg - DELTA_TURN)
-            self.m2.set_speed(avg + DELTA_TURN)
-        # elif delta == DELTA_TURN_MAX:
-        #     print("delta reached, not changing")
 
     def right(self):
-        s1 = self.m1.speed
+        if self.m1_speed > 0:
-        s2 = self.m2.speed
+            self.m2_speed = self.m1_speed * FACTOR_TRN
-        avg = (s1 + s2)/2
+        elif self.m1_speed < 0:
-        delta = s1 - s2
+            self.m2_speed = self.m1_speed * FACTOR_TRN
-        if delta <= (DELTA_TURN_MAX - 2*DELTA_TURN):
-            self.m1.inc(DELTA_TURN)
-            self.m2.dec(DELTA_TURN)
-        elif delta > (DELTA_TURN_MAX):
-            # print("delta exceeded, resetting")
-            self.m1.set_speed(avg + DELTA_TURN)
-            self.m2.set_speed(avg - DELTA_TURN)
-        # elif delta == DELTA_TURN_MAX:
-        #     print("delta reached, not changing")
 
-    ## extra
+    def accel(self):
-    def neutral(self):
+        speed = max(self.m1_speed, self.m2_speed)
-        speed = (self.m1.speed + self.m2.speed)/2
+        self.m1_speed = min(speed + DELTA_ACC, SPEED_MAX)
-        self.m1.set_speed(speed)
+        self.m2_speed = min(speed + DELTA_ACC, SPEED_MAX)
-        self.m2.set_speed(speed)
+
+    def decel(self):
+        speed = min(self.m1_speed, self.m2_speed)
+        self.m1_speed = max(speed - DELTA_DEC, SPEED_MIN)
+        self.m2_speed = max(speed - DELTA_DEC, SPEED_MIN)
+
+    def slow(self):
+        s1 = 0
+        if self.m1_speed > DELTA_DEC:
+            s1 = self.m1_speed - DELTA_DEC
+        elif self.m1_speed < -DELTA_DEC:
+            s1 = self.m1_speed + DELTA_DEC
+        self.m1_speed = s1
+
+        s2 = 0
+        if self.m2_speed > DELTA_DEC:
+            s2 = self.m2_speed - DELTA_DEC
+        elif self.m2_speed < -DELTA_DEC:
+            s2 = self.m2_speed + DELTA_DEC
+        self.m2_speed = s2
 
     def stop(self):
-        self.m1.set_speed(0)
+        self.m1_speed = 0
-        self.m2.set_speed(0)
+        self.m2_speed = 0
 
-    def rotate(self):
+    def rotate_left(self):
-        if self.m1.speed >= 0:
+        self.m1_speed = SPEED_MIN * FACTOR_ROT
-            self.m1.set_speed(0)
+        self.m2_speed = SPEED_MAX * FACTOR_ROT
-            self.m2.set_speed(0)
-            sleep(0.25)
-            self.m1.set_speed(-50)
-            self.m2.set_speed(50)
-            sleep(0.75)
-        else:
-            self.m1.set_speed(0)
-            self.m2.set_speed(0)
-            sleep(0.25)
-            self.m1.set_speed(50)
-            self.m2.set_speed(-50)
-            sleep(0.75)
 
+    def rotate_right(self):
+        self.m1_speed = SPEED_MAX * FACTOR_ROT
+        self.m2_speed = SPEED_MIN * FACTOR_ROT
+
+    def apply(self):
+        self.m1.set_speed(self.m1_speed)
+        self.m2.set_speed(self.m2_speed)
 
     def get_status(self):
-        s = "{}, {}".format(self.m1.get_status(), self.m2.get_status())
+        return("{}, {}".format(self.m1.get_status(), self.m2.get_status()))
-        return s

src/main.py

@@ -1,51 +1,49 @@
 # main.py
+# standard
+import time
+# micropython
+import ujson as json
 import machine
 import usocket as socket
-import time
-from esc import Esc
 # local
-from credentials import *
+from esc import Esc
+from credentials import SSID, PASS
 import wifi
-
 # import web
 
 ## Pins
-## M1: left, M2: right
-# PIN_M1_ENA = 5 # D1
-# PIN_M1_IN1 = 4 # D2
-# PIN_M1_IN2 = 0 # D3
-# PIN_M2_IN1 = 12 # D6
-# PIN_M2_IN2 = 13 # D7
-# PIN_M2_ENA = 15 # D8
-## M1: right, M2: left
 PIN_M1_IN1 = 12 # D6
 PIN_M1_IN2 = 13 # D7
 PIN_M1_ENA = 15 # D8
 PIN_M2_ENA = 5 # D1
 PIN_M2_IN1 = 4 # D2
 PIN_M2_IN2 = 0 # D3
-## Parameters
+## HOST
-T_NEUTRAL=0.5
+RX_ADDR = ("0.0.0.0", 8000)
-T_STATUS=2
 
-## Variables
+## Parameters
+FAIL_COUNT_MAX = 10
+
+## Runtime
 esc = None
 
 def main():
-    print("main")
     global esc
     # wifiSTA = wifi.WifiSTA(SSID, PASS)
+    # wifiSTA.connect()
     wifiAP = wifi.WifiAP(SSID, PASS)
     wifiAP.connect()
+    # Controller
     esc = Esc(PIN_M1_ENA, PIN_M1_IN1, PIN_M1_IN2,
               PIN_M2_ENA, PIN_M2_IN1, PIN_M2_IN2)
+    # Socket
     s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     s.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR, 1)
     s.settimeout(0.1)
-    s.bind(("", 80))
+    s.bind(RX_ADDR)
 
-    ts_cmd = 0
+    fail_count = 0
-    ts_status = 0
+    values = {}
     while True:
         # wifiSTA.check()
         request = ""
@@ -56,43 +54,42 @@ def main():
         except Exception as ex:
             print("Exception Type:{}, args: {}".format(type(ex).__name__, ex.args))
 
+        ## read request
         if len(request) > 0:
-            print('len > 0')
+            fail_count = 0
-            ts_cmd = time.time()
             req = request.decode()
-            print("data: {}, from: {}".format(req, addr))
+            values = json.loads(req)
+        else:
+            fail_count += 1
+            if fail_count > FAIL_COUNT_MAX:
+                print("resetting values")
+                fail_count = 0
+                values = {}
 
-            ## buttons
+        ## control
-            if "accel" in req:
+        # special
-                esc.accel()
+        if values.get("btn_b1") and not values.get("btn_b2"):
-                print(esc.get_status())
+            esc.rotate_left()
-            elif "left" in req:
+        elif values.get("btn_b2") and not values.get("btn_b1"):
-                esc.left()
+            esc.rotate_right()
-                print(esc.get_status())
+        elif values.get("btn_b1") and values.get("btn_b2"):
-            elif "decel" in req:
+            esc.stop()
+        # accel or decel
+        else:
+            if values.get("btn_dn"):
                 esc.decel()
-                print(esc.get_status())
+            elif values.get("btn_up"):
-            elif "right" in req:
+                esc.accel()
-                esc.right()
-                print(esc.get_status())
-            elif "stop" in req:
-                esc.stop()
-                print(esc.get_status())
-            elif "rotate" in req:
-                esc.rotate()
             else:
-                print("unknown: {}".format(req))
+                esc.slow()
+            # left or right
+            if values.get("btn_le"):
+                esc.left()
+            elif values.get("btn_ri"):
+                esc.right()
+        esc.apply()
 
-
+        print("[{}] {}".format(fail_count, esc.get_status()))
-        if time.time() > (ts_cmd + T_NEUTRAL):
-            ts_cmd = time.time()
-            # print(" -: neutral")
-            esc.neutral()
-            # print(esc.get_status())
-
-        if time.time() > (ts_status + T_STATUS):
-            ts_status = time.time()
-            print(esc.get_status())
 
 
 if __name__ == "__main__":

src/motor.py

@@ -1,6 +1,7 @@
-import machine
+# standard
 import time
-
+# micropy
+import machine
 
 class Motor:
     PWM_MAX = 1023
@@ -21,14 +22,13 @@ class Motor:
         self.set_speed(0)
 
     def get_status(self):
-        s = "[{}:status] pins {},{} duty {}".format(
+        return("{}: pins {},{} duty {}".format(
-            self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty())
+            self.name, self.pin1.value(), self.pin2.value(), self.pwm.duty()))
-        return s
 
-    def set_dir(self, dir):
+    def set_dir(self, d):
-        if self.dir == dir:
+        if self.dir == d:
             return
-        self.dir = dir
+        self.dir = d
         v1 = 1 if self.dir == self.DIR_FWD else 0
         v2 = 0 if self.dir == self.DIR_FWD else 1
         self.pin1.value(v1)
@@ -43,11 +43,3 @@ class Motor:
         self.speed = speed
         s = int(self.PWM_MAX * abs(speed)/100)
         self.pwm.duty(s)
-
-    def inc(self, delta):
-        speed = min(self.speed + delta, 100)
-        self.set_speed(speed)
-
-    def dec(self, delta):
-        speed = max(self.speed - delta, -100)
-        self.set_speed(speed)