From 83474b34c70de7b4aa071d392722a0b244696e58 Mon Sep 17 00:00:00 2001
From: Konstantin Koslowski <konstantin.koslowski@gmail.com>
Date: Sat, 17 Jul 2021 00:37:46 +0200
Subject: [PATCH] tx-udp: change control scheme, try to optimize

---
 pymakr.conf |  2 +-
 src/main.py | 67 ++++++++++++++++++++++++++++++++---------------------
 2 files changed, 42 insertions(+), 27 deletions(-)

diff --git a/pymakr.conf b/pymakr.conf
index f54d47b..2370b78 100644
--- a/pymakr.conf
+++ b/pymakr.conf
@@ -1,5 +1,5 @@
 {
-    "address": "/dev/ttyUSB1",
+    "address": "/dev/esp-tx",
     "username": "micro",
     "password": "python",
     "sync_folder": "src",
diff --git a/src/main.py b/src/main.py
index 4cea0b1..f7d1166 100644
--- a/src/main.py
+++ b/src/main.py
@@ -1,7 +1,7 @@
 # main.py
 import machine
 import socket
-from time import sleep
+import time
 from wifi import WifiSTA
 
 
@@ -15,36 +15,38 @@ PIN_B1 =  0 # D2
 PIN_B2 =  4 # D3
 ## Delay
 DELAY_CMD = 0.1
+DELAY_IRQ = 0.1
 ## HOST
 RX_ADDR = ("192.168.11.220", 80)
 # RX_ADDR = ("192.168.0.1", 80)
 
-ts_cmd = 0
+ts_irq = 0
 s = None
 
 def handle_btn_up(pin):
-    send_cmd("fwd")
+    global ts_irq
+    t = time.time()
+    if t > (ts_irq + DELAY_IRQ):
+        ts_irq = t
+        send_cmd("rotate")
 
-def handle_btn_dn(pin):
-    send_cmd("rev")
+# def handle_btn_b1(pin):
+#     global ts_irq
+#     t = time.time()
+#     if t > (ts_irq + DELAY_IRQ):
+#         ts_irq = t
+#         send_cmd("b1")
 
-def handle_btn_le(pin):
-    send_cmd("left")
-
-def handle_btn_ri(pin):
-    send_cmd("right")
-
-def handle_btn_b1(pin):
-    send_cmd("b1")
-
-def handle_btn_b2(pin):
-    send_cmd("b2")
+# def handle_btn_b2(pin):
+#     global ts_irq
+#     t = time.time()
+#     if t > (ts_irq + DELAY_IRQ):
+#         ts_irq = t
+#         send_cmd("b2")
 
 def send_cmd(cmd):
+    global s
     print("cmd: {}".format(cmd))
-    global s, ts_cmd
-    # if time.time() > (ts_cmd + DELAY_CMD):
-    #     ts_cmd = time.time()
     try:
         s.sendto(cmd.encode(), RX_ADDR)
     except Exception as ex:
@@ -57,19 +59,32 @@ def main():
     btn_up = machine.Pin(PIN_UP, machine.Pin.IN, machine.Pin.PULL_UP)
     btn_up.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_up)
     btn_dn = machine.Pin(PIN_DN, machine.Pin.IN, machine.Pin.PULL_UP)
-    btn_dn.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_dn)
+    # btn_dn.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_dn)
     btn_le = machine.Pin(PIN_LE, machine.Pin.IN, machine.Pin.PULL_UP)
-    btn_le.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_le)
+    # btn_le.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_le)
     btn_ri = machine.Pin(PIN_RI, machine.Pin.IN, machine.Pin.PULL_UP)
-    btn_ri.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_ri)
+    # btn_ri.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_ri)
     btn_b1 = machine.Pin(PIN_B1, machine.Pin.IN, machine.Pin.PULL_UP)
-    btn_b1.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_b1)
+    # btn_b1.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_b1)
     btn_b2 = machine.Pin(PIN_B2, machine.Pin.IN, machine.Pin.PULL_UP)
-    btn_b2.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_b2)
+    # btn_b2.irq(trigger=machine.Pin.IRQ_FALLING, handler=handle_btn_b2)
     s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 
     while True:
-        sleep(0.01)
+        if not btn_le.value():
+            send_cmd("left")
+        elif not btn_ri.value():
+            send_cmd("right")
+        elif not btn_b1.value():
+            send_cmd("accel")
+        elif not btn_b2.value():
+            send_cmd("decel")
+        elif not btn_up.value():
+            send_cmd("rotate")
+        elif not btn_dn.value():
+            send_cmd("stop")
+
+        time.sleep(DELAY_CMD)
 
 if __name__ == "__main__":
-    main()
+    main()
\ No newline at end of file