# * @file talker.py # * @author Isaac Jesus da Silva - ROBOFEI-HT - FEI 😛 # * @version V0.0.1 # * @created 08/09/2015 # * @Modified 08/09/2015 # * @e-mail [email protected] # * @brief Talker 😛 # **************************************************************************** # Program to testing write in the BlackBoard # **************************************************************************** import time import os import random from SharedMemory import SharedMemory as blackboard bkb = blackboard() #Iniciando a classe do BlackBoard bkb.write_int("PLANNING_COMMAND", 12) # Escrevendo no Blackboard print bkb.read_int("PLANNING_COMMAND") #Lendo do BlackBoard # Realizando escrita na IMU---------------------------------------------- while True: os.system('clear') num1 = random.uniform(0, 1.00) # Gerando um número randomico num2 = random.uniform(0, 1.00) # Gerando um número randomico num3 = random.uniform(0, 1.00) # Gerando um número randomico bkb.write_float("IMU_ACCEL_X", num1) #Escrevendo no BlackBoard bkb.write_float("IMU_ACCEL_Y", num2) #Escrevendo no BlackBoard bkb.write_float("IMU_ACCEL_Z", num3) #Escrevendo no BlackBoard print "Escrevendo no IMU_ACCEL_X = %1.4f" % num1 print "Escrevendo no IMU_ACCEL_Y = %1.4f" % num2
# * @file talker.py # * @author Isaac Jesus da Silva - ROBOFEI-HT - FEI 😛 # * @version V0.0.1 # * @created 08/09/2015 # * @Modified 08/09/2015 # * @e-mail [email protected] # * @brief Talker 😛 # **************************************************************************** # Program to testing write in the BlackBoard # **************************************************************************** import time import os import random from SharedMemory import SharedMemory as blackboard bkb = blackboard() #Iniciando a classe do BlackBoard # Realizando escrita na VISION---------------------------------------------- while True: #os.system('clear') num1 = 600 # Gerando um número randomico num2 = 502 # Gerando um número randomico num3 = 2 num4 = 1 bkb.write_int("VISION_MOTOR1_ANGLE", num1) #Escrevendo no BlackBoard bkb.write_int("VISION_MOTOR2_ANGLE", num2) #Escrevendo no BlackBoard bkb.write_int("COM_REFEREE", num3) #Escrevendo no BlackBoard bkb.write_int("LOCALIZATION_THETA", num4) #Escrevendo no BlackBoard print 'VISION_MOTOR1_ANGLE = %1.4f' % num1 print 'VISION_MOTOR1_ANGLE = %1.4f' % num2 print 'com_referee = %1.4f' % num3
#!/usr/bin/env python
from OpenGL.GL import *
from OpenGL.GLU import *
import pygame
from pygame.locals import *
from SharedMemory import SharedMemory as blackboard
#import serial
#ser = serial.Serial('/dev/tty.usbserial', 38400, timeout=1)
ax = ay = az = 0.0
yaw_mode = True
bkb = blackboard() #Init class BlackBoard
def resize((width, height)):
if height==0:
height=1
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45, 1.0*width/height, 0.1, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
def init():
glShadeModel(GL_SMOOTH)
glClearColor(0.0, 0.0, 0.0, 0.0)
glClearDepth(1.0)
glEnable(GL_DEPTH_TEST)
#!/usr/bin/env python
from OpenGL.GL import *
from OpenGL.GLU import *
import pygame
from pygame.locals import *
from SharedMemory import SharedMemory as blackboard
#import serial
#ser = serial.Serial('/dev/tty.usbserial', 38400, timeout=1)
ax = ay = az = 0.0
yaw_mode = True
bkb = blackboard() #Init class BlackBoard
def resize((width, height)):
if height == 0:
height = 1
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45, 1.0 * width / height, 0.1, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
def init():
glShadeModel(GL_SMOOTH)
glClearColor(0.0, 0.0, 0.0, 0.0)
class SpiralWidget(QGLWidget):
'''
Widget for drawing two spirals.
'''
bkb = blackboard() #Init class BlackBoard
ax = bkb.read_float("IMU_EULER_X")
ay = -bkb.read_float("IMU_EULER_Y")
az = bkb.read_float("IMU_EULER_Z")
def __init__(self, parent):
QGLWidget.__init__(self, parent)
# self.initializeGL()
def resizeGL(self, width=0, height=0):
'''
Resize the GL window
'''
if height == 0:
height = 1
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45, 1.0 * width / height, 0.1, 100.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# print "resize"
def initializeGL(self):
'''
Initialize GL
'''
glShadeModel(GL_SMOOTH)
glClearColor(0.0, 0.0, 0.0, 0.0)
glClearDepth(1.0)
glEnable(GL_DEPTH_TEST)
glDepthFunc(GL_LEQUAL)
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)
# print "init"
# def drawText(self, position, textString):
# font = pygame.font.SysFont ("Courier", 18, True)
# 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 paintGL(self):
'''
Drawing routine
'''
global rquad
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glLoadIdentity()
glTranslatef(0, 0.0, -7.0)
# osd_text = "pitch: " + str("{0:.2f}".format(ay)) + ", roll: " + str("{0:.2f}".format(ax))
# if yaw_mode:
# osd_line = osd_text + ", yaw: " + str("{0:.2f}".format(az))
# else:
# osd_line = osd_text
# self.drawText((-2,-2, 2), osd_line)
# the way I'm holding the IMU board, X and Y axis are switched
# with respect to the OpenGL coordinate system
if yaw_mode: # experimental
glRotatef(self.az, 0.0, 1.0, 0.0) # Yaw, rotate around y-axis
else:
glRotatef(0.0, 0.0, 1.0, 0.0)
glRotatef(self.ay, 1.0, 0.0, 0.0) # Pitch, rotate around x-axis
glRotatef(-1 * self.ax, 0.0, 0.0, 1.0) # Roll, rotate around z-axis
glBegin(GL_QUADS)
glColor3f(0.0, 1.0, 0.0)
glVertex3f(1.0, 0.2, -1.0)
glVertex3f(-1.0, 0.2, -1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f(1.0, 0.2, 1.0)
glColor3f(1.0, 0.5, 0.0)
glVertex3f(1.0, -0.2, 1.0)
glVertex3f(-1.0, -0.2, 1.0)
glVertex3f(-1.0, -0.2, -1.0)
glVertex3f(1.0, -0.2, -1.0)
glColor3f(1.0, 0.0, 0.0)
glVertex3f(1.0, 0.2, 1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f(-1.0, -0.2, 1.0)
glVertex3f(1.0, -0.2, 1.0)
glColor3f(1.0, 1.0, 0.0)
glVertex3f(1.0, -0.2, -1.0)
glVertex3f(-1.0, -0.2, -1.0)
glVertex3f(-1.0, 0.2, -1.0)
glVertex3f(1.0, 0.2, -1.0)
glColor3f(0.0, 0.0, 1.0)
glVertex3f(-1.0, 0.2, 1.0)
glVertex3f(-1.0, 0.2, -1.0)
glVertex3f(-1.0, -0.2, -1.0)
glVertex3f(-1.0, -0.2, 1.0)
glColor3f(1.0, 0.0, 1.0)
glVertex3f(1.0, 0.2, -1.0)
glVertex3f(1.0, 0.2, 1.0)
glVertex3f(1.0, -0.2, 1.0)
glVertex3f(1.0, -0.2, -1.0)
glEnd()
# print self.ax
def read_data(self):
self.ax = self.bkb.read_float("IMU_EULER_X")
self.ay = -self.bkb.read_float("IMU_EULER_Y")
self.az = self.bkb.read_float("IMU_EULER_Z")
line_done = 0
# request data by sending a dot
# ser.write(".")
#while not line_done:
# line = ser.readline()
angles = [
self.ax * 180 / 3.1415, self.ay * 180 / 3.1415,
self.az * 180 / 3.1415
] #line.split(", ")
if len(angles) == 3:
self.ax = float(angles[0])
self.ay = float(angles[1])
self.az = float(angles[2])
line_done = 1
