132 lines
4.3 KiB
Python
Executable File
132 lines
4.3 KiB
Python
Executable File
#!/usr/bin/env python
|
|
# coding: utf-8
|
|
|
|
# Load the libraries
|
|
import sys
|
|
import Gamepad
|
|
import time
|
|
sys.path.insert(0, "/home/pi/rockyborg")
|
|
import RockyBorg
|
|
|
|
# Settings for the gamepad
|
|
gamepadType = Gamepad.PS4 # Class for the gamepad (e.g. Gamepad.PS3)
|
|
joystickSpeed = 'LEFT-Y' # Joystick axis to read for up / down position
|
|
joystickSpeedInverted = True # Set this to True if up and down appear to be swapped
|
|
joystickSteering = 'RIGHT-X' # Joystick axis to read for left / right position
|
|
joystickSteeringInverted = False # Set this to True if left and right appear to be swapped
|
|
buttonSlow = 'L2' # Joystick button for driving slowly whilst held
|
|
slowFactor = 0.5 # Speed to slow to when the drive slowly button is held, e.g. 0.5 would be half speed
|
|
buttonExit = 'PS' # Joystick button to end the program
|
|
interval = 0.05 # Time between motor updates in seconds, smaller responds faster but uses more processor time
|
|
|
|
# Power settings
|
|
voltageIn = 1.2 * 8 # Total battery voltage to the RockyBorg
|
|
voltageOut = 6.0 # Maximum motor voltage
|
|
|
|
# Setup the power limits
|
|
if voltageOut > voltageIn:
|
|
maxPower = 1.0
|
|
else:
|
|
maxPower = voltageOut / float(voltageIn)
|
|
|
|
# Setup the RockyBorg
|
|
RB = RockyBorg.RockyBorg()
|
|
#RB.i2cAddress = 0x21 # Uncomment and change the value if you have changed the board address
|
|
RB.Init()
|
|
if not RB.foundChip:
|
|
boards = RockyBorg.ScanForRockyBorg()
|
|
if len(boards) == 0:
|
|
print('No RockyBorg found, check you are attached :)')
|
|
else:
|
|
print('No RockyBorg at address %02X, but we did find boards:' % (RB.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('RB.i2cAddress = 0x%02X' % (boards[0]))
|
|
sys.exit()
|
|
|
|
# Enable the motors and disable the failsafe
|
|
RB.SetCommsFailsafe(False)
|
|
RB.MotorsOff()
|
|
RB.SetMotorsEnabled(True)
|
|
|
|
# Setup the state shared with callbacks
|
|
global running
|
|
running = True
|
|
|
|
# Create the callback functions
|
|
def exitButtonPressed():
|
|
global running
|
|
print('EXIT')
|
|
running = False
|
|
|
|
# Wait for a connection
|
|
RB.MotorsOff()
|
|
RB.SetLed(True)
|
|
waitingToggle = True
|
|
if not Gamepad.available():
|
|
print('Please connect your gamepad...')
|
|
while not Gamepad.available():
|
|
time.sleep(interval * 4)
|
|
waitingToggle = not waitingToggle
|
|
if waitingToggle:
|
|
RB.SetLed(False)
|
|
else:
|
|
RB.SetLed(True)
|
|
print('Gamepad connected')
|
|
gamepad = gamepadType()
|
|
|
|
# Start the background updating
|
|
gamepad.startBackgroundUpdates()
|
|
RB.SetLed(True)
|
|
|
|
# Register the callback functions
|
|
gamepad.addButtonPressedHandler(buttonExit, exitButtonPressed)
|
|
|
|
# Keep running while joystick updates are handled by the callbacks
|
|
try:
|
|
while running and gamepad.isConnected():
|
|
# Read the latest speed and steering
|
|
if joystickSpeedInverted:
|
|
speed = -gamepad.axis(joystickSpeed)
|
|
else:
|
|
speed = +gamepad.axis(joystickSpeed)
|
|
if joystickSteeringInverted:
|
|
steering = -gamepad.axis(joystickSteering)
|
|
else:
|
|
steering = +gamepad.axis(joystickSteering)
|
|
|
|
# Work out the adjusted speed
|
|
if gamepad.isPressed(buttonSlow):
|
|
finalSpeed = speed * slowFactor
|
|
else:
|
|
finalSpeed = speed
|
|
|
|
# Determine the drive power levels based on steering angle
|
|
servoPosition = steering
|
|
driveLeft = finalSpeed
|
|
driveRight = finalSpeed
|
|
if steering < -0.05:
|
|
# Turning left
|
|
driveLeft *= 1.0 + (0.5 * steering)
|
|
elif steering > +0.05:
|
|
# Turning right
|
|
driveRight *= 1.0 - (0.5 * steering)
|
|
|
|
# Set the motors to the new speeds and tilt the servo to steer
|
|
RB.SetMotor1(-driveLeft * maxPower)
|
|
RB.SetMotor2(driveRight * maxPower)
|
|
RB.SetServoPosition(servoPosition)
|
|
|
|
# Sleep for our motor change interval
|
|
time.sleep(interval)
|
|
finally:
|
|
# Ensure the background thread is always terminated when we are done
|
|
gamepad.disconnect()
|
|
|
|
# Turn the motors off
|
|
RB.MotorsOff()
|
|
|
|
# Turn the LED off indicate we have finished
|
|
RB.SetLed(False)
|