max/masters-project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max 2021-04-22 18:51:55 +01:00
parent 93eec6996e
commit 20cfd7e60c
2 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
odrive_setup.py
View File

@ -179,3 +179,6 @@ odrv0.axis1.controller.input_pos = 10
odrv0.axis0.controller.input_pos = 0 odrv0.axis0.controller.input_pos = 0
odrv0.axis1.controller.input_pos = 0 odrv0.axis1.controller.input_pos = 0
odrv0.axis0.encoder.set_linear_count(0)
odrv0.axis1.encoder.set_linear_count(0)

32
pygame_control.py
View File

@ -5,11 +5,15 @@ import time
class Odrive: class Odrive:
def __init__(self) -> None: def __init__(self) -> None:
return
self.drive = odrive.find_any() self.drive = odrive.find_any()
time.sleep(1) time.sleep(1)
self.set_zero()
self.set_closed_loop() 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): def set_idle(self):
self.drive.axis0.requested_state = AXIS_STATE_IDLE self.drive.axis0.requested_state = AXIS_STATE_IDLE
self.drive.axis1.requested_state = AXIS_STATE_IDLE self.drive.axis1.requested_state = AXIS_STATE_IDLE
@ -20,10 +24,14 @@ class Odrive:
time.sleep(1) time.sleep(1)
def goto(self, mot_0, mot_1): def goto(self, mot_0, mot_1):
return
self.drive.axis0.controller.input_pos = mot_0 self.drive.axis0.controller.input_pos = mot_0
self.drive.axis1.controller.input_pos = mot_1 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: class Controller:
def __init__(self) -> None: def __init__(self) -> None:
@ -32,16 +40,17 @@ class Controller:
pygame.font.init() pygame.font.init()
self.font_name = pygame.font.get_default_font() self.font_name = pygame.font.get_default_font()
self.font = pygame.font.Font(self.font_name, 20) 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") pygame.display.set_caption("RAH Visual Controller V0.1")
self.run = True self.run = True
self.armed = False self.armed = False
self.prev_x = 0 self.prev_x = 500
self.prev_y = 0 self.prev_y = 0
self.a0 = 0 self.a0 = 0
self.a1 = 0 self.a1 = 0
self.a0_t = 0 self.a0_t = 0
self.a1_t = 0 self.a1_t = 0
self.v_max = self.drive.get_max_speed()
def calculate_angles(self, large_x, large_y): def calculate_angles(self, large_x, large_y):
x = large_x / 10.0 x = large_x / 10.0
@ -78,24 +87,27 @@ class Controller:
idkwhatthisis_2 = self.font.render(f"Armed: {self.armed}", True, (200, 200, 200)) 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.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)) 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 = 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_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)) 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_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, (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_3, (150, 0))
self.window.blit(idkwhatthisis_4, (200, 480)) self.window.blit(idkwhatthisis_4, (200, 780))
self.window.blit(idkwhatthisis_5, (200, 450)) self.window.blit(idkwhatthisis_5, (200, 750))
self.window.blit(idkwhatthisis_6, (0, 750))
pygame.display.update() pygame.display.update()
if self.armed: 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.goto(self.a0, self.a1)
self.drive.set_idle()
pygame.font.quit() pygame.font.quit()
pygame.quit() pygame.quit()