|
1 | | -import socket |
2 | | -import pygame |
3 | | -import sys |
4 | | -import time |
5 | | -import vgamepad as vg # Import vgamepad |
6 | | - |
7 | | -def get_motor_positions(ip_address, port, retries=60, delay=0.5): |
8 | | - """Attempts to receive motor positions from the specified IP and port.""" |
9 | | - for attempt in range(retries): |
10 | | - try: |
11 | | - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: |
12 | | - s.connect((ip_address, port)) |
13 | | - data = s.recv(1024) |
14 | | - received_str = data.decode() |
15 | | - # Parse the motor positions and touch state |
16 | | - pos_a_degrees = received_str.split(",")[0].split(":")[1].strip().split()[0] |
17 | | - pos_b_degrees = received_str.split(",")[1].split(":")[1].strip().split()[0] |
18 | | - pos_c_degrees = received_str.split(",")[2].split(":")[1].strip().split()[0] |
19 | | - touch_state = received_str.split(",")[3].split(":")[1].strip() |
20 | | - return pos_a_degrees, pos_b_degrees, pos_c_degrees, touch_state |
21 | | - except (IndexError, ValueError, socket.error) as e: |
22 | | - print(f"Error receiving motor positions (attempt {attempt + 1}/{retries}): {e}") |
23 | | - time.sleep(delay) |
24 | | - return "N/A", "N/A", "N/A", "N/A" |
25 | | - |
26 | | -def main(): |
27 | | - ip_address = 'EV3-IP' # [LOCAL] IP of the device sending motor data (replaced random local ip with placeholder). (if using custom connection might not be needed) |
28 | | - port = 12345 # Port to connect to (default port). |
29 | | - |
30 | | - # Set limits for motor positions |
31 | | - limit_a = 40 |
32 | | - limit_b = 45 |
33 | | - limit_c = 50 |
34 | | - |
35 | | - pygame.init() |
36 | | - screen = pygame.display.set_mode((400, 300)) |
37 | | - pygame.display.set_caption('Motor Positions') |
38 | | - font = pygame.font.Font(None, 36) |
39 | | - clock = pygame.time.Clock() |
40 | | - |
41 | | - # Initialize vGamepad (Xbox 360 Controller) |
42 | | - gamepad = vg.VX360Gamepad() |
43 | | - print("vGamepad initialized!") |
44 | | - |
45 | | - while True: |
46 | | - for event in pygame.event.get(): |
47 | | - if event.type == pygame.QUIT: |
48 | | - pygame.quit() |
49 | | - sys.exit() |
50 | | - |
51 | | - pos_a_degrees, pos_b_degrees, pos_c_degrees, touch_state = get_motor_positions(ip_address, port) |
52 | | - |
53 | | - if pos_a_degrees != "N/A": |
54 | | - pos_a_degrees = max(min(float(pos_a_degrees), limit_a), -limit_a) |
55 | | - if pos_b_degrees != "N/A": |
56 | | - pos_b_degrees = max(min(float(pos_b_degrees), limit_b), -limit_b) |
57 | | - if pos_c_degrees != "N/A": |
58 | | - pos_c_degrees = max(min(float(pos_c_degrees), limit_c), -limit_c) |
59 | | - |
60 | | - # Scale motor positions to joystick axis range (-32768 to 32767) |
61 | | - scaled_pos_a = int((float(pos_a_degrees) / limit_a) * 32767) |
62 | | - scaled_pos_b = int((float(pos_b_degrees) / limit_b) * 32767) |
63 | | - |
64 | | - # Scale motor C to right joystick Y-axis (-32768 to 32767) |
65 | | - if pos_c_degrees == "N/A": |
66 | | - scaled_pos_c = 0 # Default to center if data is invalid |
67 | | - else: |
68 | | - scaled_pos_c = int((float(pos_c_degrees) / limit_c) * 32767) |
69 | | - |
70 | | - # Send values to virtual gamepad |
71 | | - gamepad.left_joystick(x_value=scaled_pos_a, y_value=scaled_pos_b) # Left joystick |
72 | | - gamepad.right_joystick(x_value=0, y_value=scaled_pos_c) # Right joystick Y-axis |
73 | | - |
74 | | - # Set button state |
75 | | - if touch_state == "1": |
76 | | - gamepad.press_button(vg.XUSB_BUTTON.XUSB_GAMEPAD_A) |
77 | | - else: |
78 | | - gamepad.release_button(vg.XUSB_BUTTON.XUSB_GAMEPAD_A) |
79 | | - |
80 | | - gamepad.update() # Apply changes to virtual controller |
81 | | - |
82 | | - # Display motor positions on screen |
83 | | - screen.fill((255, 255, 255)) |
84 | | - text_a = font.render(f"Motor A Position: {pos_a_degrees} degrees", True, (0, 0, 0)) |
85 | | - text_b = font.render(f"Motor B Position: {pos_b_degrees} degrees", True, (0, 0, 0)) |
86 | | - text_c = font.render(f"Motor C Position: {pos_c_degrees} degrees", True, (0, 0, 0)) |
87 | | - text_touch = font.render(f"Button State: {touch_state}", True, (0, 0, 0)) |
88 | | - screen.blit(text_a, (20, 50)) |
89 | | - screen.blit(text_b, (20, 100)) |
90 | | - screen.blit(text_c, (20, 150)) |
91 | | - screen.blit(text_touch, (20, 200)) |
92 | | - |
93 | | - pygame.display.flip() |
94 | | - clock.tick(1000) # Update every 1 millisecond |
95 | | - |
96 | | -if __name__ == "__main__": |
97 | | - main() |
| 1 | +import socket |
| 2 | +import pygame |
| 3 | +import sys |
| 4 | +import time |
| 5 | +import vgamepad as vg # Import vgamepad |
| 6 | + |
| 7 | +def get_motor_positions(ip_address, port, retries=60, delay=0.5): |
| 8 | + """Attempts to receive motor positions from the specified IP and port.""" |
| 9 | + for attempt in range(retries): |
| 10 | + try: |
| 11 | + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: |
| 12 | + s.connect((ip_address, port)) |
| 13 | + data = s.recv(1024) |
| 14 | + received_str = data.decode() |
| 15 | + # Parse the motor positions and touch state |
| 16 | + pos_a_degrees = received_str.split(",")[0].split(":")[1].strip().split()[0] |
| 17 | + pos_b_degrees = received_str.split(",")[1].split(":")[1].strip().split()[0] |
| 18 | + pos_c_degrees = received_str.split(",")[2].split(":")[1].strip().split()[0] |
| 19 | + touch_state = received_str.split(",")[3].split(":")[1].strip() |
| 20 | + return pos_a_degrees, pos_b_degrees, pos_c_degrees, touch_state |
| 21 | + except (IndexError, ValueError, socket.error) as e: |
| 22 | + print(f"Error receiving motor positions (attempt {attempt + 1}/{retries}): {e}") |
| 23 | + time.sleep(delay) |
| 24 | + return "N/A", "N/A", "N/A", "N/A" |
| 25 | + |
| 26 | +def main(): |
| 27 | + ip_address = 'EV3-IP' # [LOCAL] IP of the device sending motor data (replaced random local ip with placeholder). (if using custom connection might not be needed) |
| 28 | + port = 12345 # Port to connect to (default port). |
| 29 | + |
| 30 | + # Set limits for motor positions |
| 31 | + limit_a = 40 |
| 32 | + limit_b = 45 |
| 33 | + limit_c = 50 |
| 34 | + |
| 35 | + pygame.init() |
| 36 | + screen = pygame.display.set_mode((400, 300)) |
| 37 | + pygame.display.set_caption('Motor Positions') |
| 38 | + font = pygame.font.Font(None, 36) |
| 39 | + clock = pygame.time.Clock() |
| 40 | + |
| 41 | + # Initialize vGamepad (Xbox 360 Controller) |
| 42 | + gamepad = vg.VX360Gamepad() |
| 43 | + print("vGamepad initialized!") |
| 44 | + |
| 45 | + while True: |
| 46 | + for event in pygame.event.get(): |
| 47 | + if event.type == pygame.QUIT: |
| 48 | + pygame.quit() |
| 49 | + sys.exit() |
| 50 | + |
| 51 | + pos_a_degrees, pos_b_degrees, pos_c_degrees, touch_state = get_motor_positions(ip_address, port) |
| 52 | + |
| 53 | + if pos_a_degrees != "N/A": |
| 54 | + pos_a_degrees = max(min(float(pos_a_degrees), limit_a), -limit_a) |
| 55 | + if pos_b_degrees != "N/A": |
| 56 | + pos_b_degrees = max(min(float(pos_b_degrees), limit_b), -limit_b) |
| 57 | + if pos_c_degrees != "N/A": |
| 58 | + pos_c_degrees = max(min(float(pos_c_degrees), limit_c), -limit_c) |
| 59 | + |
| 60 | + # Scale motor positions to joystick axis range (-32768 to 32767) |
| 61 | + scaled_pos_a = int((float(pos_a_degrees) / limit_a) * 32767) |
| 62 | + scaled_pos_b = int((float(pos_b_degrees) / limit_b) * 32767) |
| 63 | + |
| 64 | + # Scale motor C to right joystick Y-axis (-32768 to 32767) |
| 65 | + if pos_c_degrees == "N/A": |
| 66 | + scaled_pos_c = 0 # Default to center if data is invalid |
| 67 | + else: |
| 68 | + scaled_pos_c = int((float(pos_c_degrees) / limit_c) * 32767) |
| 69 | + |
| 70 | + # Send values to virtual gamepad |
| 71 | + gamepad.left_joystick(x_value=scaled_pos_a, y_value=scaled_pos_b) # Left joystick |
| 72 | + gamepad.right_joystick(x_value=0, y_value=scaled_pos_c) # Right joystick Y-axis |
| 73 | + |
| 74 | + # Set button state |
| 75 | + if touch_state == "1": |
| 76 | + gamepad.press_button(vg.XUSB_BUTTON.XUSB_GAMEPAD_A) |
| 77 | + else: |
| 78 | + gamepad.release_button(vg.XUSB_BUTTON.XUSB_GAMEPAD_A) |
| 79 | + |
| 80 | + gamepad.update() # Apply changes to virtual controller |
| 81 | + |
| 82 | + # Display motor positions on screen |
| 83 | + screen.fill((255, 255, 255)) |
| 84 | + text_a = font.render(f"Motor A Position: {pos_a_degrees} degrees", True, (0, 0, 0)) |
| 85 | + text_b = font.render(f"Motor B Position: {pos_b_degrees} degrees", True, (0, 0, 0)) |
| 86 | + text_c = font.render(f"Motor C Position: {pos_c_degrees} degrees", True, (0, 0, 0)) |
| 87 | + text_touch = font.render(f"Button State: {touch_state}", True, (0, 0, 0)) |
| 88 | + screen.blit(text_a, (20, 50)) |
| 89 | + screen.blit(text_b, (20, 100)) |
| 90 | + screen.blit(text_c, (20, 150)) |
| 91 | + screen.blit(text_touch, (20, 200)) |
| 92 | + |
| 93 | + pygame.display.flip() |
| 94 | + clock.tick(1000) # Update every 1 millisecond |
| 95 | + |
| 96 | +if __name__ == "__main__": |
| 97 | + main() |
0 commit comments