From 37a8a1a91d0df4fb4721dc7f361e86e2c83470fe Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Sat, 29 May 2021 22:26:16 +0100
Subject: [PATCH] Update

---
 odrive_server_main.py | 24 ++++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

diff --git a/odrive_server_main.py b/odrive_server_main.py
index 6e0af81..4aa7100 100644
--- a/odrive_server_main.py
+++ b/odrive_server_main.py
@@ -1,6 +1,7 @@
 import odrive
 from odrive.enums import *
 import time
+import socket
 
 class Odrive:
     def __init__(self) -> None:
@@ -37,14 +38,29 @@ class Odrive:
 
 class Server:
     def __init__(self) -> None:
-        self.x_offset = 0
-        self.x_multiplier = 1
-        self.y_offset = 0
-        self.y_multiplier = 1
+        self.armed = False
+        self.x_offset = 0  #TODO: figure out what these are...
+        self.x_multiplier = 1  #TODO: figure out what these are...
+        self.y_offset = 0  #TODO: figure out what these are...
+        self.y_multiplier = 1  #TODO: figure out what these are...
+        self.sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
+        self.udp_host = socket.gethostname()
+        self.udp_port = 59200
+        self.sock.bind((self.udp_host, self.udp_port))
+        self.init_odrive()
+        self.control_loop()
 
     def init_odrive(self):
         self.drive = Odrive()
 
+    def control_loop(self):
+        while True:
+            data, _ = self.sock.recvfrom(1024)
+            print(f"Received Message: {data}, type: {type(data)}")
+            if self.armed:
+                angle_0, angle_1 = self.calculate_angles(data[0], data[1])
+                self.drive.goto_raw(angle_0, angle_1)
+
     def calculate_angles(self, target_pos_x, target_pos_y):
         x = (target_pos_x + self.x_offset) * self.x_multiplier
         y = (target_pos_y + self.y_offset) * self.y_multiplier