It seems to happen only when there are diagnostics.watch lines left uncommented, some more than others, but I'm not quite sure since it doesn't always happen. The code is very noodly, not to mention unfinished though, so I'm not 100% sure.
To trigger the bug, at least on my machine, start the script, and then quickly press and release buttons 2 and 4 on the hydra controller that is identified as being the right one. If it doesn't happen in the next few seconds, seems it is usually better to restart the script to get it faster; but again, I'm not quite sure since it's not always reproducible.
Code: Select all
from math import *
def sign(val): #Returns -1 for negatives, 0 for zero and 1 for positives
return val and (1, -1)[val<0] #(1 * (val > 0)) + (-1 * (val < 0))
import time
def applyHydraSide():
diagnostics.watch(hydra[0].side)
diagnostics.watch(hydra[1].side)
test = 0
global L, R
if(hydra[0].side == "L"):
L = 0
R = 1
test = "It's L"
if(hydra[0].side == "R"):
L = 1
R = 0
test = "It's R"
diagnostics.watch(test)
def update():
global rx,ry,yz,rrx,rry,rrz,lx,ly,lz,lrx,lry,lrz,posdz,rotdz,thrdz,isenabled
global centerposx,centerposy,centerposz,dragposx,dragposy,dragposz, posrange, lastdpos
global centerrotx,centerroty,centerrotz,dragrotx,dragroty,dragrotz, rotrange, lastdrot
global dragincremental
global targetposx, targetposy, targetposz, curposx, curposy, curposz, momposx, momposy, momposz, lastddpos
global targetrotx, targetroty, targetrotz, currotx, curroty, currotz, momrotx, momroty, momrotz, lastddrot
posrange = 150.0
rotrange = 60.0 / 180.0 * pi
applyHydraSide()
posdz = 0.2
rotdz = 0.35
if (hydra[R].one and (not(lastdpos))):
centerposx = hydra[R].x
centerposy = hydra[R].y
centerposz = hydra[R].z
dragincremental = True
targetposx = 0
targetposy = 0
targetposz = 0
curpostx = 0
curposty = 0
curpostz = 0
lastdpos = hydra[R].one
if (hydra[R].one):
dragposx = filters.ensureMapRange(centerposx - hydra[R].x, -posrange, posrange, -posrange, posrange)
dragposy = filters.ensureMapRange(centerposy - hydra[R].y, -posrange, posrange, -posrange, posrange)
dragposz = filters.ensureMapRange(centerposz - hydra[R].z, -posrange, posrange, -posrange, posrange)
else:
dragposx = 0.0
dragposy = 0.0
dragposz = 0.0
if (hydra[R].three and (not(lastdrot))):
centerrotx = hydra[R].pitch
centerroty = hydra[R].yaw
centerrotz = hydra[R].roll
dragincremental = True
targetrotx = 0
targetroty = 0
targetrotz = 0
currotx = 0
curroty = 0
currotz = 0
lastdrot = hydra[R].three
if (hydra[R].three):
dragrotx = filters.ensureMapRange(centerrotx - hydra[R].pitch, -rotrange, rotrange, -rotrange, rotrange)
dragroty = filters.ensureMapRange(centerroty - hydra[R].yaw, -rotrange, rotrange, -rotrange, rotrange)
dragrotz = filters.ensureMapRange(centerrotz - hydra[R].roll, -rotrange, rotrange, -rotrange, rotrange)
else:
dragrotx = 0.0
dragroty = 0.0
dragrotz = 0.0
if (hydra[R].two and (not(lastddpos))):
centerposx = hydra[R].x
centerposy = hydra[R].y
centerposz = hydra[R].z
dragincremental = False
lastddpos = hydra[R].two
if (hydra[R].two):
dragposx = (centerposx - hydra[R].x)
dragposy = (centerposy - hydra[R].y)
dragposz = (centerposz - hydra[R].z)
targetposx = targetposx + dragposx * 4000.0
targetposy = targetposy + dragposy * 4000.0
targetposz = targetposz + dragposz * 4000.0
centerposx = hydra[R].x
centerposy = hydra[R].y
centerposz = hydra[R].z
if (hydra[R].four and (not(lastddrot))):
centerrotx = hydra[R].pitch
centerroty = hydra[R].yaw
centerrotz = hydra[R].roll
dragincremental = False
lastddrot = hydra[R].four
if (hydra[R].four):
dragrotx = (centerrotx - hydra[R].pitch)
dragroty = (centerroty - hydra[R].yaw)
dragrotz = (centerrotz - hydra[R].roll)
targetrotx = targetrotx + dragrotx* 500000.0
targetroty = targetroty + dragroty* 500000.0
targetrotz = targetrotz + dragrotz* 500000.0
centerrotx = hydra[R].pitch
centerroty = hydra[R].yaw
centerrotz = hydra[R].roll
if (dragincremental == False):
nextposx = curposx + momposx
nextposy = curposy + momposy
nextposz = curposz + momposz
diffposx = targetposx - curposx
diffposy = targetposy - curposy
diffposz = targetposz - curposz
diffrotx = targetrotx - currotx
diffroty = targetroty - curroty
diffrotz = targetrotz - currotz
posrad = dist(diffposx, diffposy, diffposz)
rotrad = dist(diffrotx, diffroty, diffrotz)
posnormfac = max(abs(diffposx), max(abs(diffposy), abs(diffposz)))
rotnormfac = max(abs(diffrotx), max(abs(diffroty), abs(diffrotz)))
try:
global diffposx,diffposy,diffposz
global diffrotx,diffroty,diffrotz
diffposscale = 0.001
diffrotscale = 0.001
diffposx = diffposx * diffposscale
diffrotx = diffrotx * diffrotscale
diffposy = diffposy * diffposscale
diffroty = diffroty * diffrotscale
diffposz = diffposz * diffposscale
diffrotz = diffrotz * diffrotscale
except:
pass
targetmomposx = (abs(diffposx) ** 1) * sign(diffposx)
targetmomposy = (abs(diffposy) ** 1) * sign(diffposy)
targetmomposz = (abs(diffposz) ** 1) * sign(diffposz)
targetmomrotx = (abs(diffrotx) ** 1) * sign(diffrotx)
targetmomroty = (abs(diffroty) ** 1) * sign(diffroty)
targetmomrotz = (abs(diffrotz) ** 1) * sign(diffrotz)
diffmomposx = targetmomposx - momposx
diffmomposy = targetmomposy - momposy
diffmomposz = targetmomposz - momposz
diffmomrotx = targetmomrotx - momrotx
diffmomroty = targetmomroty - momroty
diffmomrotz = targetmomrotz - momrotz
posmomnormfac = max(abs(diffmomposx), max(abs(diffmomposy), abs(diffmomposz)))
rotmomnormfac = max(abs(diffmomrotx), max(abs(diffmomroty), abs(diffmomrotz)))
posmomrad = dist(diffmomposx,diffmomposy,diffmomposz)
rotmomrad = dist(diffmomrotx,diffmomroty,diffmomrotz)
try:
global thrposx
global thrposy
global thrposz
global momposx
global momposy
global momposz
thrposx = (diffmomposx / posmomnormfac) * min(posmomnormfac, 1)
thrposy = (diffmomposy / posmomnormfac) * min(posmomnormfac, 1)
thrposz = (diffmomposz / posmomnormfac) * min(posmomnormfac, 1)
momposx = momposx + thrposx
momposy = momposy + thrposy
momposz = momposz + thrposz
except:
pass
try:
global thrrotx
global thrroty
global thrrotz
global momrotx
global momroty
global momrotz
thrrotx = (diffmomrotx / rotmomnormfac) * min(rotmomnormfac, 1)
thrroty = (diffmomroty / rotmomnormfac) * min(rotmomnormfac, 1)
thrrotz = (diffmomrotz / rotmomnormfac) * min(rotmomnormfac, 1)
momrotx = momrotx + thrrotx
momroty = momroty + thrroty
momrotz = momrotz + thrrotz
except:
pass
curposx = curposx + momposx * 1.0
curposy = curposy + momposy * 1.0
curposz = curposz + momposz * 1.0
currotx = currotx + momrotx * 1.0
curroty = curroty + momroty * 1.0
currotz = currotz + momrotz * 1.0
try:
diagnostics.watch(targetposx)
diagnostics.watch(targetposy)
diagnostics.watch(targetposz)
diagnostics.debug(chr(int((time.clock()*1000.0)) % 26 + 65))
diagnostics.debug(curposx)
##diagnostics.watch(curposx)
diagnostics.watch(curposy)
diagnostics.watch(curposz )
diagnostics.watch(diffposx)
diagnostics.watch(diffposy)
diagnostics.watch(diffposz)
diagnostics.watch(momposx)
diagnostics.watch(momposy)
diagnostics.watch(momposz)
diagnostics.watch((targetmomposx))
diagnostics.watch((targetmomposy))
diagnostics.watch((targetmomposz))
diagnostics.watch(abs(diffmomposx))
diagnostics.watch(abs(diffmomposy))
diagnostics.watch(abs(diffmomposz))
pass
except:
pass
try:
diagnostics.watch(round(thrposx * 10000.) / 10000.)
diagnostics.watch(round(thrposy * 10000.) / 10000.)
diagnostics.watch(round(thrposz * 10000.) / 10000.)
diagnostics.watch(thrposx)
diagnostics.watch(thrposy)
diagnostics.watch(thrposz)
pass
except:
pass
try:
diagnostics.watch(targetrotx)
diagnostics.watch(targetroty)
diagnostics.watch(targetrotz)
diagnostics.watch(currotx )
diagnostics.watch(curroty )
diagnostics.watch(currotz )
diagnostics.watch(diffrotx)
diagnostics.watch(diffroty)
diagnostics.watch(diffrotz)
diagnostics.watch(momrotx)
diagnostics.watch(momroty)
diagnostics.watch(momrotz)
diagnostics.watch((targetmomrotx))
diagnostics.watch((targetmomroty))
diagnostics.watch((targetmomrotz))
diagnostics.watch(abs(diffmomrotx))
diagnostics.watch(abs(diffmomroty))
diagnostics.watch(abs(diffmomrotz))
pass
except:
pas
try:
diagnostics.watch(round(thrrotx * 10000.) / 10000.)
diagnostics.watch(round(thrroty * 10000.) / 10000.)
diagnostics.watch(round(thrrotz * 10000.) / 10000.)
diagnostics.watch(thrrotx)
diagnostics.watch(thrroty)
diagnostics.watch(thrrotz)
pass
except:
pass
try:
rx = thrposx * -range
ry = thrposy * range
rz = thrposz * range
rrx = thrrotx * -range
rry = thrroty * -range
rrz = thrrotz * -range
except:
pass
else:
rx = filters.deadband(filters.ensureMapRange(-dragposx, -posrange, posrange, -range, range), posdz, -range, range)
ry = filters.deadband(-filters.ensureMapRange(-dragposy, -posrange, posrange, -range, range), posdz, -range, range)
rz = filters.deadband(filters.ensureMapRange(dragposz, -posrange, posrange, -range, range), posdz, -range, range)
Rmat = quat2mat(hydra[R].q0, hydra[R].q1, hydra[R].q2, hydra[R].q3)
rotrange = pi/8
rrx = filters.deadband(filters.ensureMapRange(-dragrotx, -rotrange, rotrange, -range, range), rotdz, -range, range)
rry = filters.deadband(filters.ensureMapRange(-dragroty, -rotrange, rotrange, -range, range), rotdz, -range, range)
rrz = filters.deadband(filters.ensureMapRange(-dragrotz, -rotrange, rotrange, -range, range), rotdz, -range, range)
try:
ppJoy[0].setAxis (AxisTypes.X, rx)
ppJoy[0].setAxis (AxisTypes.Y, ry)
ppJoy[0].setAxis (AxisTypes.ZAxis, rz)
ppJoy[0].setAxis(AxisTypes.XRotation, rrx)
ppJoy[0].setAxis(AxisTypes.YRotation, rry)
ppJoy[0].setAxis(AxisTypes.ZRotation, rrz)
ppJoy[1].setAxis(AxisTypes.X, hydra[R].joyx * range)
ppJoy[1].setAxis(AxisTypes.Y, -hydra[R].joyy * range)
ppJoy[1].setAxis(AxisTypes.ZAxis, (hydra[R].trigger * ( ((hydra[R].bumper * -1.0) + (not(hydra[R].bumper) * 1)))) * range)
except:
pass
def quat2mat(w, x, y, z):
return [ -(2 * x * z + 2 * y * w), -(2 * x * y - 2 * z *w), 1 - 2 * (y ** 2) - 2 * (z**2),
-(2 * y * z - 2 * x * w), -(1 -2 * (x ** 2) - 2 * (z ** 2)), 2 * x * y + 2 * z * w,
-(1 - 2 * (x ** 2) - 2 * (y**2)), -(2 * y * z + 2 * x * w), 2 * x * z - 2 * y * w]
def dist(x,y,z):
return sqrt((x**2) + (y**2) + (z**2))
if starting:
global thing
global rx,ry,yz,rrx,rry,rrz,lx,ly,lz,lrx,lry,lrz,posdz,rotdz,thrdz,range
global R,L
global isenabled
global centerposx,centerposy,centerposz,dragposx,dragposy,dragposz, posrange, lastdpos
global centerrotx,centerroty,centerrotz,dragrotx,dragroty,dragrotz, rotrange,lastdrot
global dragincremental
global targetposx, targetposy, targetposz, curposx, curposy, curposz, momposx, momposy, momposz, lastddpos
global targetrotx, targetroty, targetrotz, currotx, curroty, currotz, momrotx, momroty, momrotz,lastddrot
centerposx,centerposy,centerposz,dragposx,dragposy,dragposz, posrange, lastdpos = (0,)*8
centerrotx,centerroty,centerrotz,dragrotx,dragroty,dragrotz,rotrange, lastdrot = (0,)*8
targetposx, targetposy, targetposz, curposx, curposy, curposz, momposx, momposy, momposz, lastddpos = (0,)*10
targetrotx, targetroty, targetrotz, currotx, curroty, currotz, momrotx, momroty, momrotz, lastddrot = (0,)*10
dragincremental = True
isenabled = True
L = 0
R = 1
rx = 0
ry = 0
rz = 0
rrx = 0
rry = 0
rrz = 0
lx = 0
ly = 0
lz = 0
lrx = 0
lry = 0
lrz = 0
posdz = 0
rotdz = 0
thrdz = 0
range = 1024
applyHydraSide()
ppJoy[0].setRange(-range, range)
ppJoy[1].setRange(-range, range)
ppJoy[2].setRange(-range, range)
ppJoy[3].setRange(-range, range)
update()