Need4Butiá

1. butiaRemoto

import butiaAPI import time import struct import math from numpy import linalg from numpy import matlib

1. def getXY(in_file_f):
2. x = 0
3. y = 0
4. z = 0
5. event = in_file_f.read(16)
6. (time1,time2, type, code, value) = \
7. struct.unpack(fmt,event)
8. time = time2 / 1000.0
9. if type == 2 or type == 3:
10. if code == 0:
11. x = value
12. if code == 1:
13. y = value
14. if code == 2:
15. z = value
16. if type == 0 and code == 0:
17. sum = 0
18. return x,y

mor = (0,1,0) xbase = (1,0,0) ybase = (0,0,1) def normal(): xyz = getXYZ() tam = linalg.norm(xyz) mor = matlib.matrix(xyz)/tam print xyz,tam,mor ybase = matlib.cross(mor,(1,0,0)) ybase /= linalg.norm(ybase) xbase = matlib.cross(ybase,mor) xbase /= linalg.norm(xbase) print mor,linalg.norm(mor) print xbase,linalg.norm(xbase) print ybase,linalg.norm(ybase) print matlib.inner(xbase,ybase),matlib.inner(xbase,mor) print matlib.inner((1,0,0),ybase),matlib.inner(ybase,mor) print matlib.inner(mor,xyz), print matlib.inner(xbase,xyz), print matlib.inner(ybase,xyz), print (matlib.inner(mor,xyz)**2 + matlib.inner(mor,xyz)**2 + matlib.inner(mor,xyz))**0.5 return (xbase,mor,ybase)

ACCELEROMETER_DEVICE='/sys/devices/platform/lis3lv02d/position'

def getXYZ():

        return accelerometer xyz as a tuple 
       try:
           fh.seek(0)
           string = fh.read()
           xyz = eval(string)
           xyz =tuple(map(lambda nuevo: int(nuevo),xyz))
           return xyz
       except IOError as ioerror:
               print ioerror
               return (0,0,0)

def getY(): xyz=getXYZ() return matlib.inner(xyz,ybase)

def getX(): xyz=getXYZ() return matlib.inner(xyz,xbase)

def dir(n): if (n>0): return ('1',str(n)) else: return ('0',str(-n)) def dir2(iz,de): (sentIz,magIz)=dir(iz) (sentDe,magDe)=dir(de) return (str(sentIz),str(magIz),str(sentDe),str(magDe))

butiabot = butiaAPI.robot('169.254.11.29') modulos = butiabot.get_modules_list() print modulos

1. butiabot.abrirSensor()
2. butiabot.abrirMotores()

a = 'q' #no hace nada

fh = open(ACCELEROMETER_DEVICE,'r')

1. Inicializo base en equilibrio

(xbase,mor,ybase)=normal() time.sleep(1) ANGULO_MAX = 500 MOTOR_MAX = 1000

1. try:
2. while True:
3. print matlib.inner(mor,getXYZ()),
4. print matlib.inner(xbase,getXYZ()),
5. print matlib.inner(ybase,getXYZ())
6. except:
7. pass

while True: x1 = getX()[0] y1 = getY()[0] print("x" + str(x1) + "y" + str(y1)) #topeo a la mitad del recorrido iz = x1+y1 de = -(x1-y1) if(max(abs(iz),abs(de))>1000): iz=iz*1000/max(abs(iz),abs(de)) de=de*1000/max(abs(iz),abs(de)) dires=dir2(iz,de) print dires butiabot.set2MotorSpeed(*dires) print v

fh.close()

1. in_file.close()

print "fin"