Python Joystick Controller Tutorial
Python is very accessible programming language. It is an interactive language which makes it perfect for data analysis and prototyping software. There are many terms to learn such as objects, classes, functions, data types, methods, libraries, and the list goes on. An explanation of all of the terms would make for very dry reading! Like any language, it is easiest to learn when you have need to use it.
We are going to begin with a simple joystick controller for the T-Bot. You will need a generic usb joystick. A PS3 or PS4 controller will do but you will have to make some small changes to the button configuration. You con use JoystickButtonFinder.py for this.
This example is quite visually gratifying and it is the best way to control the T-Bot. Many of the terms above will be covered but only as we need them.
First we begin with the libraries. A very comprehensive introduction to python libraries can be found here but essentially, libraries are collections of modules written in C or Python.
We want to create a nice graphical user interface for our controller. The are many libraries available for this but we are going to use PyGame. More will be explained when we start to create some functionality but for now we will just import it using the following command:
import pygame
we will also import pygame.locals using:
import pygame.locals as pgl
We will also need to send and receive data from the T-Bot. We can use the T-Bot Bluetooth module from TbotTools. Before doing this we will have to let Python know where to look for the module. We need to add the directory to the Python path. We can use a library called sys for this. It can be imported as follows:
import sys
The path to the directory containg TbotTools can be added like this:
sys.path.append('/home/pi/GitHub/T-BOTS/Python')
Classes, methods and functions.
A class is a template for creating an object. Objects may include data and methods. A method is similar to a function but is associated with the object or class. We have imported the tbt class but an object hasn’t been created yet. We will create the object by creating an instance of the class. tbt requires three input values.
- The Bluetooth MAC address - use: ‘hcitool scan’ in the Raspberry Pi or Linux to find it. You wiil need to pair T-Bot with your computer for this to show up.
- The port number or name
- The name of the Bluetooth library installed on your system.
bd_addr = '98:D3:51:FD:81:AC' # use: 'hcitool scan' to scan for your T-Bot address port = 1 #------------------------------------------------------------------ # For Linux #------------------------------------------------------------------ #btcom = tbt.bt_connect(bd_addr,port,'PyBluez') btcom = tbt.bt_connect(bd_addr,port,'Socket') #------------------------------------------------------------------ # For Windows and Mac #------------------------------------------------------------------ #port = 'COM5' #port = '/dev/tty.George-DevB' #baudrate = 38400 #bd_addr = 'Empty' #btcom = tbt.bt_connect(bd_addr,port,'PySerial',baudrate)
Initialize all imported pygame modules.
pygame.init()
No set the display window and load the imagery:
# Set the width and height of the screen (width, height).
screen = pygame.display.set_mode((512, 294))
pygame.display.set_caption("Player 1")
# Used to manage how fast the screen updates.
clock = pygame.time.Clock()
dirpath = os.path.dirname(os.path.realpath(__file__))+'/Images'
# Use convert for the large images. This is the fastest format for blitting
# Background images
bg = pygame.image.load(dirpath+'/Simple/Controller.png').convert()
bgG = pygame.image.load(dirpath+'/Simple/Offline.png').convert()
# Do not use convert for the following images
# Button images
dpad = pygame.image.load(dirpath+'/Simple/dpad.png')
dpadU = pygame.image.load(dirpath+'/Simple/dpadU.png')
dpadD = pygame.image.load(dirpath+'/Simple/dpadD.png')
dpadL = pygame.image.load(dirpath+'/Simple/dpadL.png')
Set the coordinates for the button positions:
posdpad = (102, 75) posbpad = (327, 75) posstickL = (165, 130) posstickR = (287, 130) posL = (108,15) posR = (337,15)
See the code for the full list of images.
Another simple class in the TBotTools library is the text positioning class. You can instantiate it like this:
textPrint = pgt.TextPrint(pygame.Color('white'))
You can see the code here.
Now we need to instantiate the PyGame joystick class and get some information about the connected joystick.
# Initialize the joystick. pygame.joystick.init() joystick = pygame.joystick.Joystick(0)# 0 for first joystick, 1 for the next etc. joystick.init() name = joystick.get_name() axes = joystick.get_numaxes() hats = joystick.get_numhats()
Reading data back from the T-Bot can suffer delays. It is useful to create a special user event so that reading the data does not slow the frame rate. It's not so important for this controller, but it will become more relevant later when you want to add a video stream to the controller.
readdataevent = pygame.USEREVENT+1 pygame.time.set_timer(readdataevent, 60)
Finally we get to the main program loop.
# Loop until the user clicks the close button or q is pressed
done = False
# -------- Main Program Loop -----------
while not done:
if pygame.event.get(readdataevent):
oldvals = btcom.get_data(oldvals)
for event in pygame.event.get():
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
if event.type == pgl.KEYDOWN and event.key == pgl.K_q:
done = True
The first event is the timer event we setup for reading the data from the T-Bot over Bluetooth. The second type to detect if the user close the window and the third is detecting key presses on the keyboard.
We want to monitor the BT connection and restore it automatically if it is lost.
if btcom.connected():
screen.blit(bg, [0, 0])
else:
tries = 0
while btcom.connected() < 1 and tries < 10:
print('Connecting ...')
screen.blit(bgG, [0, 0])
pygame.display.flip()
try:
print('Try '+str(tries+1)+' of 10')
btcom.connect(0)
btcom.connect(1)
tries+=1
except:
print('Something went wrong')
if btcom.connected() < 1:
print('Exiting Program')
done = True
else:
tries = 0
The joystic is composed of three parts:
- Hats - The four arrow buttons on the left lide of the controller, also known as the D-Pad.
- Axes - The analogue sticks.
- Buttons - The rest of the buttons.
Starting with the hat; we will check for button presses and adjust some variables in response:
for i in range(hats):
hat = joystick.get_hat(i)
if hat[1] == 1:
speedfactor += 0.05
elif hat[1] == -1:
speedfactor -= 0.05
elif hat[0] == -1:
speedlimit -= 5
elif hat[0] == 1:
speedlimit += 5
if speedlimit >= 100:
speedlimit = 100
if speedlimit <= 0:
speedlimit = 0
if speedfactor >= 5:
speedfactor = 5
if speedfactor <= 0:
speedfactor = 0
Now we will do the same for the axes:
axis0 = joystick.get_axis(0)
axis1 = joystick.get_axis(1)
axis2 = joystick.get_axis(2)
axis3 = joystick.get_axis(3)
#
if abs(axis0)+abs(axis1)+abs(axis2)+abs(axis3) != 0:
slowfactor = 1+joystick.get_button(7)
turn = 200+int(((axis0+(axis2*0.5))*speedfactor*100/slowfactor))
speed = 200-int(((axis1+(axis3*0.5))*speedfactor*100/slowfactor))
if speed > 200+speedlimit:
speed = 200+speedlimit
if speed < 200-speedlimit:
speed = 200-speedlimit
if turn > 200+turnspeedlimit:
turn = 200+turnspeedlimit
if turn < 200-turnspeedlimit:
turn = 200-turnspeedlimit
sendstring = str(turn)+str(speed)+'Z'
sendcount = btcom.send_data(sendstring,sendcount)
else:
sendstring = '200200Z'
sendcount = btcom.send_data(sendstring,sendcount)
and finally, for the buttons:
if joystick.get_button(0):
buttonstring = '200200F' # trim +ve
sendcount = btcom.send_data(buttonstring,sendcount)
elif joystick.get_button(2):
buttonstring = '200200E' # trim -ve
sendcount = btcom.send_data(buttonstring,sendcount)
elif joystick.get_button(1):
buttonstring = '200200B' # kps +ve
sendcount = btcom.send_data(buttonstring,sendcount)
elif joystick.get_button(3):
buttonstring = '200200A' # kps -ve
sendcount = btcom.send_data(buttonstring,sendcount)
elif joystick.get_button(9):
buttonstring = '200200T' # kps -ve
sendcount = btcom.send_data(buttonstring,sendcount)
Note, for the axes and button presses, we are sending data to the T-Bot.
The remaining code inside the loop is for highlighting the buttons and displaying data on the graphical representation. The blit screen.blit() is the method that is drawing the buttons.
# ------------------ Highlight buttons ----------------#
screen.blit(dpad,posdpad)
screen.blit(bpad,posbpad)
screen.blit(stick,(posstickL[0]+axis0*5,posstickL[1]+axis1*5))
screen.blit(stick,(posstickR[0]+axis2*5,posstickR[1]+axis3*5))
if hat[0] == 1:
screen.blit(dpadR,posdpad)
elif hat[0] == -1:
screen.blit(dpadL,posdpad)
elif hat[1] == 1:
screen.blit(dpadU,posdpad)
elif hat[1] == -1:
screen.blit(dpadD,posdpad)
else:
screen.blit(dpad,posdpad)
if (hat[0] == -1) & (hat[1] == 1):
screen.blit(dpadUL,posdpad)
elif (hat[0] == 1) & (hat[1] == -1):
screen.blit(dpadDR,posdpad)
elif (hat[0] == 1 & hat[1] == 1):
screen.blit(dpadUR,posdpad)
elif hat[0] == -1 & hat[1] == -1:
screen.blit(dpadDL,posdpad)
if joystick.get_button(0):
screen.blit(bpadU,posbpad)
elif joystick.get_button(1):
screen.blit(bpadR,posbpad)
elif joystick.get_button(2):
screen.blit(bpadD,posbpad)
elif joystick.get_button(3):
screen.blit(bpadL,posbpad)
else:
screen.blit(bpad,posbpad)
if joystick.get_button(0) & joystick.get_button(1):
screen.blit(bpadUR,posbpad)
elif joystick.get_button(1) & joystick.get_button(2):
screen.blit(bpadDR,posbpad)
elif joystick.get_button(2) & joystick.get_button(3):
screen.blit(bpadDL,posbpad)
elif joystick.get_button(0) & joystick.get_button(3):
screen.blit(bpadUL,posbpad)
if joystick.get_button(4):
screen.blit(L1,posL)
elif joystick.get_button(6):
screen.blit(L2,posL)
elif joystick.get_button(5):
screen.blit(R1,posR)
elif joystick.get_button(7):
screen.blit(R2,posR)
else:
screen.blit(bpad,posbpad)
if joystick.get_button(4) & joystick.get_button(6):
screen.blit(L1L2,posL)
elif joystick.get_button(5) & joystick.get_button(7):
screen.blit(R1R2,posR)
elif joystick.get_button(4) & joystick.get_button(5):
screen.blit(L1,posL)
screen.blit(R1,posR)
elif joystick.get_button(4) & joystick.get_button(7):
screen.blit(L1,posL)
screen.blit(R2,posR)
elif joystick.get_button(6) & joystick.get_button(5):
screen.blit(L2,posL)
screen.blit(R1,posR)
elif joystick.get_button(6) & joystick.get_button(7):
screen.blit(L2,posL)
screen.blit(R2,posR)
if joystick.get_button(4) & joystick.get_button(6) & joystick.get_button(5):
screen.blit(L1L2,posL)
screen.blit(R1,posR)
elif joystick.get_button(4) & joystick.get_button(6) & joystick.get_button(7):
screen.blit(L1L2,posL)
screen.blit(R2,posR)
elif joystick.get_button(4) & joystick.get_button(5) & joystick.get_button(7):
screen.blit(L1,posL)
screen.blit(R1R2,posR)
elif joystick.get_button(5) & joystick.get_button(6) & joystick.get_button(7):
screen.blit(L2,posL)
screen.blit(R1R2,posR)
if joystick.get_button(4) & joystick.get_button(5) & joystick.get_button(6) & joystick.get_button(7):
screen.blit(L1L2,posL)
screen.blit(R1R2,posR)
textPrint.abspos(screen, "Gyro Data: {}".format(str(oldvals[3])),(10,10))
textPrint.tprint(screen, "KPS: {}".format(str(oldvals[0])))
textPrint.tprint(screen, "KP: {}".format(str(oldvals[1])))
textPrint.tprint(screen, "Trim: {}".format(str(oldvals[2])))
textPrint.abspos(screen, "Speed Factor: {}".format(str(speedfactor)),(415,10))
textPrint.tprint(screen, "Speed Limit: {}%".format(str(speedlimit)))
textPrint.tprint(screen, "{} FPS".format(str(int(clock.get_fps()))))
The pygame.display.flip() method clears the frame. If this part is forgotten, the drawn buttons will acumulate into an ugly mess. The clock.tick() method sets the maximum frame rate.
pygame.display.flip()
# Limit to 30 frames per second.
clock.tick(30)
Finally, this last bit of code is for a graceful exit:
pygame.display.quit()
pygame.quit()
btcom.connect(0)
print('Connection Closed')
The full code can be found here. Don't forget to check the blog for the latest information or the KliK Robotics Facebook Group. OpenCV, machine learning, and advanced control theory examples are coming soon. If you can't wait, have a look in the Developement folder.