diff --git a/camera_control.py b/camera_control.py new file mode 100644 index 0000000..c35dc97 --- /dev/null +++ b/camera_control.py @@ -0,0 +1,19 @@ +import cv2 + +cap = cv2.VideoCapture(0) + +# Check if the webcam is opened correctly +if not cap.isOpened(): + raise IOError("Cannot open webcam") + +while True: + ret, frame = cap.read() + frame = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA) + cv2.imshow('Input', frame) + + c = cv2.waitKey(1) + if c == 27: #Esc + break + +cap.release() +cv2.destroyAllWindows() \ No newline at end of file diff --git a/latency.py b/latency.py new file mode 100644 index 0000000..2590554 --- /dev/null +++ b/latency.py @@ -0,0 +1,26 @@ +import time +from pygame_control import Odrive + + + +drive = Odrive() +print() +print('sending drive in 1 sec') +time.sleep(1) +print('NOW') +drive.goto(0.09, 3.255) +print('5...') +time.sleep(1) +print('4...') +time.sleep(1) +print('3...') +time.sleep(1) +print('2...') +time.sleep(1) +print('1...') +time.sleep(1) +# for i in range(50): +print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') +drive.goto(-0.8, 2.29) +time.sleep(1) +drive.set_idle() \ No newline at end of file diff --git a/odrive_server_main.py b/odrive_server_main.py index eb90e89..1033630 100644 --- a/odrive_server_main.py +++ b/odrive_server_main.py @@ -40,10 +40,10 @@ class Odrive: class Server: def __init__(self) -> None: self.armed = False - self.x_offset = 0 #TODO: figure out what these are... - self.x_multiplier = 1 #TODO: figure out what these are... - self.y_offset = 0 #TODO: figure out what these are... - self.y_multiplier = 1 #TODO: figure out what these are... + self.x_offset = 0 + self.x_multiplier = -98/10 # NOTE: Remove the /10 + self.y_offset = 0 + self.y_multiplier = 85/10 # NOTE: Remove the /10 self.sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) self.udp_host = socket.gethostname() self.udp_port = 59200 diff --git a/scaled_pygame_control.py b/scaled_pygame_control.py new file mode 100644 index 0000000..22f1241 --- /dev/null +++ b/scaled_pygame_control.py @@ -0,0 +1,117 @@ +import pygame +import odrive +from odrive.enums import * +import time + +class Odrive: + def __init__(self) -> None: + 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) + self.drive.axis0.controller.config.input_mode = 1 + self.drive.axis1.controller.config.input_mode = 1 + self.drive.axis0.controller.config.input_filter_bandwidth = 30 + self.drive.axis1.controller.config.input_filter_bandwidth = 30 + self.drive.axis0.motor.config.current_lim = 65 + self.drive.axis1.motor.config.current_lim = 65 + self.drive.axis0.motor.config.current_lim_margin = 30 + self.drive.axis1.motor.config.current_lim_margin = 30 + + def set_idle(self): + self.drive.axis0.requested_state = AXIS_STATE_IDLE + self.drive.axis1.requested_state = AXIS_STATE_IDLE + + 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 goto(self, mot_0, mot_1): + self.drive.axis0.controller.input_pos = mot_0 + self.drive.axis1.controller.input_pos = mot_1 + + +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.x_offset = 0 #TODO: figure out what these are... + self.x_multiplier = -98 + self.y_offset = 0 #TODO: figure out what these are... + self.y_multiplier = 85 + + + def calculate_angles(self, target_pos_x, target_pos_y) -> float: + 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 + + def start(self): + while self.run: + pygame.time.delay(20) + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self.run = False + + mouse_x, mouse_y = pygame.mouse.get_pos() + keys = pygame.key.get_pressed() + + 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, 800)) + pygame.draw.rect(self.window, (0, 128, 255), (0, mouse_y, 500, 1)) + idkwhatthisis = self.font.render(f"X: {mouse_x} | Y: {mouse_y}", True, (255, 255, 255)) + idkwhatthisis_3 = self.font.render(f"X: {mouse_x/500} | Y: {(mouse_y)/500}", True, (255, 255, 255)) + idkwhatthisis_4 = self.font.render(f"M0: {self.a0:.3f} | M1: {self.a1:.3f}", True, (255, 255, 255)) + self.window.blit(idkwhatthisis, (0, 0)) + self.window.blit(idkwhatthisis_2, (0, 450)) + self.window.blit(idkwhatthisis_3, (150, 0)) + self.window.blit(idkwhatthisis_4, (200, 450)) + pygame.display.update() + + if self.armed: + self.a0, self.a1 = self.calculate_angles(mouse_x/500, mouse_y/500) + self.drive.goto(self.a0, self.a1) + + self.drive.set_idle() + pygame.font.quit() + pygame.quit() + +if __name__ == "__main__": + c = Controller() + c.start()