From f0f846dc04f32e7a86207bf64cfe3c70d49fa87d Mon Sep 17 00:00:00 2001
From: spirkelmann <simon.pirkelmann@gmail.com>
Date: Sun, 14 Apr 2019 22:07:47 +0200
Subject: [PATCH] extended remote control to work with keyboard arrow keys

---
 micropython_firmware/main.py           | 72 ++++++++++++++++----------
 micropython_firmware/remote_control.py | 35 +++++++++++--
 2 files changed, 75 insertions(+), 32 deletions(-)

diff --git a/micropython_firmware/main.py b/micropython_firmware/main.py
index 8dcafff..dfcdd1a 100644
--- a/micropython_firmware/main.py
+++ b/micropython_firmware/main.py
@@ -1,5 +1,6 @@
 import machine
 import time
+import usocket
 
 class Motor:
     def __init__(self, enable, dir1, dir2):
@@ -34,6 +35,11 @@ class Robot:
         print("setting up motors")
         self.m1 = Motor(4, 16, 5)
         self.m2 = Motor(14, 0, 2)
+        
+        # wait for connections on port 1234
+        self.socket = usocket.socket()
+        self.addr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
+        self.socket.bind(self.addr)
         print("setup complete")
 
     def forward(self, seconds):
@@ -74,31 +80,43 @@ class Robot:
         self.forward(0.5)
         self.backward(0.5)
 
+    def remote_control(self):
+        print("waiting for connections on {}".format(self.addr))
+        self.socket.listen(1)
+        res = self.socket.accept() # this blocks until someone connects to the socket
+        comm_socket = res[0]
+        print("connected!")
+        listening = True
+        while listening:
+             # expected data: '(u1, u2)'\n"
+             # where ui = control for motor i
+             # ui \in [-1.0, 1.0]
+             data = comm_socket.readline()
+             data_str = data.decode()
+             print("Data received: {}".format(data_str))
+             try:
+                 print("processing data = {}".format(data_str))
+                 l = data_str.strip('()\n').split(',')
+                 print("l = {}".format(l))
+                 u1 = float(l[0])
+                 print("u1 = {}".format(u1))
+                 u2 = float(l[1])
+                 print("u2 = {}".format(u2))
+             except ValueError:
+                 print("ValueError: Data has wrong format.")
+                 print("Data received: {}".format(data_str))
+                 print("Shutting down")
+                 u1 = u2 = 0.0
+                 listening = False
+             except IndexError:
+                 print("IndexError: Data has wrong format.")
+                 print("Data received: {}".format(data_str))
+                 print("Shutting down")
+                 u1 = u2 = 0.0
+                 listening = False
+             finally:    
+                 self.m1.control(u1)
+                 self.m2.control(u2)
+
 wall_e = Robot()
-
-import usocket
-
-# wait for connections on port 1234
-mysocket = usocket.socket()
-myaddr = usocket.getaddrinfo('192.168.4.1', 1234)[0][-1]
-mysocket.bind(myaddr)
-mysocket.listen(1)
-res = mysocket.accept() # this blocks until someone connects to the socket
-comm_socket = res[0]
-
-# read data until 'q\n' is sent
-listening = True
-while listening:
-     data = comm_socket.readline()
-     data_str = data.decode()
-     print(data_str)
-     if data_str == 'f\n':
-         wall_e.forward(0.5)
-     elif data_str == 'b\n':
-         wall_e.backward(0.5)
-     elif data_str == 'l\n':
-         wall_e.spin_left(0.5)
-     elif data_str == 'r\n':
-         wall_e.spin_right(0.5)
-     elif data_str == 'q\n':
-         listening = False
+wall_e.remote_control()
diff --git a/micropython_firmware/remote_control.py b/micropython_firmware/remote_control.py
index dbfaa0c..ccfe3a8 100644
--- a/micropython_firmware/remote_control.py
+++ b/micropython_firmware/remote_control.py
@@ -1,10 +1,35 @@
 import socket
+import pygame
 
 rc_socket = socket.socket()
 rc_socket.connect(('192.168.4.1', 1234)) # connect to robot
 
-rc_socket.send('l\n') # left  
-rc_socket.send('r\n') # right 
-rc_socket.send('f\n') # forward
-rc_socket.send('b\n') # backward
-rc_socket.send('q\n') # quit
+pygame.init()
+pygame.display.set_mode()
+
+while True:
+    events = pygame.event.get()
+    for event in events:
+        if event.type == pygame.KEYDOWN:
+            if event.key == pygame.K_LEFT:
+                u1 = -1.0
+                u2 = 1.0
+                print("turn left: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_RIGHT:
+                u1 = 1.0
+                u2 = -1.0
+                print("turn right: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_UP:
+                u1 = 1.0
+                u2 = 1.0
+                print("forward: ({},{})".format(u1, u2))
+            elif event.key == pygame.K_DOWN:
+                u1 = -1.0
+                u2 = -1.0
+                print("forward: ({},{})".format(u1, u2))
+            rc_socket.send('({},{})\n'.format(u1, u2))
+        elif event.type == pygame.KEYUP:
+            u1 = 0.0
+            u2 = 0.0
+            print("key released, resetting: ({},{})".format(u1, u2))
+            rc_socket.send('({},{})\n'.format(u1, u2))