From 93eec6996e4fc171b309f530b1fa8a7ad4060c08 Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Thu, 22 Apr 2021 15:46:53 +0100
Subject: [PATCH] Update

---
 pygame_control.py | 135 +++++++++++++++++++++++++++++++---------------
 1 file changed, 91 insertions(+), 44 deletions(-)

diff --git a/pygame_control.py b/pygame_control.py
index 04b196d..a594001 100644
--- a/pygame_control.py
+++ b/pygame_control.py
@@ -1,57 +1,104 @@
 import pygame
+import odrive
+from odrive.enums import *
+import time
 
-pygame.init()
-pygame.font.init()
-font_name = pygame.font.get_default_font()
-font = pygame.font.Font(font_name, 20)
-window = pygame.display.set_mode((500, 500))
-pygame.display.set_caption("RAH Visual Controller V0.1")
+class Odrive:
+    def __init__(self) -> None:
+        return
+        self.drive = odrive.find_any()
+        time.sleep(1)
+        self.set_closed_loop()
 
-run = True
-armed = False
+    def set_idle(self):
+        self.drive.axis0.requested_state = AXIS_STATE_IDLE
+        self.drive.axis1.requested_state = AXIS_STATE_IDLE
 
-prev_x = 0
-prev_y = 0
+    def set_closed_loop(self):
+        self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        time.sleep(1)
 
-def update_and_send_pos_to_odrive():
-    pass
+    def goto(self, mot_0, mot_1):
+        return
+        self.drive.axis0.controller.input_pos = mot_0
+        self.drive.axis1.controller.input_pos = mot_1
 
 
-while run:
-    pygame.time.delay(20)
-    for event in pygame.event.get():
-        if event.type == pygame.QUIT:
-            run = False
+class Controller:
+    def __init__(self) -> None:
+        self.drive = Odrive()
+        pygame.init()
+        pygame.font.init()
+        self.font_name = pygame.font.get_default_font()
+        self.font = pygame.font.Font(self.font_name, 20)
+        self.window = pygame.display.set_mode((500, 500))
+        pygame.display.set_caption("RAH Visual Controller V0.1")
+        self.run = True
+        self.armed = False
+        self.prev_x = 0
+        self.prev_y = 0
+        self.a0 = 0
+        self.a1 = 0
+        self.a0_t = 0
+        self.a1_t = 0
 
+    def calculate_angles(self, large_x, large_y):
+        x = large_x / 10.0
+        y = large_y / 10.0
+        pi = 3.142
+        pulley_diameter = 10.0  # cm
+        distance_per_revolution = pi * pulley_diameter
 
-    mouse_x, mouse_y = pygame.mouse.get_pos()
+        target_angle_0 = (y / distance_per_revolution) - (x / distance_per_revolution)
+        target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution)  # TODO: Derive this matrix
 
-    keys = pygame.key.get_pressed()
-    if keys[pygame.K_SPACE]:
-        armed = True
-        window.fill((64, 0, 0))
-        idkwhatthisis_2 = font.render(f"Armed: {armed}", True, (255, 0, 0))
-        prev_x = mouse_x
-        prev_y = mouse_y
-        # clear where the striker currently is
-    else:
-        armed = False
-        window.fill((10, 10, 10))
-        pygame.draw.circle(window, (255, 0, 0), (prev_x, prev_y), (20))
-        idkwhatthisis_2 = font.render(f"Armed: {armed}", True, (200, 200, 200))
-        # draw where the striker currently is
+        return target_angle_0, target_angle_1
 
-    # window.fill((0, 0, 0))
-    pygame.draw.circle(window, (255, 0, 0), (mouse_x, mouse_y), (20))
-    pygame.draw.rect(window, (0, 255, 0), (mouse_x, 0, 1, 500))
-    pygame.draw.rect(window, (0, 128, 255), (0, mouse_y, 500, 1))
-    idkwhatthisis = font.render(f"X: {mouse_x} | Y: {mouse_y}", True, (255, 255, 255))
-    window.blit(idkwhatthisis, (0, 0))
-    window.blit(idkwhatthisis_2, (0, 480))
-    pygame.display.update()
+    def start(self):
+        while self.run:
+            pygame.time.delay(20)
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    self.run = False
 
-    if armed:
-        update_and_send_pos_to_odrive()
+            mouse_x, mouse_y = pygame.mouse.get_pos()
+            keys = pygame.key.get_pressed()
 
-pygame.font.quit()
-pygame.quit()
+            if keys[pygame.K_SPACE]:
+                self.armed = True
+                self.window.fill((64, 0, 0))
+                idkwhatthisis_2 = self.font.render(f"Armed: {self.armed}", True, (255, 0, 0))
+                self.prev_x = mouse_x
+                self.prev_y = mouse_y
+            else:
+                self.armed = False
+                self.window.fill((10, 10, 10))
+                pygame.draw.circle(self.window, (255, 0, 0), (self.prev_x, self.prev_y), (20))
+                idkwhatthisis_2 = self.font.render(f"Armed: {self.armed}", True, (200, 200, 200))
+
+            pygame.draw.circle(self.window, (255, 0, 0), (mouse_x, mouse_y), (20))
+            pygame.draw.rect(self.window, (0, 255, 0), (mouse_x, 0, 1, 500))
+            pygame.draw.rect(self.window, (0, 128, 255), (0, mouse_y, 500, 1))
+            idkwhatthisis = self.font.render(f"X: {mouse_x} | Y: {500 - mouse_y}", True, (255, 255, 255))
+            idkwhatthisis_3 = self.font.render(f"X: {mouse_x/100} | Y: {(500 - mouse_y)/100}", True, (255, 255, 255))
+            idkwhatthisis_4 = self.font.render(f"M0: {self.a0:.3f} | M1: {self.a1:.3f}", True, (255, 255, 255))
+            self.a0_t, self.a1_t = self.calculate_angles(mouse_x, 500 - mouse_y)
+            idkwhatthisis_5 = self.font.render(f"M0: {self.a0_t:.3f} | M1: {self.a1_t:.3f}", True, (0, 255, 255))
+            self.window.blit(idkwhatthisis, (0, 0))
+            self.window.blit(idkwhatthisis_2, (0, 480))
+            self.window.blit(idkwhatthisis_3, (150, 0))
+            self.window.blit(idkwhatthisis_4, (200, 480))
+            self.window.blit(idkwhatthisis_5, (200, 450))
+            pygame.display.update()
+
+            if self.armed:
+                self.a0, self.a1 = self.calculate_angles(mouse_x, 500 - mouse_y)
+                self.drive.goto(self.a0, self.a1)
+
+        pygame.font.quit()
+        pygame.quit()
+
+if __name__ == "__main__":
+    c = Controller()
+    c.start()