def _tick(self, x, y, theta, urgency=0, keepFacing=False, relative=False, useAvoidance=True, useOnlySonarAvoid=False): urgency = min(1.0, urgency) self.keepFacing = keepFacing # Convert everything to relative. if relative: vectorToTarget = Vector2D.Vector2D(x, y) facingTurn = theta else: vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(Vector2D.Vector2D(x, y), theta) facingTurn = MathUtil.normalisedTheta(facingTurn) self.currentTarget = vectorToTarget targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading()) # forward/left are used for final adjustments, rotate to mean pos after the turn is made if vectorToTarget.isShorterThan(400) or keepFacing: forward = vectorToTarget.x * cos(-facingTurn) - vectorToTarget.y * sin(-facingTurn) left = vectorToTarget.x * sin(-facingTurn) + vectorToTarget.y * cos(-facingTurn) else: forward = vectorToTarget.x left = vectorToTarget.y self.currentState.urgency = urgency self.currentState.tick(forward, left, targetHeading, facingTurn)
def _tick(self, x, y, theta, urgency=0, keepFacing=False, relative=False, useAvoidance=True, extraObstacles=[]): # LedOverride.override(LedOverride.leftEye, Constants.LEDColour.off) # LedOverride.override(LedOverride.rightEye, Constants.LEDColour.off) self.keepFacing = keepFacing # Save destination for walkingTo self.world.b_request.walkingToX = int(x) self.world.b_request.walkingToY = int(y) if relative: new = FieldGeometry.addRrToRobot(Global.myPose(), x, y) self.world.b_request.walkingToX = int(new[0]) self.world.b_request.walkingToY = int(new[1]) # Convert everything to relative. if relative: vectorToTarget = Vector2D.Vector2D(x, y) facingTurn = theta else: for i in range(0, len(extraObstacles)): extraObstacles[ i] = FieldGeometry.globalPointToRobotRelativePoint( extraObstacles[i]) vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose( Vector2D.Vector2D(x, y), theta) # always keep heading the same after avoidance facingTurn = MathUtil.normalisedTheta(facingTurn) targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading()) if useAvoidance: vectorToTarget = self.calculateDestinationViaObstacles( vectorToTarget, extraObstacles) # avoidance doesn't change which way robot's facing self.currentTarget = vectorToTarget forward = vectorToTarget.x left = vectorToTarget.y self.currentState.urgency = urgency # myPose = Global.myPose() # print "Pos: (%5.0f, %5.0f < %2.2f) - CurrTarget: (%5.0f, %5.0f < %2.2f) fac?%s, rel?%s, flt: (%4.0f, %4.0f < T%2.2f, F%2.2f)" % ( # myPose.x, myPose.y, degrees(myPose.theta), x, y, degrees(theta), "Y" if keepFacing else "N", "Y" if relative else "N", forward, left, degrees(targetHeading), degrees(facingTurn) # ) self.currentState.tick(forward, left, targetHeading, facingTurn)
def _tick(self, x, y, theta, urgency=0, keepFacing=False, relative=False, useAvoidance=True, useOnlySonarAvoid=False): urgency = min(1.0, urgency) self.keepFacing = keepFacing # Convert everything to relative. if relative: vectorToTarget = Vector2D.Vector2D(x, y) facingTurn = theta else: vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose( Vector2D.Vector2D(x, y), theta) facingTurn = MathUtil.normalisedTheta(facingTurn) self.currentTarget = vectorToTarget targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading()) # forward/left are used for final adjustments, rotate to mean pos after the turn is made if vectorToTarget.isShorterThan(400) or keepFacing: forward = vectorToTarget.x * cos( -facingTurn) - vectorToTarget.y * sin(-facingTurn) left = vectorToTarget.x * sin( -facingTurn) + vectorToTarget.y * cos(-facingTurn) else: forward = vectorToTarget.x left = vectorToTarget.y self.currentState.urgency = urgency self.currentState.tick(forward, left, targetHeading, facingTurn)