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()
Exemplo n.º 2
0
    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()
Exemplo n.º 3
0
    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
Exemplo n.º 10
0
 def catch(self):
     self.finished = True
     self.robot.catcher = "closed"
     #self.robot.set_busy_for(1.1)
     return CommandDict.catch()
Exemplo n.º 11
0
 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()