def centerHeadToPoint(self, pos): cur_pan = dm.headGetPan() cur_tilt = dm.headGetTilt() dist_x = self.IMG_WIDTH / 2. - pos[0] dist_y = self.IMG_HEIGHT / 2. - pos[1] if self.debug: print(dist_x, dist_y) new_pan = cur_pan + (dist_x * self.STEP_SIZE) new_tilt = cur_tilt + (dist_y * self.STEP_SIZE) return new_pan, new_tilt
def centerHeadToPoint(pos, debug=False): cur_pan = dm.headGetPan() cur_tilt = dm.headGetTilt() print("POSS:", pos) dist_x = 320. - pos[0] dist_y = 240. - pos[1] if debug: print(dist_x, dist_y) print("Pan: {} Tilt: {}".format(cur_pan, cur_tilt)) new_pan = cur_pan + (dist_x * STEP_SIZE) new_tilt = cur_tilt + (dist_y * STEP_SIZE) return new_pan, new_tilt
# Get obj info from tracker obj_info = tracker.getObj() if obj_info is not None: pos, _ = obj_info p, t = tracker.centerHeadToPoint(pos) dm.headMoveByAngle(p, t) ### STATE CHANGE STUFF ### target_info = TargetInfo() state = States.OBSERVE_TARGET ########################## else: cur_pan = dm.headGetPan() cur_tilt = dm.headGetTilt() if cur_pan > 120: look_dir = 0 elif cur_pan < 0: look_dir = 1 if look_dir == 1: dm.headMoveByAngle(cur_pan + LOOK_STEP, cur_tilt) else: dm.headMoveByAngle(cur_pan - LOOK_STEP, cur_tilt) # Observe target motion to predict its position later elif state == States.OBSERVE_TARGET: print("Observe Target.")
pygame.init() screen = pygame.display.set_mode((300, 300)) CONFIG_INI = "/home/juarez/Darwin-tools/Data/config.ini" # Sets up all the Action, Walking, Head and Motion Manager dm.initMotionManager(CONFIG_INI) # Stand up in a nice motion dm.playMotion(16) dm.headMoveToHome() while True: # Get current values of pan and tilt cur_pan = dm.headGetPan() cur_tilt = dm.headGetTilt() for event in pygame.event.get(): if event.type == QUIT: sys.exit(1) keys = pygame.key.get_pressed() # Amount to increment in each angle tilt_i = 0 pan_i = 0 mod = 1 if keys[K_LSHIFT]: mod = 0.25 if keys[K_s]: