def BUOY_FOUND(self,event):
if(event.color == vision.Color.RED):
#get rotation matrix from robot frame to intertial frame, then apply it to the x,y measurements
#this rotation matrix must exclude yaw, this ends up looking a bit silly because of quaternion math
q1 = self.stateEstimator.getEstimatedOrientation()
v1 = math.Vector3(event.x,event.y,0)
#now get the quaternion to unrotate our yaw, by unrotating by our level quaternion
qp = control.holdCurrentHeadingHelper(q1)
#by rotating to the inertial frame, then rotating to the body yaw frame we acheive our goal
#rotate coordinates from body frame to inertial frame
v1p = qp*(q1.UnitInverse()*v1)
event.x = v1p.x
event.y = v1p.y
self.run(event)
def _angleyaw(self, event):
"""
When we want to yaw the robot, send a command to the controller
to rotate the robot around the body z axis proportional to the
event command. When we receive a zero rate command, we should
only pass along the command to the controller ONCE.
"""
change = (event.number / 100.0) * self._angleYawGain
if change == 0 and self._lastYawCommand == 0:
# dont sent ZERO command twice
# note that we do want to send commands repeatedly if
# the speed is not zero
pass
else:
ori = self._estimator.getEstimatedOrientation()
qHeading = control.holdCurrentHeadingHelper(ori)
self._controller.rotate(qHeading * math.Quaternion(
math.Degree(change), math.Vector3.UNIT_Z))
self._lastYawCommand = change