controller demofun

1 files changed, 51 insertions, 0 deletions

diff --git a/robot/test/dummy.py b/robot/test/dummy.py
new file mode 100644
index 0000000..5265fa0
--- /dev/null
+++ b/robot/test/dummy.py
@@ -0,0 +1,51 @@
+import pygame
+import serial
+pygame.init()
+joysticks = []
+clock = pygame.time.Clock()
+keepPlaying = True
+
+left_right_stick = 0
+forward_stick = 0
+backward_stick = 0
+
+def sign(num):
+  if num == 0:
+    return 0
+  elif num > 0:
+    return 1
+  else:
+    return -1
+
+serport = serial.Serial("/dev/ttyACM0", 9600, timeout=1)
+def send_speed(left, right):
+  total = ""
+  total += f"{(sign(left) + 1):02x}"
+  total += f"{(abs(left)):02x}"
+  total += f"{(sign(right) + 1):02x}"
+  total += f"{(abs(right)):02x}"
+
+  serport.write(bytes(total, 'utf-8'))
+  serport.flush()
+  print(total)
+
+for i in range(0, pygame.joystick.get_count()):
+  joysticks.append(pygame.joystick.Joystick(i))
+  joysticks[-1].init()
+  print ("Detected joystick "),joysticks[-1].get_name(),"'"
+
+def make_values():
+  left = 255 - max(0, left_right_stick) * 255
+  right = 255 + min(0, left_right_stick) * 255
+  speed_mod = backward_stick + forward_stick
+  return (int(left * speed_mod) , int(right * speed_mod))
+
+while keepPlaying:
+  clock.tick(60)
+  values = make_values()
+  send_speed(values[0], values[1])
+  for event in pygame.event.get():
+    if event.type != 1536: continue
+    if event.axis == 0: left_right_stick = event.value
+    if event.axis == 2: backward_stick = event.value * -0.5 - 1
+    if event.axis == 5: forward_stick = event.value * 0.5 + 1