From 20cfd7e60c54df2ba919379894a2215e713c8b30 Mon Sep 17 00:00:00 2001
From: Max <Hunt>
Date: Thu, 22 Apr 2021 18:51:55 +0100
Subject: [PATCH] Update

---
 odrive_setup.py   |  3 +++
 pygame_control.py | 32 ++++++++++++++++++++++----------
 2 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/odrive_setup.py b/odrive_setup.py
index 925be05..fa2bd94 100644
--- a/odrive_setup.py
+++ b/odrive_setup.py
@@ -179,3 +179,6 @@ odrv0.axis1.controller.input_pos = 10
 
 odrv0.axis0.controller.input_pos = 0
 odrv0.axis1.controller.input_pos = 0
+
+odrv0.axis0.encoder.set_linear_count(0)
+odrv0.axis1.encoder.set_linear_count(0)
diff --git a/pygame_control.py b/pygame_control.py
index a594001..c97d6d3 100644
--- a/pygame_control.py
+++ b/pygame_control.py
@@ -5,11 +5,15 @@ import time
 
 class Odrive:
     def __init__(self) -> None:
-        return
         self.drive = odrive.find_any()
         time.sleep(1)
+        self.set_zero()
         self.set_closed_loop()
 
+    def set_zero(self):
+        self.drive.axis0.encoder.set_linear_count(0)
+        self.drive.axis1.encoder.set_linear_count(0)
+
     def set_idle(self):
         self.drive.axis0.requested_state = AXIS_STATE_IDLE
         self.drive.axis1.requested_state = AXIS_STATE_IDLE
@@ -20,10 +24,14 @@ class Odrive:
         time.sleep(1)
 
     def goto(self, mot_0, mot_1):
-        return
         self.drive.axis0.controller.input_pos = mot_0
         self.drive.axis1.controller.input_pos = mot_1
 
+    def get_max_speed(self):
+        v_max_0 = self.drive.axis0.controller.config.vel_limit
+        #v_max_1 = self.drive.axis1.controller.config.vel_limit
+        return v_max_0
+
 
 class Controller:
     def __init__(self) -> None:
@@ -32,16 +40,17 @@ class Controller:
         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))
+        self.window = pygame.display.set_mode((500, 800))
         pygame.display.set_caption("RAH Visual Controller V0.1")
         self.run = True
         self.armed = False
-        self.prev_x = 0
+        self.prev_x = 500
         self.prev_y = 0
         self.a0 = 0
         self.a1 = 0
         self.a0_t = 0
         self.a1_t = 0
+        self.v_max = self.drive.get_max_speed()
 
     def calculate_angles(self, large_x, large_y):
         x = large_x / 10.0
@@ -78,24 +87,27 @@ class Controller:
                 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, 255, 0), (mouse_x, 0, 1, 800))
             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)
+            self.a0_t, self.a1_t = self.calculate_angles(mouse_y*1.1, 990 - mouse_x)
             idkwhatthisis_5 = self.font.render(f"M0: {self.a0_t:.3f} | M1: {self.a1_t:.3f}", True, (0, 255, 255))
+            idkwhatthisis_6 = self.font.render(f"VMAX: {self.v_max}", True, (255, 100, 0))
             self.window.blit(idkwhatthisis, (0, 0))
-            self.window.blit(idkwhatthisis_2, (0, 480))
+            self.window.blit(idkwhatthisis_2, (0, 780))
             self.window.blit(idkwhatthisis_3, (150, 0))
-            self.window.blit(idkwhatthisis_4, (200, 480))
-            self.window.blit(idkwhatthisis_5, (200, 450))
+            self.window.blit(idkwhatthisis_4, (200, 780))
+            self.window.blit(idkwhatthisis_5, (200, 750))
+            self.window.blit(idkwhatthisis_6, (0, 750))
             pygame.display.update()
 
             if self.armed:
-                self.a0, self.a1 = self.calculate_angles(mouse_x, 500 - mouse_y)
+                self.a0, self.a1 = self.calculate_angles(mouse_y*1.1, 990 - mouse_x*2)
                 self.drive.goto(self.a0, self.a1)
 
+        self.drive.set_idle()
         pygame.font.quit()
         pygame.quit()