I am trying to create a program which reads the orientation of an Arduino using PySerial and then represents this 3D orientation in PyGame on a computer.
I have everything working however the data stream coming from the Arduino via PySerial seems to be delayed by about 5 seconds and the delay gradually increases.
I believe that this is something to do with the Pygame loop conflicting with the Pyserial readline command. I have tried to use threading to separate the two loops however when I tried this the pygame window is left white but the data stream does not have a delay anymore and is a lot more responsive.
I then read that Pygame loop is not threading safe. How would I get round this problem?
Main.py
#---------------------------------------------
# Drone Configurator/Tester, for configuring the PIDS and viewing the angles of the quadcopter
#---------------------------------------------
# Matthew Haywood
# Used to configure the drones PIDS and to view the angles of the drone etc..
from projectionViewer import ProjectionViewer
import wireframe
import numpy as np
from obj_loader import OBJ_loader
import random
import os
from Classes.Drone import Drone
from Classes.Light import Light
from Classes.SerialConnection import SerialConnection
from GridGenerator import GridGenerator
import threading
import math
import pygame
connection = SerialConnection('/dev/cu.usbmodem5103EA572', 115200, timeout=0.05)
#Get Values from arduino for Roll, Pitch and Yaw and update the 3D model
origin = wireframe.Wireframe()
origin.addNodes([[0,0,0], [1000, 0, 0], [0, 1000, 0], [0, 0, 1000]])
origin.addEdges([(0,3), (0,2), (0,1)])
pv = ProjectionViewer(1200, 1000, origin, connection)
pv.addWireframe('origin', origin)
serialThread = threading.Thread(target=pv.serialConnection.receiveData(), args=())
light1 = Light((500, 1000, 0), 1)
pv.addLight('Light1', light1)
wf = wireframe.Wireframe()
translate_drone_1 = wf.translationMatrix(-600,0,0)
pv.wireframes['drone1'].transform(translate_drone_1)
wf = wireframe.Wireframe()
translate_drone_2 = wf.rotateYMatrix(-math.pi*1/4.0)
pv.wireframes['drone1'].transform(translate_drone_2)
wf = wireframe.Wireframe()
translate_drone_1 = wf.translationMatrix(600,0,0)
pv.wireframes['drone1'].transform(translate_drone_1)
pvThread = threading.Thread(target=pv.run(), args=())
pvThread.setDaemon(True)
serialThread.setDaemon(True)
serialThread.start()
pvThread.start()
# pv.run()
Serial Connection Class
import serial
import io
import sys
import time
class SerialConnection:
def __init__(self, port, baud, timeout):
self.port = port
self.baud = baud
self.timeout = timeout
self.connect()
def connect(self):
try:
self.ser = serial.Serial(self.port, self.baud, timeout=self.timeout)
self.sio = io.TextIOWrapper(io.BufferedRWPair(self.ser,self.ser))
sys.stdout.flush()
except:
print("Could not connect")
def receiveData(self):
msg= self.sio.readline()
msg = msg.split()
return msg
Projection viewer.py
from wireframe import *
import pygame
from obj_loader import OBJ_loader
import numpy as np
from Classes.camera import *
from Classes.Drone import Drone
import time
import math
import random
class ProjectionViewer:
''' Displays 3D Objects on a Pygame Screen '''
def __init__(self, width, height, center_point, serialConnection):
self.width = width
self.height = height
self.screen = pygame.display.set_mode((width, height))
pygame.display.set_caption('3D Renderer')
self.background = (10,10,50)
self.drone = Drone([0,0,0], 0, 0)
self.drone1Wireframe = self.drone.getWireframe()
grid_loader = OBJ_loader('./Assets/grid.obj', 100)
self.grid = grid_loader.create_wireframe()
self.grid.showFaces = False
self.flat_terrain = True
#Setup camera
self.camera = Camera([0,0,0],0,0)
self.center_point = center_point
self.wireframes = {'grid':self.grid, 'drone1':self.drone1Wireframe}
self.lights = {}
if self.flat_terrain == False:
self.add_terrain_height()
self.displayNodes = False
self.displayEdges = True
self.displayFaces = True
self.nodeColour = (255,255,255)
self.edgeColour = (200,200,200)
self.nodeRadius = 2
self.serialConnection = serialConnection
pygame.init()
def run(self):
key_to_function = {
#Camera Controls
pygame.K_LEFT: (lambda x: x.rotate_about_camera('Y', 0.05)),
pygame.K_RIGHT:(lambda x: x.rotate_about_camera('Y', -0.05)),
pygame.K_DOWN: (lambda x: x.translateAll([0, 10, 0])),
pygame.K_UP: (lambda x: x.translateAll([0, -10, 0])),
pygame.K_w: (lambda x: x.move_cam_forward(20)),
pygame.K_s: (lambda x: x.move_cam_backward(20)),
pygame.K_a: (lambda x: x.move_cam_left(20)),
pygame.K_d: (lambda x: x.move_cam_right(20)),
#Flying Controls
pygame.K_u: (lambda x: x.throttle_up()),
pygame.K_j: (lambda x: x.throttle_down()),
pygame.K_h: (lambda x: x.tilt_right()),
pygame.K_k: (lambda x: x.tilt_left()),
pygame.K_o: (lambda x: x.drone_yaw('r')),
pygame.K_p: (lambda x: x.drone_yaw('l')),
pygame.K_i: (lambda x: x.pitch_forward()),
pygame.K_y: (lambda x: x.pitch_backward())
}
running = True
flag = False
while running:
keys = pygame.key.get_pressed()
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if keys[pygame.K_LEFT]:
key_to_function[pygame.K_LEFT](self)
if keys[pygame.K_RIGHT]:
key_to_function[pygame.K_RIGHT](self)
if keys[pygame.K_DOWN]:
key_to_function[pygame.K_DOWN](self)
if keys[pygame.K_UP]:
key_to_function[pygame.K_UP](self)
if keys[pygame.K_u]:
key_to_function[pygame.K_u](self)
if keys[pygame.K_h]:
key_to_function[pygame.K_h](self)
if keys[pygame.K_k]:
key_to_function[pygame.K_k](self)
if keys[pygame.K_o]:
key_to_function[pygame.K_o](self)
if keys[pygame.K_p]:
key_to_function[pygame.K_p](self)
if keys[pygame.K_i]:
key_to_function[pygame.K_i](self)
if keys[pygame.K_y]:
key_to_function[pygame.K_y](self)
if keys[pygame.K_w]:
key_to_function[pygame.K_w](self)
if keys[pygame.K_a]:
key_to_function[pygame.K_a](self)
if keys[pygame.K_s]:
key_to_function[pygame.K_s](self)
if keys[pygame.K_d]:
key_to_function[pygame.K_d](self)
roll = self.serialConnection.receiveData()[0]
pitch = self.serialConnection.receiveData()[1]
self.positionDronePitch(float(pitch))
self.positionDroneRoll(float(roll))
self.display()
self.drone_display()
pygame.display.flip()
def addWireframe(self, name, wireframe):
self.wireframes[name] = wireframe
#translate to center
wf = Wireframe()
matrix = wf.translationMatrix(-self.width/2,-self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
wf = Wireframe()
matrix = wf.translationMatrix(self.width,self.height,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def addLight(self, name, light):
self.lights[name] = light
lightWireframe = Wireframe()
lightPosition = np.array([[light.position[0], light.position[1], light.position[2]]])
lightWireframe.addNodes(lightPosition)
self.wireframes[name] = lightWireframe
def display(self):
self.screen.fill(self.background)
for wireframe in self.wireframes.values():
wireframe.transform_for_perspective((self.width/2, self.height/2), self.camera.fov, self.camera.zoom)
if self.displayNodes:
for node in wireframe.perspective_nodes:
if node[2] > 0 and node[2] < 10000 and node[0] > 0 and node[0] < 1199:
pygame.draw.circle(self.screen, self.nodeColour, (int(node[0]), int(node[1])), self.nodeRadius, 0)
else:
pass
if self.displayEdges and wireframe.showEdges:
for n1, n2 in wireframe.edges:
clipN1 = self.clipNode(wireframe.perspective_nodes[n1])
clipN2 = self.clipNode(wireframe.perspective_nodes[n2])
if type(clipN1) == int or type(clipN2) == int:
pass
else:
pygame.draw.aaline(self.screen, self.edgeColour, clipN1[:2], clipN2[:2], 1)
else:
pass
if self.displayFaces and wireframe.showFaces:
for face in wireframe.faces:
n1, n2, n3 = face.vertices
clipN1 = self.clipNode(wireframe.perspective_nodes[n1])
clipN2 = self.clipNode(wireframe.perspective_nodes[n2])
clipN3 = self.clipNode(wireframe.perspective_nodes[n3])
if type(clipN1) == int or type(clipN2) == int or type(clipN3) == int:
pass
else:
cull = self.backFaceCull(clipN1, clipN2, clipN3)
if cull:
pass
else:
pygame.draw.polygon(self.screen, [self.processLighting(face) * x for x in face.material], [clipN1[:2], clipN2[:2], clipN3[:2]], 0)
else:
pass
def processLighting(self, face):
directionVector = [None, None, None]
for light in self.lights.values():
directionVector[0] = ( light.position[0] - face.fNormal[0])
directionVector[1] = ( light.position[1] - face.fNormal[1])
directionVector[2] = ( light.position[2] - face.fNormal[2])
olddirectionVectorX = directionVector[0]
olddirectionVectorY = directionVector[1]
olddirectionVectorZ = directionVector[2]
directionVector[0] = directionVector[0] / math.sqrt((olddirectionVectorX**2) + (olddirectionVectorY**2) + (olddirectionVectorZ**2))
directionVector[1] = directionVector[1] / math.sqrt((olddirectionVectorX**2) + (olddirectionVectorY**2) + (olddirectionVectorZ**2))
directionVector[2] = directionVector[2] / math.sqrt((olddirectionVectorX**2) + (olddirectionVectorY**2) + (olddirectionVectorZ**2))
cosTheta = self.clamp((directionVector[0]*face.fNormal[0]) + (directionVector[1]*face.fNormal[1]) + (directionVector[2]*face.fNormal[2]), 0, 1)
return cosTheta
def clamp(self, num, min_value, max_value):
return max(min(num, max_value), min_value)
def add_terrain_height(self):
prev_height = random.randint(1,300)
for node in self.wireframes['grid'].nodes:
node[1] = prev_height + random.randint(-50,50)
prev_height = node[1]
def backFaceCull(self, n1, n2, n3):
answer = ((n1[0] * n2[1]) + (n2[0]* n3[1]) + (n3[0] * n1[1])) - ((n3[0] * n2[1]) + (n2[0] * n1[1]) + (n1[0] * n3[1]))
if answer > 0:
return True
else:
return False
def clipNode(self, node):
x = self.width
y = self.height
z = 50000
clippedNode = 0
if node[0] > x or node[0] < 0:
return 0
if node[1] > y or node[0] < 0:
return 0
if node[2] < z and node[2] > 0:
clippedNode = node
else:
return 0
return clippedNode
def translateAll(self, vector):
''' Translate all wireframes along a given axis by d units '''
wf = Wireframe()
matrix = wf.translationMatrix(*vector)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def scaleAll(self, vector):
wf = Wireframe()
matrix = wf.scaleMatrix(*vector)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def rotateAll(self, axis, theta):
wf = Wireframe()
if axis == 'X':
matrix = wf.rotateXMatrix(theta)
elif axis == 'Y':
matrix = wf.rotateYMatrix(theta)
elif axis == 'Z':
matrix = wf.rotateZMatrix(theta)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def rotate_about_Center(self, Axis, theta):
#First translate Centre of screen to 0,0
wf = Wireframe()
matrix = wf.translationMatrix(-self.width/2,-self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
#Do Rotation
wf = Wireframe()
if Axis == 'X':
matrix = wf.rotateXMatrix(theta)
elif Axis == 'Y':
matrix = wf.rotateYMatrix(theta)
elif Axis == 'Z':
matrix = wf.rotateZMatrix(theta)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
#Translate back to centre of screen
wf = Wireframe()
matrix = wf.translationMatrix(self.width/2,self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def rotate_about_camera(self, Axis, theta):
wf = Wireframe()
matrix = wf.translationMatrix(-self.width/2, -self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
#Do Rotation
wf = Wireframe()
if Axis == 'X':
matrix = wf.rotateXMatrix(theta)
elif Axis == 'Y':
matrix = wf.rotateYMatrix(theta)
elif Axis == 'Z':
matrix = wf.rotateZMatrix(theta)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
#Translate back to original position
wf = Wireframe()
matrix = wf.translationMatrix(self.width/2,self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
self.camera.set_position(self.center_point)
self.camera.hor_angle += theta
if self.camera.hor_angle >= 2*math.pi:
self.camera.hor_angle -= 2*math.pi
elif self.camera.hor_angle < -2*math.pi:
self.camera.hor_angle += 2*math.pi
def scale_centre(self, vector):
wf = Wireframe()
matrix = wf.translationMatrix(-self.width/2,-self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
wf = Wireframe()
matrix = wf.scaleMatrix(*vector)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
wf = Wireframe()
matrix = wf.translationMatrix(self.width/2,self.height/2,0)
for wireframe in self.wireframes.values():
wireframe.transform(matrix)
def move_cam_forward(self, amount):
#Moving the camera forward will be a positive translation in the z axis for every other object.
self.translateAll([0,0,-amount])
self.camera.set_position(self.center_point)
def move_cam_backward(self, amount):
self.translateAll([0,0,amount])
self.camera.set_position(self.center_point)
def move_cam_left(self, amount):
self.translateAll([-amount,0,0])
self.camera.set_position(self.center_point)
def move_cam_right(self, amount):
self.translateAll([amount,0,0])
self.camera.set_position(self.center_point)
def Toggle_Nodes(self):
if self.displayNodes == True:
self.displayNodes = False
else:
self.displayNodes = True
def throttle_up(self):
self.drone.increase_altitude(20, self.camera)
def drone_yaw(self, direction):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.yaw_(direction, (1/30)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def tilt_left(self):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.roll_drone(-(1/15)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def tilt_right(self):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.roll_drone((1/15)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def pitch_forward(self):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.pitch_drone(-(1/30)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def pitch_backward(self):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.pitch_drone((1/30)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def positionDroneRoll(self, amount):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.roll_drone_absolute_value((amount/360)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def positionDronePitch(self, amount):
prev = self.camera.hor_angle
self.rotate_about_camera('Y', -self.camera.hor_angle)
self.drone.pitch_drone_absolute_value((amount/360)*math.pi, self.camera)
self.rotate_about_camera('Y', prev)
def drone_display(self):
font = pygame.font.Font('freesansbold.ttf', 20)
text = font.render(f'POS(X,Y,Z): {self.drone.position[0]:.2f}, {self.drone.position[1]:.2f}, {self.drone.position[2]:.2f}', True, (255,255,255),(self.background))
textRect = text.get_rect()
textRect.center = (1000, 175)
self.screen.blit(text, textRect)