I think I might have come across the problem...I need to be able to calculate what yaw and pitch I should have so that the red dot is on top of the target. In order to do that I have to calculate the appropriate yaw/pitch based on the target position and my head position. However, I think it's constantly updating the targetYawPitch (targetYawPitch[0] = yaw, targetYawPitch[1] = pitch) based on how I'm moving and thus I'm getting funky results.
For example, let's say to look at A I need targetYawPitch[0] 2.3178497724, targetYawPitch[1] -11.1697861608 (This is what is originally calculated). I need those values to remain fixed so that when my yaw and pitch get within the 1 degree hotspot I trigger the next target. However, you'll notice that I'm hitting the target at: ((currentYaw - self.targetYawPitch[0]) < self.hotspotTolerance) = ((2.63679909706 -2.89097381937) < True)
True
((currentPitch - self.targetYawPitch[1]) < self.hotspotTolerance) = ((-4.72426462173 --5.53390520328) < True)
True
Hit Target A at [2.8909738193736922, -5.5339052032813196]
While the 2.89 is close to the original 2.31 the -5.5 is not at all close to the original -11.16. (See output below).
Similar things happen with other targets as well...sometimes I need to be far below the target to trigger the hotspot.
I have no idea how to fix this...
Going to next target
targetYawPitch [None, None]
newTarget A
currentTargetPos [0.17000000178813934, 0.99000000953674316, 4.1999998092651367]
targetYawPitch[0] 2.3178497724
targetYawPitch[1] -11.1697861608
CheckHeading
currentYaw -5.38346481323
currentPitch -10.2835168839
currentYaw -5.38346481323
currentPitch -10.2835168839
currentYaw -4.48255825043
currentPitch -11.2283706665
.
.
.
currentYaw 2.61482572556
currentPitch -3.40586543083
currentYaw 2.63679909706
currentPitch -4.72426462173
((currentYaw - self.targetYawPitch[0]) < self.hotspotTolerance) = ((2.63679909706 -2.89097381937) < True)
True
((currentPitch - self.targetYawPitch[1]) < self.hotspotTolerance) = ((-4.72426462173 --5.53390520328) < True)
True
Hit Target A at [2.8909738193736922, -5.5339052032813196]
Going to next target
targetYawPitch [None, None]
oldTarget A
newTarget H
currentTargetPos [1.2699999809265137, 1.3799999952316284, 4.1999998092651367]
targetYawPitch[0] 18.8303756023
targetYawPitch[1] 0.32497106846
*** Reached point A
Elapsed time =11.7494435745 seconds
currentYaw 2.54890584946
currentPitch -4.94399785995
currentYaw 2.48298597336
currentPitch -5.25162506104
currentYaw 2.48298597336
currentPitch -5.25162506104
.
.
.
currentYaw 18.6773281097
currentPitch -1.09866631031
currentYaw 19.0948219299
currentPitch 0.703146457672
((currentYaw - self.targetYawPitch[0]) < self.hotspotTolerance) = ((19.0948219299 -18.7518220628) < True)
True
((currentPitch - self.targetYawPitch[1]) < self.hotspotTolerance) = ((0.703146457672 -0.285288887733) < True)
True
Hit Target H at [18.751822062820551, 0.28528888773343869]
Going to next target
targetYawPitch [None, None]
oldTarget H
newTarget I
oldOldTarget A
currentTargetPos [2.3299999237060547, 1.0349999666213989, 4.1999998092651367]
targetYawPitch[0] 31.7820902079
targetYawPitch[1] -4.17668542975
*** Reached point H
Elapsed time =14.3208748598 seconds
|