#  * @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
Esempio n. 2
0
#  * @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
Esempio n. 3
0
#!/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)
Esempio n. 4
0
#!/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)
Esempio n. 5
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