import serial, math,time from visual import * ser = serial.Serial('COM5', 115200) data1=0; data2=0; data3=0; data4=0; data5=0; data6=0; data7=0; X=0; Y=0; Z=0; signo=0; scene.title='ECE 31289 UPB' mybox = box(pos=(50,10,57/2), length=100, height=20, width=57) pointer = arrow(pos=(0,0,0),axis=(0,100,0),shaftwidth=1) pointer1 = arrow(pos=(0,0,0),axis=(0,0,100),shaftwidth=1) pointer2 = arrow(pos=(0,0,0),axis=(100,0,0),shaftwidth=1) mybox.opacity=0.5 mybox.color= color.blue pointer.color= color.cyan pointer1.color= color.red pointer2.color= color.yellow picth=0 roll=0 yaw=0 scene.autoscale =0 theta = 0 # initial polar angle of shaft (from vertical) thetadot = 0 # initial rate of change of polar angle alpha = 0 # initial spin angle alphadot = 0.0 # initial rate of change of spin angle (spin ang. velocity) phi = -pi/2. # initial azimuthal angle phidot = 0 # initial rate of change of azimuthal angle dt = 0.0001 while 1: keys1=ord(ser.read(1)) if(keys1==7): data1=ord(ser.read(1)) data2=ord(ser.read(1)) data3=ord(ser.read(1)) data4=ord(ser.read(1)) data5=ord(ser.read(1)) data6=ord(ser.read(1)) data7=ord(ser.read(1)) X=(data1 & 1)*128 | (((data1>>1)&1)<<16) | (data2 & 127)| ((data3 & 127)<<8) if(X & 4096):# paso a signed short 16 bits X=-0x1000 + (X & 4095) Y=(data1>>2 & 1)*128 | (((data1>>3)&1)<<16) |(data4 & 127) | ((data5 & 127)<<8) if(Y & 4096):# paso a signed short 16 bits Y=-0x1000 + (Y & 4095) Z=(data1>>4 & 1)*128 | (((data1>>5)&1)<<16) |(data6 & 127) | ((data7 & 127)<<8) if(Z & 4096):# paso a signed short 16 bits Z=-0x1000 + (Z & 4095) print 'X='+ str(X) + ' Y=' + str(Y) + ' Z=' + str(Z) #pitch=math.atan((X/(math.sqrt(Y**2 +Z**2)+0.01))) #roll=math.atan((Y/(math.sqrt(X**2 +Z**2)+0.01))) #yaw=math.atan((math.sqrt(X**2 +Y**2)/(Z+0.01))) mybox.axis= X,Y,Z #mybox.axis= pitch,roll,yaw mybox.length=30 mybox.width=100 mybox.height=57