Initial commit

pioneer-x360.py

@@ -0,0 +1,64 @@
+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()
+