MeArm/joystick.py at master · ForToffee/MeArm · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
import pygame, sys, time, os
import servoblst as servo

os.environ["SDL_VIDEODRIVER"] = "dummy"

pygame.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()
screen = pygame.display.set_mode((400,300))

sc = servo.ServoController()
grip = 1
elbow = 3
shoulder = 4
hip = 5

def reset():
	sc.setAngle(grip, 90)
	sc.setAngle(elbow, 0)
	sc.setAngle(shoulder, 25)
	sc.setAngle(hip, 0)

threshold = 0.5
def handleMovement(servo_id, event_value, inc=2):
	ret = ''
	if event_value > threshold:
		ret = sc.incAngle(servo_id, inc)
	elif event_value < threshold * -1:
		ret = sc.incAngle(servo_id, inc * -1)

	if ret <> '':
		print "servo %d = %d degrees" % (servo_id, ret)

reset()
interval = 0.1
try:
	run = True
	while run:
		events = pygame.event.get()
		for event in events:

			# Check if one of the joysticks has moved
			if event.type == pygame.JOYAXISMOTION:
				if event.axis == 1:
					handleMovement(elbow, event.value)

				if event.axis == 2:
					handleMovement(hip, event.value * -1)

				if event.axis == 3:
					handleMovement(shoulder, event.value * -1)

			if event.type == pygame.JOYBUTTONDOWN:
				if event.button == 0:	#select
					run = False
				if event.button == 3:	#start
					reset()
				if event.button == 12:	#triangle
					sc.setAngle(grip, 90)
				if event.button == 14:	#X
					sc.setAngle(grip, 30)
				else:
					print event.button

		time.sleep(interval)
except KeyboardInterrupt:
	pass

except Exception as e:
	print e
	pass

reset()
sc.clean_up()