In [3]:
# Code adopted from http://blog.bitify.co.uk/2013/11/3d-opengl-visualisation-of-data-from.html
import pygame
import serial
import sys
import struct
import math
import threading
from OpenGL.GL import *
from OpenGL.GLU import *
from math import radians
from pygame.locals import *
SCREEN_SIZE = (800, 600)
SCALAR = .5
SCALAR2 = 0.2
class read_thread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.running = True
self.start()
self.values = 0, 0, 0, 0
self.lock = threading.Lock()
def stop(self):
self.running = False
def get_values(self):
with self.lock:
return self.values
def run(self):
while (self.running):
soc = serial.Serial("/dev/ttyUSB0", 921600)
try:
values = soc.read_until().decode('ascii')
yaw, pitch, roll, acc = map(float, values.split(";")[0:4])
with self.lock:
self.values = yaw, pitch, roll, acc
except Exception as error:
print(error, values, self.values)
def resize(width, height):
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45.0, float(width) / height, 0.001, 10.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
gluLookAt(0.0, 1.0, -5.0,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0)
def init():
pygame.font.init()
glEnable(GL_DEPTH_TEST)
glClearColor(0.0, 0.0, 0.0, 0.0)
glShadeModel(GL_SMOOTH)
glEnable(GL_BLEND)
glEnable(GL_POLYGON_SMOOTH)
glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
glEnable(GL_COLOR_MATERIAL)
glEnable(GL_LIGHTING)
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));
def drawText(position, textString):
font = pygame.font.Font (None, 64)
textSurface = font.render(textString, True, (255,255,255,255), (0,0,0,255))
textData = pygame.image.tostring(textSurface, "RGBA", True)
glRasterPos3d(*position)
glDrawPixels(textSurface.get_width(), textSurface.get_height(), GL_RGBA, GL_UNSIGNED_BYTE, textData)
def run():
pygame.init()
ser = read_thread()
screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
resize(*SCREEN_SIZE)
init()
clock = pygame.time.Clock()
cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7))
angle = 0
x_angle = 0
y_angle = 0
z_angle = 0
while True:
then = pygame.time.get_ticks()
for event in pygame.event.get():
if event.type == QUIT:
ser.stop()
pygame.quit()
return
if event.type == KEYUP and event.key == K_ESCAPE:
ser.stop()
pygame.quit()
return
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
yaw, pitch, roll, acc = ser.get_values()
drawText((2.5, 1, 1), "Y%+6.1f" % (yaw));
drawText((0.5, 1, 1), "P%+6.1f" % (pitch));
drawText((-1.5, 1, 1), "R%+6.1f" % (roll));
drawText((0.5, 1.5 + acc/10.0, 1), "A%+6.3f" % (acc));
drawText((-1.5, 1.5, 1), "C%+6.3f" % (roll - yaw));
# yaw, pitch, roll = map(math.radians, [yaw, pitch, roll])
# x_angle = math.degrees(math.cos( pitch ) * math.cos( yaw ))
# y_angle = math.degrees(math.sin( yaw ))
# z_angle = math.degrees(math.sin( pitch ) * math.cos( yaw ))
x_angle = roll
y_angle = yaw + 90
z_angle = pitch
glColor((1.,1.,1.))
glLineWidth(1)
glBegin(GL_LINES)
for x in range(-20, 22, 2):
glVertex3f(x/10.,-1,-1)
glVertex3f(x/10.,-1,1)
for x in range(-20, 22, 2):
glVertex3f(x/10.,-1, 1)
glVertex3f(x/10., 1, 1)
for z in range(-10, 12, 2):
glVertex3f(-2, -1, z/10.)
glVertex3f( 2, -1, z/10.)
for z in range(-10, 12, 2):
glVertex3f(-2, -1, z/10.)
glVertex3f(-2, 1, z/10.)
for z in range(-10, 12, 2):
glVertex3f( 2, -1, z/10.)
glVertex3f( 2, 1, z/10.)
for y in range(-10, 12, 2):
glVertex3f(-2, y/10., 1)
glVertex3f( 2, y/10., 1)
for y in range(-10, 12, 2):
glVertex3f(-2, y/10., 1)
glVertex3f(-2, y/10., -1)
for y in range(-10, 12, 2):
glVertex3f(2, y/10., 1)
glVertex3f(2, y/10., -1)
glEnd()
glPushMatrix()
glRotate(float(y_angle), 0, -1, 0)
glRotate(float(z_angle), 0, 0, 1)
glRotate(float(x_angle), 1, 0, 0)
cube.render()
glPopMatrix()
pygame.display.flip()
class Cube(object):
def __init__(self, position, color):
self.position = position
self.color = color
# Cube information
num_faces = 6
vertices = [ (-1.0, -0.05, 0.5),
(1.0, -0.05, 0.1),
(1.0, 0.05, 0.1),
(-1.0, 0.05, 0.5),
(-1.0, -0.05, -0.5),
(1.0, -0.05, -0.1),
(1.0, 0.05, -0.1),
(-1.0, 0.05, -0.5) ]
normals = [ (0.0, 0.0, +1.0), # front
(0.0, 0.0, -1.0), # back
(+1.0, 0.0, 0.0), # right
(-1.0, 0.0, 0.0), # left
(0.0, +1.0, 0.0), # top
(0.0, -1.0, 0.0) ] # bottom
vertex_indices = [ (0, 1, 2, 3), # front
(4, 5, 6, 7), # back
(1, 5, 6, 2), # right
(0, 4, 7, 3), # left
(3, 2, 6, 7), # top
(0, 1, 5, 4) ] # bottom
def render(self):
then = pygame.time.get_ticks()
vertices = self.vertices
# Draw all 6 faces of the cube
glBegin(GL_QUADS)
for face_no in range(self.num_faces):
if face_no == 4:
glColor(1, .5, .7)
else:
glColor(self.color)
glNormal3dv(self.normals[face_no])
v1, v2, v3, v4 = self.vertex_indices[face_no]
glVertex(vertices[v1])
glVertex(vertices[v2])
glVertex(vertices[v3])
glVertex(vertices[v4])
glEnd()
run()
In [ ]:
In [ ]: