forked from piborg/zeroborg
-
Notifications
You must be signed in to change notification settings - Fork 0
/
zbJoystick.py
163 lines (155 loc) · 6.21 KB
/
zbJoystick.py
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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
#!/usr/bin/env python
# coding: Latin-1
# Load library functions we want
import time
import os
import sys
import pygame
import ZeroBorg
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
# Setup the ZeroBorg
ZB = ZeroBorg.ZeroBorg()
#ZB.i2cAddress = 0x44 # Uncomment and change the value if you have changed the board address
ZB.Init()
if not ZB.foundChip:
boards = ZeroBorg.ScanForZeroBorg()
if len(boards) == 0:
print 'No ZeroBorg found, check you are attached :)'
else:
print 'No ZeroBorg at address %02X, but we did find boards:' % (ZB.i2cAddress)
for board in boards:
print ' %02X (%d)' % (board, board)
print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
print 'ZB.i2cAddress = 0x%02X' % (boards[0])
sys.exit()
#ZB.SetEpoIgnore(True) # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
ZB.SetCommsFailsafe(True)
failsafe = ZB.GetCommsFailsafe()
if failsafe:
break
if not failsafe:
print 'Board %02X failed to report in failsafe mode!' % (ZB.i2cAddress)
sys.exit()
ZB.ResetEpo()
# Settings for the joystick
axisUpDown = 1 # Joystick axis to read for up / down position
axisUpDownInverted = False # Set this to True if up and down appear to be swapped
axisLeftRight = 2 # Joystick axis to read for left / right position
axisLeftRightInverted = False # Set this to True if left and right appear to be swapped
buttonResetEpo = 3 # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8 # Joystick button number for driving slowly whilst held (L2)
slowFactor = 0.5 # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
buttonFastTurn = 9 # Joystick button number for turning fast (R2)
interval = 0.00 # Time between updates in seconds, smaller responds faster but uses more processor time
# Power settings
voltageIn = 8.4 # Total battery voltage to the ZeroBorg (change to 9V if using a non-rechargeable battery)
voltageOut = 6.0 # Maximum motor voltage
# Setup the power limits
if voltageOut > voltageIn:
maxPower = 1.0
else:
maxPower = voltageOut / float(voltageIn)
# Setup pygame and wait for the joystick to become available
ZB.MotorsOff()
os.environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
#pygame.display.set_mode((1,1))
print 'Waiting for joystick... (press CTRL+C to abort)'
while True:
try:
try:
pygame.joystick.init()
# Attempt to setup the joystick
if pygame.joystick.get_count() < 1:
# No joystick attached, toggle the LED
ZB.SetLed(not ZB.GetLed())
pygame.joystick.quit()
time.sleep(0.1)
else:
# We have a joystick, attempt to initialise it!
joystick = pygame.joystick.Joystick(0)
break
except pygame.error:
# Failed to connect to the joystick, toggle the LED
ZB.SetLed(not ZB.GetLed())
pygame.joystick.quit()
time.sleep(0.1)
except KeyboardInterrupt:
# CTRL+C exit, give up
print '\nUser aborted'
ZB.SetLed(True)
sys.exit()
print 'Joystick found'
joystick.init()
ZB.SetLed(False)
try:
print 'Press CTRL+C to quit'
driveLeft = 0.0
driveRight = 0.0
running = True
hadEvent = False
upDown = 0.0
leftRight = 0.0
# Loop indefinitely
while running:
# Get the latest events from the system
hadEvent = False
events = pygame.event.get()
# Handle each event individually
for event in events:
if event.type == pygame.QUIT:
# User exit
running = False
elif event.type == pygame.JOYBUTTONDOWN:
# A button on the joystick just got pushed down
hadEvent = True
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
hadEvent = True
if hadEvent:
# Read axis positions (-1 to +1)
if axisUpDownInverted:
upDown = -joystick.get_axis(axisUpDown)
else:
upDown = joystick.get_axis(axisUpDown)
if axisLeftRightInverted:
leftRight = -joystick.get_axis(axisLeftRight)
else:
leftRight = joystick.get_axis(axisLeftRight)
# Apply steering speeds
if not joystick.get_button(buttonFastTurn):
leftRight *= 0.5
# Determine the drive power levels
driveLeft = -upDown
driveRight = -upDown
if leftRight < -0.05:
# Turning left
driveLeft *= 1.0 + (2.0 * leftRight)
elif leftRight > 0.05:
# Turning right
driveRight *= 1.0 - (2.0 * leftRight)
# Check for button presses
if joystick.get_button(buttonResetEpo):
ZB.ResetEpo()
if joystick.get_button(buttonSlow):
driveLeft *= slowFactor
driveRight *= slowFactor
# Set the motors to the new speeds
ZB.SetMotor1(-driveLeft * maxPower)
ZB.SetMotor2(-driveLeft * maxPower)
ZB.SetMotor3(driveRight * maxPower)
ZB.SetMotor4(driveRight * maxPower)
# Change the LED to reflect the status of the EPO latch
ZB.SetLed(ZB.GetEpo())
# Wait for the interval period
time.sleep(interval)
# Disable all drives
ZB.MotorsOff()
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
ZB.MotorsOff()
print