65 lines
1.3 KiB
Python
65 lines
1.3 KiB
Python
|
from os import sys
|
||
|
|
from inputs import get_gamepad
|
||
|
|
from drive import RosAriaDriver
|
||
|
|
from queue import Queue
|
||
|
|
from threading import Thread
|
||
|
|
|
||
|
|
class PioneerX360():
|
||
|
|
|
||
|
|
def __init__(self, n):
|
||
|
|
self.pad = get_gamepad()
|
||
|
|
self.pioneer = RosAriaDriver('PIONIER' + n + '/')
|
||
|
|
self.axes = {
|
||
|
|
"Y": 0,
|
||
|
|
"X": 0
|
||
|
|
}
|
||
|
|
self.mapper = {
|
||
|
|
# "BTN_TL": if_button_pressed(pioneer.GripperUp),
|
||
|
|
# "BTN_TR": if_button_pressed(pioneer.GripperDown),
|
||
|
|
# "BTN_SOUTH": if_button_pressed(pioneer.GripperOpen),
|
||
|
|
# "BTN_WEST": if_button_pressed(pioneer.GripperClose),
|
||
|
|
# "ABS_Y": go_T,
|
||
|
|
"BTN_MODE": exit,
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
def if_button_pressed(fun):
|
||
|
|
def foo(state):
|
||
|
|
if state == 1:
|
||
|
|
fun()
|
||
|
|
return foo
|
||
|
|
|
||
|
|
def update_axes(self, event):
|
||
|
|
axis = event.code[4:]
|
||
|
|
deviation = event.state
|
||
|
|
self.axes[axis] = deviation
|
||
|
|
|
||
|
|
|
||
|
|
def publish_cmd(self):
|
||
|
|
print(self.axes)
|
||
|
|
if self.axes["Y"] < -10000:
|
||
|
|
self.pioneer.SetSpeed(0.1, 0, 0)
|
||
|
|
elif self.axes["Y"] > 10000:
|
||
|
|
self.pioneer.SetSpeed(-0.1, 0, 0)
|
||
|
|
else:
|
||
|
|
self.pioneer.SetSpeed(0, 0, 0)
|
||
|
|
|
||
|
|
def run(self):
|
||
|
|
while 1:
|
||
|
|
events = get_gamepad()
|
||
|
|
self.publish_cmd()
|
||
|
|
for event in events:
|
||
|
|
self.publish_cmd()
|
||
|
|
if event.code[0:3] == "ABS":
|
||
|
|
self.update_axes(event)
|
||
|
|
else:
|
||
|
|
try:
|
||
|
|
self.mapper[event.code](event.state)
|
||
|
|
except KeyError:
|
||
|
|
pass
|
||
|
|
# print(event.code, event.state)
|
||
|
|
|
||
|
|
p = PioneerX360(sys.argv[1])
|
||
|
|
p.run()
|
||
|
|
|