From 06c29ced605ca4f89fbfa35f64157203749ee1c8 Mon Sep 17 00:00:00 2001
From: Hendrik Langer <hendrik+dev@xd0.de>
Date: Thu, 23 Jul 2020 00:36:30 +0200
Subject: [PATCH] serial connection

---
 raspberry/roberto/Serial.py                 | 50 +++++++++++++++++++++
 raspberry/roberto/__init__.py               |  2 +
 raspberry/roberto/views/websocket/routes.py | 18 +++++++-
 3 files changed, 68 insertions(+), 2 deletions(-)
 create mode 100644 raspberry/roberto/Serial.py

diff --git a/raspberry/roberto/Serial.py b/raspberry/roberto/Serial.py
new file mode 100644
index 0000000..979c532
--- /dev/null
+++ b/raspberry/roberto/Serial.py
@@ -0,0 +1,50 @@
+import time
+import threading
+
+import serial
+
+class Serial(object):
+    def __init__(self, devicename='/dev/ttyUSB0', baudrate=115200):
+        self.devicename = devicename
+        self.alive = None
+        try:
+            self.serialport = serial.Serial(devicename, baudrate, timeout=3.0)
+            self.serialport.flushInput()
+            self.serialport.write("\n")
+            self.alive = True
+            print("Arduino connection established..")
+        except serial.SerialException as e:
+            print('could not open port {!r}: {}\n'.format(devicename, e))
+        except:
+            print("Failed to open serial device: " + self.serialport.name)
+
+        self.receiver_thread = threading.Thread(target=self.reader, name='rx')
+        self.receiver_thread.daemon = True
+        self.receiver_thread.start()
+
+    def __del__(self):
+        self.alive = False
+        self.serialport.close()
+
+    def write(self, buf):
+        if not self.alive:
+            print("Serial port not connected")
+            return
+        try:
+            if isinstance(buf, str):
+                buf = bytes(buf, "utf8")
+            self.serialport.flushInput()
+            self.serialport.write(buf+"\n")  # or "\r\n" ?
+            print("sent " + str(buf))
+        except:
+            print("Error writing to: " + self.serialport.name + " Data: " + str(buf))
+
+    def reader(self):
+        try:
+            while self.alive and self.serialport:
+                data = self.serialport.read(self.serialport.in_waiting or 1)
+                if data:
+                    print(data)
+        except serial.SerialException:
+            self.alive = False
+            raise
diff --git a/raspberry/roberto/__init__.py b/raspberry/roberto/__init__.py
index 8ee42eb..c535229 100644
--- a/raspberry/roberto/__init__.py
+++ b/raspberry/roberto/__init__.py
@@ -18,6 +18,8 @@ login.login_view = "users.login"
 socketio = SocketIO()
 from roberto.camera.camera_opencv import Camera
 camera = Camera()
+from roberto.Serial import Serial
+serial = Serial()
 
 ######################################
 #### Application Factory Function ####
diff --git a/raspberry/roberto/views/websocket/routes.py b/raspberry/roberto/views/websocket/routes.py
index 36b9222..d77978e 100644
--- a/raspberry/roberto/views/websocket/routes.py
+++ b/raspberry/roberto/views/websocket/routes.py
@@ -26,6 +26,20 @@ def test_disconnect():
     print('Client disconnected')
 
 @socketio.on('axes', namespace='/gamepad')
-def gamepad_axes(message):
+def gamepad_axes(axes_data):
     print('GAMEPAD axes')
-    print(message)
+    print(axes_data)
+    from roberto import serial
+    x = axes_data['0']
+    y = axes_data['1']
+    r = axes_data['3']
+    m0 = +y - x - r
+    m1 = +y + x - r
+    m2 = -y + x - r
+    m3 = -y - x - r
+    m0 *= 4000
+    m1 *= 4000
+    m2 *= 4000
+    m3 *= 4000
+    command = 'S '+str(int(m0))+','+str(int(m1))+','+str(int(m2))+','+str(int(m3))+'\n'
+    serial.write(bytes(command, "utf8"))