# * @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