Exemplo n.º 1
0
 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)
Exemplo n.º 2
0
 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