Update
This commit is contained in:
Max
parent
2b400958df
commit
93eec6996e
@ -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()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user