Motors off when ps3 controller is too far away

Submitted by Thieu on Tue, 11/17/2020 - 17:12

Forums:

Dear Piborg,

Is it possible to let the motors stop spinning when the ps3 controller is too far away, somekind of safety.

Greetings Thieu

piborg's picture

piborg

Wed, 11/18/2020 - 10:50

It is possible, we usually do this by checking how long it has been since the controller has provided an update for the inputs.

If your code is polling the controller inputs (checking them at a regular interval), you can check how long it has been since an input change by counting how many times you have polled without any update.

The early DoodleBorg code has this feature: DoodleBorg - Controlled using a PS3 controller.

In the DoddleBorg code we keep a loopsWithoutEvent counter. If we do not see an event for a polling interval the counter is increased by one. Whenever we see an event the counter is reset to zero. When loopsWithoutEvent exceeds controllerLostLoops we decide the controller has lost contact and we stop the motors.

If your code blocks until a new event happens you will need a watchdog thread instead. This runs in the background and will turn motors off if the time since the last update is too long.

Our WebUI examples have this feature: monsterWeb.py.

In this code the class Watchdog is a thread that turns the motors off when its event property has not been set recently. The code sets the event by calling watchdog.event.set() when it gets an update, where watchdog is an instance of the Watchdog class. The time delay is set by the if self.event.wait(1): line, where 1 means 1 second.

Thieu

Wed, 11/25/2020 - 18:24

Dear Piborg, i changed my code with doodleborg code but it seems that is doesnt work it gives a few faults but i dont know what. Here is the code that im working with it works with 2 diablos. Greetings Thieu.

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
# Load library functions we want
from __future__ import print_function
from diablo import *
from time import sleep
from os import environ
from sys import exit, stdout, stderr
import pygame
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(7, GPIO.OUT, initial=GPIO.LOW)
 
# Power settings
voltageIn = 24.0              # Total battery voltage to the Diablos
voltageOut = 18.0             # Maximum motor voltage
  
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)
 
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
stdout = stderr
 
# Setup the left Diablo
DIABLO1 = Diablo()
DIABLO1.i2cAddress = 38
DIABLO1.Init()
if not DIABLO1.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO1.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO1.ResetEpo()
  
# Setup the right Diablo
DIABLO2 = Diablo()
DIABLO2.i2cAddress = 55
DIABLO2.Init()
if not DIABLO2.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO2.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO2.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 = 16                     # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving fast whilst held (L2)
slowFactor = 1.0                        # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed
buttonFastTurn = 9                      # Joystick button number for turning fast (R2)
buttoncross = 14                        # Button cross for lights on               
buttoncircle = 13                       # Button circle for lights off
buttonsquare = 15                       # Button square for claxon on
buttontriangle = 12                     # Button triangle for claxon off
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20                # Number of loops til diablos shut down
 
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
print ('Waiting for joystick... (press CTRL+C to quit)')
while True:
    try:
        try:
            pygame.joystick.init()
            # Attempt to setup the joystick
            if pygame.joystick.get_count() < 1:
                pygame.joystick.quit()
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print ('\nUser aborted')
print ('Joystick found')
joystick.init()
 
try:
    print('Motors are ready to drive.')
    print('Press CTRL+C to quit')
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    loopsWithoutEvent = 0
    controllerLost = False
    # 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
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.HIGH)
                if event.button == buttoncircle:
                    GPIO.output(8, GPIO.LOW)
                if event.button == buttonsquare:
                    GPIO.output(7, GPIO.HIGH)
                if event.button == buttontriangle:
                    GPIO.output(7, GPIO.LOW)
                if event.button == r1:
            axisUpDownInverted = 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):
                    DIABLO.ResetEpo()
                if not joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                # Set the motors to the new speeds
                    DIABLO1.SetMotors(driveLeft * maxPower)
                    DIABLO2.SetMotors(driveRight * maxPower)
                if hadEvent:
                # Reset the controller lost counter
                    loopsWithoutEvent = 0
                    if controllerLost:
                # We had lost the controller, we have now found it again
                        print ('Controller re-connected, move joystick to resume operation')
                for DIABLO1 in Diablo:
                    controllerLost = False
                for DIABLO2 in Diablo:
                    controllerLost = False
                # Attempt to reset the joystick module
                del joystick
                pygame.joystick.quit()
                pygame.joystick.init()
                if pygame.joystick.get_count() < 1:
                    # Controller has been disconnected, poll for reconnection
                    print ('Controller disconnected!')
                    while pygame.joystick.get_count() < 1:
                        time.sleep(interval * (controllerLostLoops / 10))
                        pygame.joystick.quit()
                        pygame.joystick.init()
                        # Grab the joystick again
                joystick = pygame.joystick.Joystick(0)
                joystick.init()
                continue
            # Skip to the next loop after the interval
            time.sleep(interval)
            continue
        else:
            # No events this loop, check if it has been too long since we saw an event
            loopsWithoutEvent += 1
            if loopsWithoutEvent > controllerLostLoops:
                # It has been too long, disable control!
                print ('Controller lost!')
                for DIABLO1 in Diablo:
                    DIABLO1.MotorsOff()
                    DIABLO2.MotorsOff()
                for DIABLO2 in Diablo:
                    DIABLO1.MotorsOff()
                    DIABLO2.MotorsOff()
                controllerLost = True
            # Wait for the interval period
        sleep(interval)
    # Disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.ResetEpo()
    DIABLO2.ResetEpo()
    print('Terminated')
print()
GPIO.cleanup()

Thieu

Wed, 11/25/2020 - 19:48

Maybe this code is a little better
Greetings

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
# Load library functions we want
from __future__ import print_function
from diablo import *
from time import sleep
from os import environ
from sys import exit, stdout, stderr
import pygame
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
 
# Power settings
voltageIn = 24.0              # Total battery voltage to the Diablo
voltageOut = 18.5            # Maximum motor voltage
  
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)
 
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
stdout = stderr
 
# Setup the left Diablo
DIABLO1 = Diablo()
DIABLO1.i2cAddress = 38
DIABLO1.Init()
if not DIABLO1.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO1.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO1.ResetEpo()
  
# Setup the right Diablo
DIABLO2 = Diablo()
DIABLO2.i2cAddress = 55
DIABLO2.Init()
if not DIABLO2.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO2.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO2.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 = 3                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 16                     # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving fast whilst held (L2)
slowFactor = 1.0                        # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed
buttonFastTurn = 7                      # Joystick button number for turning fast (R2)
buttoncross = 0                        # Button cross for lights on               
buttoncircle = 1                       # Button circle for lights off
buttonsquare = 3                        # Button square for claxon on
buttontriangle = 2                      # Button triangle for claxon off
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20                # Number of loops til diablos shut down
 
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
pygame.init()
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:
                pygame.joystick.quit()
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print ('\nUser aborted')
print ('Joystick found')
joystick.init()
 
try:
    print('Motors are ready to drive.')
    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
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.HIGH)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.HIGH)
            elif event.type == pygame.JOYBUTTONUP:
                # A button on the joystick just got released
                hadEvent = True
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.LOW)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.LOW)
            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):
                    DIABLO.ResetEpo()
                if not joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                # Set the motors to the new speeds
                    DIABLO1.SetMotors(driveLeft * maxPower)
                    DIABLO2.SetMotors(driveRight * maxPower)
                if hadEvent:
                # Reset the controller lost counter
                    loopsWithoutEvent = 0
                    if controllerLost:
                # We had lost the controller, we have now found it again
                        print ('Controller re-connected, move joystick to resume operation')
                for Diablox in DIABLO:
                    controllerLost = False
                # Attempt to reset the joystick module
                del joystick
                pygame.joystick.quit()
                pygame.joystick.init()
                if pygame.joystick.get_count() < 1:
                    # Controller has been disconnected, poll for reconnection
                    print ('Controller disconnected!')
                    while pygame.joystick.get_count() < 1:
                        time.sleep(interval * (controllerLostLoops / 10))
                        pygame.joystick.quit()
                        pygame.joystick.init()
                        # Grab the joystick again
                joystick = pygame.joystick.Joystick(0)
                joystick.init()
                continue
            # Skip to the next loop after the interval
            time.sleep(interval)
            continue
        else:
            # No events this loop, check if it has been too long since we saw an event
            loopsWithoutEvent += 1
            if loopsWithoutEvent > controllerLostLoops:
                # It has been too long, disable control!
                print ('Controller lost!')
                for Diablox in DIABLO:
                    DIABLO1.MotorsOff()
                    DIABLO2.MotorsOff()
                controllerLost = True
        # Wait for the interval period
        sleep(interval)
    # Disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.ResetEpo()
    DIABLO2.ResetEpo()
    print('Terminated')
print()
GPIO.cleanup()
piborg's picture

piborg

Thu, 11/26/2020 - 17:06

I think you are close, but there are a few things that need to be sorted to get things working :)

The first is that the indenting is a bit mixed up, probably because of cutting and pasting from another script. Unfortunately Python does not like a mixture of spaces and tabs for the indenting. I have attached an image of where the tabs are below (shown by the red dots) so you can see what I mean.

The second thing is not really a problem, simply that you do not need the if hadEvent: line twice. The inner one can be removed as it has already been checked at that point in the code :)

The third issue is the for Diablox in DIABLO: lines. In both cases these are invalid (there is no DIABLO), but the lines under them are correct ones. Also the DIABLO.ResetEpo() line should be two lines for DIABLO1 and DIABLO2.

The fourth part is the # Attempt to reset the joystick module section. This is intended to handle a pygame errors losing the controller entirely and recover, but it requires other detection code to work. Instead we can have the script turn the motors off and quit in the event of any unexpected error by adding another except block at the end without any error type.

The final part is just a value change. Currently the value of interval is 0, meaning it will check for events as fast as possible. This means the 20 loops set by controllerLostLoops may be very quick, making the lost detection too sensitive. I would set interval to 0.1 (ten checks per second maximum) to mean at least 2 seconds (20 × 0.1 = 2) without any update from the controller will trigger the detection. Once it works you can adjust either of these values to change the minimum delay.

I have made these changes (they sound worse then they actually are) and have attached the updated script below. Give it a try and let me know if it works or if there are still problems :)

Images: 
Attachments: 
Thieu.txt

Thieu

Fri, 11/27/2020 - 18:37

Hallo there i tested the code but it seems to be that line 216 is wrong. I have a picture of it attached
Greetings

Images: 
piborg's picture

piborg

Fri, 11/27/2020 - 20:15

Line 216 should be:

1
    print("Unexpected error:", exc_info()[0])

Thieu

Sat, 11/28/2020 - 06:44

Hello, i removed the "sys" but still getting an error

Images: 
piborg's picture

piborg

Sat, 11/28/2020 - 10:40

NameError suggests that there is another mistake in the code earlier on.

Comment out the unexpected error handling code:

1
2
3
4
5
6
7
8
9
10
11
12
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
    print('Terminated')
#except:
#    # Unexpected error, disable all drives
#    DIABLO1.MotorsOff()
#    DIABLO2.MotorsOff()
#    print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()

This will let Python handle the error itself, which should give us the line number for the problem :)

Thieu

Mon, 11/30/2020 - 05:58

Dear Piborg, the code almost works there is only one issue.
When i leave the interval on 0.1 it seems that the ps3 controller reacts very slow when i change it to 0.05 it reacts better but the motors sometimes stop for a few millisecs and than turns again.

Greetings Thieu

piborg's picture

piborg

Mon, 11/30/2020 - 11:03

It looks like I missed a couple of problems in the code :(

The first is that it is updating the motors and waiting for each input changed, it should be just once per interval. There are also two sleep calls instead of just one.

The second is that the else block on line 183 does not line up with an if block like it is intended to.

This version should work better and without the strange behaviour from before. You can lower the interval again if the controls respond too slowly like before.

Attachments: 
Thieu.txt

Thieu

Sun, 12/27/2020 - 18:15

Maybe intresting for some other raspbery enthusiasts, some pictures of my covid project.

Images: 

Thieu

Sun, 12/27/2020 - 18:19

Rest of the pictures.

Images: 

Thieu

Mon, 03/29/2021 - 18:42

Dear Piborg, i have still a fault in the program. When i connect the controller it immediatly gives the controller lost message on the terminal window, i dont know whats wrong. Ive attached the code.
Greetings Thieu

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
# Load library functions we want
from __future__ import print_function
from diablo import *
from time import sleep
from os import environ
from sys import exit, stdout, stderr
import pygame
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
 
# Power settings
voltageIn = 24.0              # Total battery voltage to the Diablo
voltageOut = 12.0            # Maximum motor voltage
  
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)
 
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
stdout = stderr
 
# Setup the left Diablo
DIABLO1 = Diablo()
DIABLO1.i2cAddress = 38
DIABLO1.Init()
if not DIABLO1.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO1.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO1.ResetEpo()
  
# Setup the right Diablo
DIABLO2 = Diablo()
DIABLO2.i2cAddress = 55
DIABLO2.Init()
if not DIABLO2.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO2.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO2.ResetEpo()
 
 
# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = True               # Set this to True if up and down appear to be swapped
axisLeftRight = 3                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 16                     # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving fast whilst held (L2)
slowFactor = 1.0                        # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed
buttonFastTurn = 7                      # Joystick button number for turning fast (R2)
buttoncross = 0                         # Joystick button cross              
buttoncircle = 1                        # Joystick button circle
buttontriangle = 2                      # Joystick button triangle
buttonsquare = 3                        # Joystick button square
buttonL1 = 4                            # Joystick button L1
buttonR1 = 5                            # Joystick button R1
interval = 0.1                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
 
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
pygame.init()
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:
                pygame.joystick.quit()
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print ('\nUser aborted')
print ('Joystick found')
joystick.init()
 
try:
    print('Motors are ready to drive.')
    print('Press CTRL+C to quit')
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    loopsWithoutEvent = 0
    controllerLost False
    # 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
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.HIGH)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.HIGH)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.HIGH)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.HIGH)
                if event.button == buttonL1:
                    axisUpDownInverted = True
                    axisLeftRightInverted = False
                if event.button == buttonR1:
                    axisUpDownInverted = False
                    axisLeftRightInverted = True
            elif event.type == pygame.JOYBUTTONUP:
                # A button on the joystick just got released
                hadEvent = True
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.LOW)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.LOW)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.LOW)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.LOW)
            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):
                    DIABLO.ResetEpo()
                if not joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                    DIABLO1.SetMotors(driveLeft * maxPower)
                    DIABLO2.SetMotors(driveRight * maxPower)
                    if controllerLost:
                        controllerLost = False
                        else:
                            loopsWithoutEvent += 1
                            if loopsWithoutEvent > controllerLostLoops:
                                print('Controller lost!')
                                DIABLO1.MotorsOff()
                                DIABLO2.MotorsOff()
                                controllerLost = True
                                sleep(interval)
                                # Disable all drives
                                DIABLO1.MotorsOff()
                                DIABLO2.MotorsOff()
                     
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.ResetEpo()
    DIABLO2.ResetEpo()
    print('Terminated')
except:
    # Unexpected error, disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
    print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()
piborg's picture

piborg

Tue, 03/30/2021 - 09:51

I think there are two problems with the code.

1. Indentation below the "Set the motors to the new speeds" comment.

The lines below the comment should be shifted to the left so that the DIABLO1.SetMotors(driveLeft * maxPower) line starts in the same column as the comment itself.

2. The sleep(interval) line is in the wrong place.

You want the outer while running: loop to have a delay at the end every time, not just in some cases. Move the sleep(interval) line down so it is the last line in the loop and change the indentation so it starts in the same column as the if hadEvent: line further up.

Thieu

Tue, 03/30/2021 - 18:48

Dear Piborg, ive tried what you said and a few other things but its still not working fine. When i push the joystick forward so that the motors are turning and i walk away to disconnect the controller while the motors are turning, the controllers disconnect but the motors keep on turning and dont stop unless i restart the progam again. i also dont get the controller lost message on the terminal.

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
# Load library functions we want
from __future__ import print_function
from diablo import *
from time import sleep
from os import environ
from sys import exit, stdout, stderr
import pygame
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
 
# Power settings
voltageIn = 24.0              # Total battery voltage to the Diablo
voltageOut = 12.0            # Maximum motor voltage
  
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)
 
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
stdout = stderr
 
# Setup the left Diablo
DIABLO1 = Diablo()
DIABLO1.i2cAddress = 38
DIABLO1.Init()
if not DIABLO1.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO1.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO1.ResetEpo()
  
# Setup the right Diablo
DIABLO2 = Diablo()
DIABLO2.i2cAddress = 55
DIABLO2.Init()
if not DIABLO2.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO2.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO2.ResetEpo()
 
 
# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = True               # Set this to True if up and down appear to be swapped
axisLeftRight = 3                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 16                     # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving fast whilst held (L2)
slowFactor = 1.0                        # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed
buttonFastTurn = 7                      # Joystick button number for turning fast (R2)
buttoncross = 0                         # Joystick button cross              
buttoncircle = 1                        # Joystick button circle
buttontriangle = 2                      # Joystick button triangle
buttonsquare = 3                        # Joystick button square
buttonL1 = 4                            # Joystick button L1
buttonR1 = 5                            # Joystick button R1
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
 
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
pygame.init()
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:
                pygame.joystick.quit()
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print ('\nUser aborted')
print ('Joystick found')
joystick.init()
 
try:
    print('Motors are ready to drive.')
    print('Press CTRL+C to quit')
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    loopsWithoutEvent = 0
    controllerLost = False
    # 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
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.HIGH)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.HIGH)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.HIGH)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.HIGH)
                if event.button == buttonL1:
                    axisUpDownInverted = True
                    axisLeftRightInverted = False
                if event.button == buttonR1:
                    axisUpDownInverted = False
                    axisLeftRightInverted = True
            elif event.type == pygame.JOYBUTTONUP:
                # A button on the joystick just got released
                hadEvent = True
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.LOW)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.LOW)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.LOW)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.LOW)
            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):
                    DIABLO.ResetEpo()
                if not joystick.get_button(buttonSlow):
                    driveLeft *= slowFactor
                    driveRight *= slowFactor
                # Set the motors to the new speeds
                    DIABLO1.SetMotors(driveLeft * maxPower)
                    DIABLO2.SetMotors(driveRight * maxPower)
                    if hadEvent:
                        # Reset the controller lost counter
                        loopsWithoutEvent = 0
                        controllerLost = False
                    else:
                        loopsWithoutEvent += 1
                        if loopsWithoutEvent > controllerLostLoops:
                            print('Controller lost!')
                            DIABLO1.MotorsOff()
                            DIABLO2.MotorsOff()
                            controllerLost = True
                            # Disable all drives
                            DIABLO1.MotorsOff()
                            DIABLO2.MotorsOff()
            sleep(interval)
                     
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.ResetEpo()
    DIABLO2.ResetEpo()
    print('Terminated')
except:
    # Unexpected error, disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
    print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()
piborg's picture

piborg

Tue, 03/30/2021 - 23:14

The check to see if you have had input is within the larger if hadEvent: section, this means it cannot run when there is no input!

I have had a go at fixing things. Give this version a try:

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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
# Load library functions we want
from __future__ import print_function
from diablo import *
from time import sleep
from os import environ
from sys import exit, stdout, stderr
import pygame
import RPi.GPIO as GPIO
 
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(13, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(8, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(10, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(11, GPIO.OUT, initial=GPIO.LOW)
 
# Power settings
voltageIn = 24.0              # Total battery voltage to the Diablo
voltageOut = 12.0            # Maximum motor voltage
  
# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)
 
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
stdout = stderr
 
# Setup the left Diablo
DIABLO1 = Diablo()
DIABLO1.i2cAddress = 38
DIABLO1.Init()
if not DIABLO1.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO1.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO1.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO1.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO1.ResetEpo()
  
# Setup the right Diablo
DIABLO2 = Diablo()
DIABLO2.i2cAddress = 55
DIABLO2.Init()
if not DIABLO2.foundChip:
    boards = ScanForDiablo()
    if len(boards) == 0:
        print('No Diablo found, check you are attached :)')
    else:
        print('No Diablo at address %02X, but we did find boards:' % (DIABLO2.i2cAddress))
        for board in boards:
            print('    %02X (%d)' % (board, board))
        print('If you need to change the I2C address change the set-up line so it is correct, e.g.')
        print('DIABLO2.i2cAddress = 0x%02X' % (boards[0]))
    exit()
#DIABLO2.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
DIABLO2.ResetEpo()
 
 
# Settings for the joystick
axisUpDown = 1                          # Joystick axis to read for up / down position
axisUpDownInverted = True               # Set this to True if up and down appear to be swapped
axisLeftRight = 3                       # Joystick axis to read for left / right position
axisLeftRightInverted = False           # Set this to True if left and right appear to be swapped
buttonResetEpo = 16                     # Joystick button number to perform an EPO reset (Start)
buttonSlow = 8                          # Joystick button number for driving fast whilst held (L2)
slowFactor = 1.0                        # Speed to slow to when the drive fast button is not held, e.g. 0.5 would be half speed
buttonFastTurn = 7                      # Joystick button number for turning fast (R2)
buttoncross = 0                         # Joystick button cross              
buttoncircle = 1                        # Joystick button circle
buttontriangle = 2                      # Joystick button triangle
buttonsquare = 3                        # Joystick button square
buttonL1 = 4                            # Joystick button L1
buttonR1 = 5                            # Joystick button R1
interval = 0.00                         # Time between updates in seconds, smaller responds faster but uses more processor time
controllerLostLoops = 20
 
# Setup pygame and wait for the joystick to become available
environ["SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window
environ["SDL_AUDIODRIVER"] = "dummy" # Removes ALSA faults
pygame.init()
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:
                pygame.joystick.quit()
            else:
                # We have a joystick, attempt to initialise it!
                joystick = pygame.joystick.Joystick(0)
                break
        except pygame.error:
            # Failed to connect to the joystick
            pygame.joystick.quit()
            time.sleep(0.1)
    except KeyboardInterrupt:
        # CTRL+C exit, give up
        print ('\nUser aborted')
print ('Joystick found')
joystick.init()
 
try:
    print('Motors are ready to drive.')
    print('Press CTRL+C to quit')
    driveLeft = 0.0
    driveRight = 0.0
    running = True
    hadEvent = False
    upDown = 0.0
    leftRight = 0.0
    loopsWithoutEvent = 0
    controllerLost = False
    # 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
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.HIGH)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.HIGH)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.HIGH)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.HIGH)
                if event.button == buttonL1:
                    axisUpDownInverted = True
                    axisLeftRightInverted = False
                if event.button == buttonR1:
                    axisUpDownInverted = False
                    axisLeftRightInverted = True
            elif event.type == pygame.JOYBUTTONUP:
                # A button on the joystick just got released
                hadEvent = True
                if event.button == buttoncircle:
                    GPIO.output(13, GPIO.LOW)
                if event.button == buttoncross:
                    GPIO.output(8, GPIO.LOW)
                if event.button == buttonsquare:
                    GPIO.output(10, GPIO.LOW)
                if event.button == buttontriangle:
                    GPIO.output(11, GPIO.LOW)
            elif event.type == pygame.JOYAXISMOTION:
                # A joystick has been moved
                hadEvent = True
 
        # Update the motor output if any events occurred
        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):
                DIABLO.ResetEpo()
            if not joystick.get_button(buttonSlow):
                driveLeft *= slowFactor
                driveRight *= slowFactor
            # Set the motors to the new speeds
            DIABLO1.SetMotors(driveLeft * maxPower)
            DIABLO2.SetMotors(driveRight * maxPower)
            # Reset the controller lost counter
            loopsWithoutEvent = 0
            controllerLost = False
        elif not controllerLost:
            # No input, increment no event counter
            loopsWithoutEvent += 1
            if loopsWithoutEvent > controllerLostLoops:
                # Counter limit exceeded, disable all drives
                print('Controller lost!')
                DIABLO1.MotorsOff()
                DIABLO2.MotorsOff()
                controllerLost = True
        sleep(interval)
 
except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
    print('Terminated')
except:
    # Unexpected error, disable all drives
    DIABLO1.MotorsOff()
    DIABLO2.MotorsOff()
    print("Unexpected error:", exc_info()[0])
print()
GPIO.cleanup()

Thieu

Wed, 03/31/2021 - 17:26

This code works much better, many thanks. The only thing is that i have to restart the program when the controller is lost.

Greetings Thieu

Subscribe to Comments for &quot;Motors off when ps3 controller is too far away &quot;