From 0edfc29ce6cf194ff035b9df1052d6e055c8aada Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Sat, 29 May 2021 22:20:53 +0100
Subject: [PATCH] Update

---
 odrive_server_main.py | 58 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)
 create mode 100644 odrive_server_main.py

diff --git a/odrive_server_main.py b/odrive_server_main.py
new file mode 100644
index 0000000..6e0af81
--- /dev/null
+++ b/odrive_server_main.py
@@ -0,0 +1,58 @@
+import odrive
+from odrive.enums import *
+import time
+
+class Odrive:
+    def __init__(self) -> None:
+        self.drive = odrive.find_any()
+        time.sleep(1)
+        self.init_odrive()
+        self.set_zero()
+        self.set_closed_loop()
+
+    def init_odrive(self) -> None:
+        self.drive.axis0.controller.config.input_mode = 1
+        self.drive.axis1.controller.config.input_mode = 1
+        self.drive.axis0.motor.config.current_lim = 70
+        self.drive.axis1.motor.config.current_lim = 70
+        self.drive.axis0.motor.config.current_lim_margin = 30
+        self.drive.axis1.motor.config.current_lim_margin = 30
+
+    def set_zero(self) -> None:
+        self.drive.axis0.encoder.set_linear_count(0)
+        self.drive.axis1.encoder.set_linear_count(0)
+
+    def set_idle(self) -> None:
+        self.drive.axis0.requested_state = AXIS_STATE_IDLE
+        self.drive.axis1.requested_state = AXIS_STATE_IDLE
+
+    def set_closed_loop(self) -> None:
+        self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        time.sleep(1)
+
+    def goto_raw(self, mot_0, mot_1) -> None:
+        self.drive.axis0.controller.input_pos = mot_0
+        self.drive.axis1.controller.input_pos = mot_1
+
+class Server:
+    def __init__(self) -> None:
+        self.x_offset = 0
+        self.x_multiplier = 1
+        self.y_offset = 0
+        self.y_multiplier = 1
+
+    def init_odrive(self):
+        self.drive = Odrive()
+
+    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
+        pi = 3.142
+        pulley_diameter = 10.0  # cm
+        distance_per_revolution = pi * pulley_diameter
+
+        target_angle_0 = (y / distance_per_revolution) - (x / distance_per_revolution)
+        target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution)
+
+        return target_angle_0, target_angle_1