You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
80 lines
2.1 KiB
80 lines
2.1 KiB
4 years ago
|
#!/usr/bin/env python3
|
||
|
|
|
||
|
|
import sys, argparse
|
||
|
|
import time
|
||
|
|
import threading
|
||
|
|
|
||
|
|
import serial
|
||
|
|
import pygame
|
||
|
|
|
||
|
|
|
||
|
|
def serialreceive(ser):
|
||
|
|
print("Serial running")
|
||
|
|
while ser:
|
||
|
|
try:
|
||
|
|
if ser.in_waiting > 0:
|
||
|
|
line = ser.read(ser.in_waiting)
|
||
|
|
print(line)
|
||
|
|
except Exception:
|
||
|
|
pass
|
||
|
|
|
||
|
|
|
||
|
|
def main(argv):
|
||
|
|
parser = argparse.ArgumentParser()
|
||
|
|
parser.add_argument('-P', '--serialport', help="serial port to open", default="/dev/ttyUSB0")
|
||
|
|
parser.add_argument('--delay', help="add delay before startup", type=int, default=1)
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
pygame.init()
|
||
|
|
pygame.joystick.init()
|
||
|
|
joysticks = [pygame.joystick.Joystick(x) for x in range(pygame.joystick.get_count())]
|
||
|
|
for joystick in joysticks:
|
||
|
|
joystick.init()
|
||
|
|
name = joystick.get_name()
|
||
|
|
print("Found joystick: ")
|
||
|
|
print(name)
|
||
|
|
|
||
|
|
with serial.Serial(args.serialport, 115200, timeout=12) as ser:
|
||
|
|
receivethread = threading.Thread(target=serialreceive, args=(ser,))
|
||
|
|
receivethread.daemon = True
|
||
|
|
receivethread.start()
|
||
|
|
time.sleep(args.delay)
|
||
|
|
|
||
|
|
axis_data = {0:0,1:0,2:0,3:0,4:0}
|
||
|
|
try:
|
||
|
|
while True:
|
||
|
|
for event in pygame.event.get():
|
||
|
|
if event.type == pygame.JOYAXISMOTION:
|
||
|
|
axis_data[event.axis] = round(event.value,2)
|
||
|
|
if abs(axis_data[event.axis]) <= 0.15:
|
||
|
|
axis_data[event.axis] = 0;
|
||
|
|
print(axis_data)
|
||
|
|
|
||
|
|
x = axis_data[0]
|
||
|
|
y = axis_data[1]
|
||
|
|
r = axis_data[3]
|
||
|
|
m0 = +y - x - r
|
||
|
|
m1 = +y + x - r
|
||
|
|
m2 = -y + x - r
|
||
|
|
m3 = -y - x - r
|
||
|
|
m0 *= 4000
|
||
|
|
m1 *= 4000
|
||
|
|
m2 *= 4000
|
||
|
|
m3 *= 4000
|
||
|
|
command = 'S '+str(int(m0))+','+str(int(m1))+','+str(int(m2))+','+str(int(m3))+'\n'
|
||
|
|
ser.write(bytes(command, "utf8"))
|
||
|
|
#ser.flush()
|
||
|
|
#time.sleep(0.05)
|
||
|
|
print(command)
|
||
|
|
except KeyboardInterrupt:
|
||
|
|
print("KeyboardInterrupt. Exiting")
|
||
|
|
|
||
|
|
#ser.write(b'S 800,800,-800,-800\n')
|
||
|
|
#time.sleep(2)
|
||
|
|
ser.write(b'DISABLE')
|
||
|
|
|
||
|
|
print("serial closed")
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
main(sys.argv[1:])
|