def nextCommand(self): current_angle = self.robot.angle current_x = self.robot.x mid_x = (self.min_x + self.max_x) / 2 return self.go_to_asym(mid_x,self.robot.y) # If the robot is pointing left if current_angle > math.pi / 2 and current_angle < 3*math.pi / 2: # If the robot is on the right side of the zone if math.fabs(self.robot.x - self.max_x) < return_threshold: return self.go_forward(50) # If the robot is on the left side of the zone elif math.fabs(self.robot.x - self.min_x) < return_threshold: return self.go_backward(50) # If the robot is pointing right elif (current_angle > 0 and current_angle < math.pi / 2) or current_angle > 3*math.pi / 2: # If the robot is on the right side of the zone if math.fabs(self.robot.x - self.max_x) < return_threshold: return self.go_backward(50) # If the robot is on the left side of the zone elif math.fabs(self.robot.x - self.min_x) < return_threshold: return self.go_forward(50) else: print "Massive f*****g error!" return CommandDict.stop() command = self.go_to(mid_x, self.robot.y, speed=75) if command: return command return CommandDict.stop()
def update(self, model_positions): """ Main planner update function. Should be called once per frame. :param model_positions: A dictionary containing the positions of the objects on the pitch. (See VisionWrapper.model_positions) :return: The next command to issue to the robot. """ # Update the world state with the given positions self.world.update_positions(model_positions) # Update whether the robot is marked as busy on consol self.robot.is_busy() if self.world.ball != None: consol.log("Catcher", self.robot.catcher, "Robot") if self.current_plan.isValid() and not self.current_plan.isFinished(): return self.current_plan.nextCommand() else: # If self.current_plan is invalid, then choose a new plan and return a command from it for plan in self.plans: if plan.isValid(): prevPlan = self.current_plan same_pl = False if self.current_plan is plan: same_pl = True self.current_plan = plan if not same_pl: self.current_plan.initi(prevPlan) # self.current_plan.reset() return self.current_plan.nextCommand() else: return CommandDict.stop()
def __init__(self, pitch, color, our_side, video_port=0, comm_port='/dev/ttyACM0', quick=False, is_attacker=False): """ Entry point for the SDP system. Params: [int] video_port port number for the camera [string] comm_port port number for the arduino [int] pitch 0 - main pitch, 1 - secondary pitch [string] our_side the side we're on - 'left' or 'right' # """ self.controller = Controller(comm_port) if not quick: print("Waiting 10 seconds for serial to initialise") time.sleep(10) # Kick once to ensure we are in the correct position self.controller.update(CommandDict.kick()) self.pitch = pitch # Set up the vision system self.vision = VisionWrapper(pitch, color, our_side, video_port) # Set up the planner self.planner = Planner(our_side, pitch, attacker=is_attacker) # Set up GUI self.GUI = GUI(calibration=self.vision.calibration, pitch=pitch, launch=self) self.color = color self.side = our_side self.control_loop()
def nextCommand(self): ''' if self.robot.catcher != "prepared": self.robot.catcher = "prepared" return CommandDict.prepare() ''' # If we need to move to the ball, then get the command and return it # command = self.go_to(self.world.ball.x, self.world.ball.y, speed=75) command = self.go_to_asym(self.world.ball.x, self.world.ball.y, forward=True, max_speed = 70, min_speed=50) distance = self.robot.get_euclidean_distance_to_point(self.world.ball.x, self.world.ball.y) # this is a useful function that tells you how rotation aligns with wanted rotation dot = self.robot.get_dot_to_target(self.world.ball.x, self.world.ball.y) # if very close to ball if distance < DISTANCE_ERROR and dot > 0.96: self.finished = True self.robot.catcher = "closed" self.robot.set_busy_for(1.1) return CommandDict.catch() return command
def go_to_intercept(self, x, y): # Forward direction is < pi/2 on a circle. distance = self.robot.get_euclidean_distance_to_point(x, y) if distance == 0: return CommandDict.stop() command = ( self.go_forward(100) if (abs(self.robot.get_rotation_to_point(x, y)) < pi / 2) else self.go_backward(100) ) return command
def nextCommand(self): # Center of the robot's zone (x, y) = self.world.pitch.zones[self.robot.zone].center() command = self.go_to(x, y) if command is not False: return command else: self.finished = True return CommandDict.stop()
def nextCommand(self): their_atk = self.world.their_attacker # Center of the robot's zone (x,y) = self.world.pitch.zones[self.robot._zone].center() # Aim at friendly attacker position (gx,gy) = (self.world.our_defender.x, self.world.our_defender.y) isBlocked = self.blocked(gx, gy, their_atk.x, their_atk.y, self.robot.x, self.robot.y) #isBlocked = planning.utilities.is_shot_blocked(self.world, self.robot, self.world.their_attacker) if not isBlocked: # if our pass path is not blocked angle = self.robot.get_rotation_to_point(gx, gy) command = self.rotate_to(angle, fudge=0.1) # Check if we're done rotating if not command == False: return command # Otherwise kick the ball else: self.finished = True self.robot.catcher = "open" return self.kick(80) print "BLOCKED!" (cent_x, cent_y) = self.world.pitch.zones[self.robot.zone].center() # find the middle y position of the zone if their_atk.y >= cent_y: # if their attacker is on the top half we want to go toward the bottom half of the pitch goto_y = cent_y - cent_y/2 else: goto_y = cent_y + cent_y/2 command = self.go_to(cent_x, goto_y) if command: return command else: print "BLAAAAAAAAAAAAAAAAAAAAAAAH" return CommandDict.stop()
def nextCommand(self): if self.world.pitch.is_within_bounds(self.robot, self.robot.x, self.robot.y): return self.go_to(self.world.pitch.width / 4.0, self.world.pitch.height / 2.0, 100) else: return CommandDict.stop()
def nextCommand(self): # Plan is always finished to allow switching to other plans at any point. self.finished = True rotation_error = math.pi/15 (gx, gy) = self.goalCentre() consol.log("(gx, gy)", (gx,gy), "TakeShot") command = None dedge = 60 timeout = self.get_time() > 1.0 consol.log('state', self.state, 'ShootAll') if self.state == 'init': if timeout: self.nstate = 'go1' command = CommandDict.stop() elif self.state == 'go1': p = (self.midX, self.max_y -dedge) if self.arrived(*p): self.nstate = 'go2' elif self.has_clear_shot(): self.nstate = 'swing' self.res_timer() else: command = self.move_to(*p) elif self.state == 'go2': p = (self.midX, dedge) if self.arrived(*p): self.nstate = 'go1' elif self.has_clear_shot(): self.nstate = 'swing' self.res_timer() else: command = self.move_to(*p) elif self.state == 'swing': p = self.goalCentre() #if not self.has_clear_shot(): # self.nstate = 'go1' #else: if self.robot.get_dot_to_target(gx, gy) > 0.85: command = self.kick_new() self.res_timer() self.nstate = 'wait' else: command = self.look_at(gx, gy, max_speed=50, min_speed=40) elif self.state == 'wait': if timeout: self.robot.hball = False self.finished = True command = CommandDict.stop() ''' elif self.state == 'swing': p = self.goalCentre() if not self.has_clear_shot(): self.nstate = 'wall' elif timeout: command = self.kick_new() else: command = self.move_to(*p) elif self.state == 'testwall': if timeout: self.nstate = 'wall' command = self.catch() elif self.state == 'wall': p = (self.midX, self.midY) if self.arrived(*p): self.nstate = 'wallsh' self.res_timer() else: command = self.move_to(*p) elif self.state == 'wallsh': p = ((self.robot.x + gx) * 0.5, 0.0) if timeout: command = self.kick_new() self.res_timer() else: command = self.move_to(*p) ''' self.state = self.nstate return command
def catch(self): self.finished = True self.robot.catcher = "closed" #self.robot.set_busy_for(1.1) return CommandDict.catch()
def stop(self): """ Generates a stop command """ return CommandDict.stop()
def nextCommand(self): their_defender = self.world.their_defender # Center of the robot's zone (x, y) = self.world.pitch.zones[self.robot.zone].center() # Center of the goal (gx, gy) = (self.world.their_goal.get_polygon()[0][0], self.world.pitch.height / 2) isBlocked = self.blocked(gx, gy, their_defender.x, their_defender.y) consol.log('Blocked', isBlocked, 'Shoot Goal Plan') if not isBlocked: angle = self.robot.get_rotation_to_point(gx, gy) command = self.rotate_to(angle, fudge=0.2) # Check if we're done rotating if command is not False: return command # Otherwise kick the ball else: self.finished = True self.robot.catcher = "open" self.robot.set_busy_for(1.1) return self.kick() close_to_goal = False center = self.world.pitch.zones[self.world.their_defender.zone].center() if their_defender.x < 250: if their_defender.x < center[0]: close_to_goal = True else: if their_defender.x > center[0]: close_to_goal = True if close_to_goal == True: gy = gy - 75 angle = self.robot.get_rotation_to_point(gx, gy) command = self.rotate_to(angle, fudge=0.2) # Check if we're done rotating if not command == False: return command # Otherwise kick the ball else: self.finished = True self.robot.catcher = "open" self.robot.set_busy_for(1.1) return self.kick() else: #TODO #if self.robot.x is out of it's zone: # target_x = centre of robot's zone #else: # target_x = robot.x if their_defender.y < self.robot.y and self.robot.y < 200: #TODO retrieve the actual max Y for the pitch here command = self.go_to(self.robot.x, self.robot.y + 150) elif not self.robot.y < 20 and not self.robot.y > 200: # If robot was isn't too near the edge command = self.go_to(self.robot.x, self.robot.y -150) else: command = self.go_to(self.robot.x, 100) # If can't get round opponent on the wings, move towards the centre if command: return command else: command = self.go_to(self.robot.x, 200 - self.robot.y) # If that doesn't work, better to do something than nothing. if command: return command else: pdb.set_trace() return CommandDict.stop()