def look_at(self, x, y, min_speed = 50, max_speed = 80):
        """
        Generates commands for the robot to rotate to a specific angle
        :param x: X coordinate of the point to turn to
        :param y: Y coordinate of the point to turn to
        :return:  returns a CommandDict with the next command
        """

        min_rot_speed = min_speed
        max_rot_speed = max_speed


        rob_pos = np.array([self.robot.x, self.robot.y])
        target_pos = np.array([x, y])
        vec = target_pos - rob_pos
        av = vec / np.linalg.norm(vec)

        rv = Plan.angle_to_vector(self.robot.angle)


        dot = np.dot(av, rv)
        cross = np.cross(rv, av)

        consol.log('dot', dot, 'Plan')
        consol.log('cross', cross, 'Plan')

        speed = np.interp(dot, [0.0, 1.0], [max_rot_speed, min_rot_speed])

        direction = "Right" if cross < 0 else "Left"
        kick = "None"
        return CommandDict(speed, direction, kick)
    def has_clear_shot(self):
        obstacle_width=25

        (target_x, target_y) = self.goalCentre()
        their_defender = self.world.their_defender

        #If their defender is not on the pitch, return True:
        consol.log("defender pos", (their_defender.x, their_defender.y), "TakeShot")
        consol.log("defender angle", their_defender.angle, "TakeShot")
        if their_defender._vector.x == their_defender._vector.y and their_defender._vector.x == 0:
            return True

        obstacle_x = their_defender.x
        obstacle_y = their_defender.y

        d_y = self.robot.y - target_y
        d_x = self.robot.x - target_x
        if d_x == 0:
            d_x = 0.1
        m = d_y/float(d_x)
        c = self.robot.y - m*self.robot.x
        #Compare y-coords when x is equal:
        ball_y_at_obstacle = m*obstacle_x + c
        if math.fabs(ball_y_at_obstacle - obstacle_y)<obstacle_width:
            return False
        return True
    def rotate_fade(self, angle, min_speed = 50, max_speed = 80):
        """
        Generates commands for the robot to rotate to a specific angle
        :param angle: Radians to turn to
        :return: a CommandDict with the next command
        """

        min_rot_speed = min_speed
        max_rot_speed = max_speed

        av = Plan.angle_to_vector(angle)
        rv = Plan.angle_to_vector(self.robot.angle)


        dot = np.dot(av, rv)
        cross = np.cross(rv, av)

        consol.log('dot', dot, 'Plan')
        consol.log('cross', cross, 'Plan')

        speed = np.interp(dot, [0.0, 1.0], [max_rot_speed, min_rot_speed])

        direction = "Right" if cross < 0 else "Left"
        kick = "None"
        return CommandDict(speed, direction, kick)
    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 set_motor(self, motor, speed, scale=False, delay=0):
        # If the motor is already running at this speed, do not send the command
        # this creates bugs when enabling comms so i commented it out
        #if speed == self.current_motor_speeds[motor]:
         #   return

        
        if scale:
            scaled_speed = self.get_scaled_speed(motor, speed)
        else:
            scaled_speed = speed


        if motor == 'left':
            log("Left speed", speed, "Comms")

        if motor == 'right':
            log("Right speed", speed, "Comms")


        # floats are to large to transfer over serial as sting
        int_val = int(scaled_speed)

        self._write_serial("s {0} {1} {2}".format(self.motorPins[motor], int_val, delay))
        lp = 0.3
        self.current_motor_speeds[motor] = (1.0 - lp) *self.current_motor_speeds[motor] + lp *speed
    def is_busy(self):
        if hasattr(self, 'busy_until'):
            busy = datetime.datetime.now() < self.busy_until
        else:
            busy = False

        consol.log("Is Busy", busy, "Robot")
        return busy
 def isValid(self):
     """
     Current constraints are:
         - Robot must have the ball
         - Shot must not be blocked(not implemented)
     """
     consol.log("Clear shot", self.has_clear_shot(), "TakeShot")
     return self.robot.has_ball(self.world.ball)  and (not self.robot.is_busy())
    def enabled(self, e):
        if not e:
            self.stop()




        self._enabled = e
        log('Communication enabled (press C to toggle)', self.enabled, "Comms")
    def __init__(self, calibration):
        consol.log('use y r b d p and click on objects in video to calibrate', None)
        self.color = 'plate'
        # self.pre_options = pre_options
        self.calibration = calibration
        self.maskWindowName = "Mask " + self.color
        self.frame = None

        self.setWindow()
Beispiel #10
0
    def __init__(self, calibration):
        consol.log('use y r b d p and click on objects in video to calibrate', None)
        self.color = 'red'
        # self.pre_options = pre_options
        self.calibration = calibration
        self.maskWindowName = "Mask " + self.color
        self.frameWindowName = "Frame window"
        self.frame = None

        self.setWindow()
 def isValid(self):
     """
     Current constraints are:
         - Robot must have the ball
         - Robot must be within 50px of the wall
     """
     consol.log("distance_from_wall", self.distance_from_wall(), "RetreatFromWall")
     is_valid = self.robot.has_ball(self.world.ball) and self.distance_from_wall() <= 50 and (not self.robot.is_busy())
     consol.log("is_valid", is_valid, "RetreatFromWall")
     return is_valid
def update():
    ra = robot_api.robot_api

    dt = time() - FrameEst.last_time
    fest = FrameEst(dt, ra.current_motor_speeds["right"], ra.current_motor_speeds["left"], time())

    consol.log("time", time(), "Future")

    FrameEst.est = [x for x in FrameEst.est if x.time > time() - FrameEst.vision_late] + [fest]

    FrameEst.last_time = time()
    def __init__(self, calibration, name="Camera calibration"):

        consol.log('set camera settings so that all objects are detected',
                   None)

        self.calibration = calibration
        self.frameWindowName = name
        self.frame = None
        self.setting = 'camera'

        self.setWindow()
    def ball_distance_from_wall(self):

        "Returns the distance of the ball from the wall nearest to it."
        cur_y = self.world.ball.y
        bottom_dist = cur_y
        top_dist = self.max_y - cur_y
        consol.log("top_dist", top_dist, "GrabAll")
        consol.log("bottom_dist", bottom_dist, "GrabAll")
        if top_dist < bottom_dist:
            return top_dist
        else:
            return bottom_dist
def is_shot_blocked(world, our_robot, their_robot):
    '''
    Checks if our robot could shoot past their robot
    '''
    predicted_y = predict_y_intersection(
        world, their_robot.x, our_robot, full_width=True, bounce=True)
    if predicted_y is None:
        return True
    consol.log('##########', (predicted_y, their_robot.y, their_robot.length), 'Utils')
    b = abs(predicted_y - their_robot.y) < their_robot.length
    consol.log('is blocked', b, 'Utils')
    return b
 def isValid(self):
     """
     Current constraints are:
         - Robot must have the ball
         - Shot must not be blocked
     """
     #return True
     consol.log("Clear shot", self.has_clear_shot(), "ShootAll")
     ball_dist = self.robot.get_euclidean_distance_to_point(*self.get_ball_pos()) > 200 and self.world.ball.x != 0
     if ball_dist:
         self.robot.hball = False
     return self.robot.hball
    def isMatched(self, robot, ball):
        max_e_dist = 40


        their_defender = self.world.their_defender
        (target_x, target_y) = self.goalCentre()




        ball_y = self.world.ball.y
        ball_x = self.world.ball.x
        zone = self.world.pitch.zones[self.robot.zone]


        zone = self.world.pitch.zones[self.robot.zone]


        between = (ball_x - target_x) * (ball_x - self.midX) < 0.0
        if between or ball_x < 1.0:
            def_ang = their_defender._vector.angle
            def_pos = their_defender._vector

            p1 = array( [self.midX, 0.0] )
            p2 = array( [self.midX, self.max_y] )

            ad = array([math.cos(def_ang), math.sin(def_ang)])

            p3 = array( [def_pos.x, def_pos.y] )
            p4 = array( p3 + ad)

            si = MyMath.seg_intersect( p1,p2, p3,p4)
            #log_dot(si, 'yellow', 'haha')
            #log('inter', si, 'MatchY')
            g_to_mid = self.midX - target_x


            y = si[1]

            y = clip(y, max_e_dist, self.max_y - max_e_dist)

            consol.log('facing', g_to_mid * ad[0] > 0.0, 'MatchY')

            if g_to_mid * ad[0] > 0.0:
                return math.fabs(robot.y - y) < ERROR
            else:
                return math.fabs(robot.y - self.midY) < ERROR

        else:
            y = clip(ball.y, max_e_dist, self.max_y - max_e_dist)
            return math.fabs(robot.y - y) < ERROR
    def nextCommand(self):

        their_defender = self.world.their_defender
        (target_x, target_y) = self.goalCentre()




        ball_y = self.world.ball.y
        ball_x = self.world.ball.x
        zone = self.world.pitch.zones[self.robot.zone]


        zone = self.world.pitch.zones[self.robot.zone]

        max_e_dist = 40
        between = (ball_x - target_x) * (ball_x - self.midX) < 0.0
        if between or ball_x < 1.0:
            def_ang = their_defender._vector.angle
            def_pos = their_defender._vector

            p1 = array( [self.midX, 0.0] )
            p2 = array( [self.midX, self.max_y] )

            ad = array([math.cos(def_ang), math.sin(def_ang)])

            p3 = array( [def_pos.x, def_pos.y] )
            p4 = array( p3 + ad)

            si = MyMath.seg_intersect( p1,p2, p3,p4)
            #log_dot(si, 'yellow', 'haha')
            #log('inter', si, 'MatchY')
            g_to_mid = self.midX - target_x


            y = si[1]

            y = clip(y, max_e_dist, self.max_y - max_e_dist)

            consol.log('facing', g_to_mid * ad[0] > 0.0, 'MatchY')

            if g_to_mid * ad[0] > 0.0:
                command = self.go_to_asym(zone.center()[0], y, max_speed=100, min_speed=50)
            else:
                command = self.go_to_asym(self.midX, self.midY, max_speed=100, min_speed=50)

        else:
            y = clip(ball_y, max_e_dist, self.max_y - max_e_dist)
            command = self.go_to_asym(zone.center()[0], ball_y, max_speed=100, min_speed=50)
        return command
    def __init__(self, device_path=None, baud_rate=None):
        # check if there are valid parameters

        self._enabled = False
        if (device_path is not None and baud_rate is not None):
            try:
                self.serial = Serial(device_path, baud_rate, timeout=0.1)
            except:
                log("Error in initalizing serial connection. Is the path correct?", False, 'Comms')
                #alias the _write_serial function so we don't throw errors
                self._write_serial = self._write_serial_debug

        #just for printing
        self.enabled = False

        global robot_api
        robot_api = self
    def has_clear_shot(self):

        edge = 50

        if self.robot.y < 50 or self.robot.y > self.max_y - 50:
            return False

        obstacle_width=25

        (target_x, target_y) = self.goalCentre()
        their_defender = self.world.their_defender

        pos_diff = math.fabs(their_defender._vector.y - self.robot.y)




        #if no defender
        if their_defender._vector.y < 1.0:
            consol.log("clear shoot", True, "ShootAll")
            return True

        rob_y = self.robot.y

        relo = rob_y - target_y
        reld = their_defender._vector.y - target_y
        consol.log("clear shoot", relo * reld < 0.0, "ShootAll")
        return relo * reld < 0.0 and pos_diff > 50




        #If their defender is not on the pitch, return True:
        consol.log("defender pos", (their_defender.x, their_defender.y), "ShootAll")
        consol.log("defender angle", their_defender.angle, "ShootAll")
        if their_defender._vector.x == their_defender._vector.y and their_defender._vector.x == 0:
            return True

        obstacle_x = their_defender.x
        obstacle_y = their_defender.y

        d_y = self.robot.y - target_y
        d_x = self.robot.x - target_x
        if d_x == 0:
            d_x = 0.1
        m = d_y/float(d_x)
        c = self.robot.y - m*self.robot.x
        #Compare y-coords when x is equal:
        ball_y_at_obstacle = m*obstacle_x + c
        if math.fabs(ball_y_at_obstacle - obstacle_y)<obstacle_width:
            return False
        return True
 def isValid(self):
     """
     Current constraints are:
         - Ball must be within the robot's zone
         - Robot must not have the ball
         - Ball must be near a wall
     """        
     # See if self.has_ball exists:
     if not hasattr(self, 'has_ball'):
         self.has_ball = True
         
     consol.log("Ball dist from wall", self.ball_distance_from_wall(), "GrabNearWall")
     near_dist = 40 # Note DO NOT CHANGE WITHOUT ALSO CHANGING IN GRABBALLPLAN
     if self.world.ball is not None and self.world.ball.velocity <= 3:
         isValid = self.world.pitch.is_within_bounds(self.robot, self.world.ball.x, self.world.ball.y) and \
                ((not self.robot.has_ball(self.world.ball)) or (not self.has_ball)) and \
                self.ball_distance_from_wall <= near_dist and (not self.robot.is_busy())
     else:
         isValid = False
     consol.log("isValid", isValid, "GrabNearWall")
     return isValid
    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")

        #If we are facing the goal, shoot!
        consol.log("Scalar product", self.robot.get_dot_to_target(gx, gy), "TakeShot")
        if self.robot.get_dot_to_target(gx, gy) > 0.985:
            if self.frames_passed >= self.frames_before_shot:
                self.finished = True
                self.robot.catcher = "open"
                self.robot.set_busy_for(1.1)
                return self.kick()
            else:
                self.frames_passed += 1
                return self.stop()
        else:
            command = self.look_at(gx, gy, max_speed=55, min_speed=40)
            return command
def get_future():
    d = [0, 0]
    a = 0

    consol.log("frames", [(int(x.len * 1000), int(x.sl), int(x.sr)) for x in FrameEst.est], "Future")

    for i in FrameEst.est:
        das = (
            np.interp((i.sr - i.sl) * 0.5, [-100, -10, 10, 100], [-FrameEst.rot_speed, 0, 0, FrameEst.rot_speed])
            * i.len
        )
        a += das

        dds = np.interp((i.sl + i.sr) * 0.5, [-100, -40, 40, 100], [-FrameEst.speed, 0, 0, FrameEst.speed]) * i.len

        # assuming average angle
        d += rot_vec([dds, 0], a * 0.5)

    # consol.log('future rel', (d,a), 'Future')

    return (d, a)
 def shoot(self, gx, gy):
     angle = self.angle_to_goal(self.robot.x, self.robot.y, gx, gy)
     command = self.rotate_fade(angle, min_speed = 50, max_speed = 70)
     consol.log("Goal angle",angle,"Attacking")
     # Check if we're done rotating
     consol.log("Delta_angle", math.fabs(angle - self.robot.angle), "Attacking")
     if math.fabs(angle - self.robot.angle) > math.pi/24:
         consol.log("Command", command, "Attacking")
         return command
     # If we are then kick the ball
     else:
         self.finished = True
         self.robot.catcher = "open"
         self.robot.target_y = None
         consol.log("Command", "Kick", "Attacking")
         return self.kick()
    def _write_serial(self, data):
        if not self.enabled:
           return


        ack = False

        num_attempts = 0
        # Test code that will drop the majority of commands to test fault tollerance

        data_bytes = str.encode(data)
        data_bytes += '\r'
        self.serial.write(data_bytes)

        log("Sent command", format(data), "Comms")
        log_time('Comms')

        return


        while not ack:
            data_bytes = str.encode(data)
            data_bytes += '\r'
            self.serial.write(data_bytes)
            num_attempts += 1

            log("Sent command", format(data), "Comms")
            log_time('Comms')

            try:
                ack_str = self.serial.read(4)
                log("Ack string", ack_str, "Comms")
                print ('ack:' +ack_str)

                if ack_str == "ack6":
                    ack = True
                else:
                    ack = False
            except SerialException as ex:
                ack = False

            log("Ack", ack, "Comms")
            
            if num_attempts >= 40:
                raise Exception("Too many attempts to send command")
    def nextCommand(self):
        # Plan is always finished to allow switching to other plans at any point
        # E.g. making a shot if an opening is available.
        self.finished = True

        consol.log("(mx, my)", (self.mx,self.my), "WallShotPlan")

        #If we are not at the move target, move there:
        distance = self.robot.get_euclidean_distance_to_point(self.mx, self.my)
        consol.log("Distance to move target", distance, "WallShotPlan")
        if distance > 18 and not self.has_moved:
            command = self.go_to_asym(self.mx, self.my, forward=False, max_speed = 85, min_speed=50)
            return command
        #Otherwise, make a wall shot:
        else:    
            #Flag that move is complete:
            self.has_moved = True
            #Aim at x= slightly in fron of centre of opponents zone, y=nearer edge of pitch
            centre_x = self.world.pitch.zones[self.world.their_defender.zone].center()[0]
            if self.world.their_defender.zone == 0:
                target_x = centre_x 
            elif self.world.their_defender.zone == 3:
                target_x = centre_x 
            if self.robot.y > self.world.pitch.zones[self.robot.zone].center()[1]:
                target_y = self.max_y + 10
            else:
                target_y = -10

            consol.log("Shoot_target", (target_x, target_y), "WallShotPlan")
            consol.log("Dot to target", self.robot.get_dot_to_target(target_x, target_y), "WallShotPlan")
            if self.robot.get_dot_to_target(target_x, target_y) > 0.991:
                if self.frames_passed >= self.frames_before_shot:
                    self.finished = True
                    self.robot.catcher = "open"
                    self.robot.set_busy_for(1.1)
                    return self.kick()
                else:                    
                    self.frames_passed += 1
                    return self.stop()
            else:
                self.frames_passed = 0                
                command = self.look_at(target_x, target_y, max_speed=55, min_speed=40)
                return command
    def update(self, command):
        if command is None:
            return

        speed = command["speed"]
        direction = command["direction"]
        kick = command["kick"]

        log_time('dict_control')
        log('speed', speed, 'dict_control')
        log('direction', direction, 'dict_control')
        log('kick', kick, 'dict_control')

        if direction == "Forward":
            log_time('dict_control', 'time go forward called')
            if 'speedr' in command:
                speed_right = command["speedr"]
                self.robot_api.go_forward_asym(speed, speed_right)
            else:
                self.robot_api.go_forward(speed)

        elif (direction == "Backward"):
            self.robot_api.go_backward(speed)
        elif (direction == "Right"):
            self.robot_api.turn_right(speed)
        elif (direction == "Left"):
            self.robot_api.turn_left(speed)
        elif (direction == "None"):
            for i in range(0, 10):
                self.robot_api.stop()
                time.sleep(.001)

        if (kick == "Kick"):
            for i in range(0, 10):
                self.robot_api.kick(100)
                time.sleep(.001)
        elif (kick == "Prepare"):
            log_time("dict_control", "prepare")
            for i in range(0, 10):
                self.robot_api.prepare_catch()
                time.sleep(.001)
        elif (kick == "Catch"):
            for i in range (0, 10):
                self.robot_api.catch(100)
                time.sleep(.001)
    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 draw(self, frame, model_positions, actions, regular_positions, fps,
             dState, aState, a_action, d_action, grabbers, our_color, our_side,
             key=None, preprocess=None):
        """
        Draw information onto the GUI given positions from the vision and post processing.

        NOTE: model_positions contains coordinates with y coordinate reversed!
        """
        # Get general information about the frame
        frame_height, frame_width, channels = frame.shape


        # turn stuff off and on, i linked launch class in a variable called launch


        robot = self.launch.controller.robot_api

        if key == ord('c'):
            Planner.planner.current_plan.initi(None)
            robot.enabled = not robot.enabled
            World.world.our_attacker.hball = False



        if key == ord('k'):
            self.consol_enabled = not self.consol_enabled

        if key == 27:
            robot.enabled = False

        consol.log('Press k to disable/enable consol', True)
        #draw console
        if(self.consol_enabled):
            consol.draw()



        # Draw the calibration gui
        self.calibration_gui.show(frame, key)
        # Draw dividors for the zones
        self.draw_zones(frame, frame_width, frame_height)

        their_color = list(TEAM_COLORS - set([our_color]))[0]

        key_color_pairs = zip(
            ['our_defender', 'their_defender', 'our_attacker', 'their_attacker'],
            [our_color, their_color]*2)

        self.draw_ball(frame, regular_positions['ball'])

        for key, color in key_color_pairs:
            self.draw_robot(frame, regular_positions[key], color)

        # Draw fps on the canvas
        if fps is not None:
            self.draw_text(frame, 'FPS: %.1f' % fps, 0, 10, BGR_COMMON['green'], 1)

        if preprocess is not None:
            # print preprocess
            preprocess['normalize'] = self.cast_binary(
                cv2.getTrackbarPos(self.NORMALIZE, self.VISION))
            preprocess['background_sub'] = self.cast_binary(
                cv2.getTrackbarPos(self.BG_SUB, self.VISION))

        if grabbers:
            self.draw_grabbers(frame, grabbers, frame_height)

        # Extend image downwards and draw states.
        blank = np.zeros_like(frame)[:200, :, :]
        frame_with_blank = np.vstack((frame, blank))
        self.draw_states(frame_with_blank, aState, (frame_width, frame_height))

        if model_positions and regular_positions:
            pass
            for key in ['our_defender', 'our_attacker', 'their_defender', 'their_attacker']: #'ball',
                if model_positions[key] and regular_positions[key]:
                    self.data_text(
                        frame_with_blank, (frame_width, frame_height), our_side, key,
                        model_positions[key].x, model_positions[key].y,
                        model_positions[key].angle, model_positions[key].velocity)
                    self.draw_velocity(
                        frame_with_blank, (frame_width, frame_height),
                        model_positions[key].x, model_positions[key].y,
                        model_positions[key].angle, model_positions[key].velocity)
        # Draw center of uncroppped frame (test code)
        # cv2.circle(frame_with_blank, (266,147), 1, BGR_COMMON['black'], 1)

        cv2.imshow(self.VISION, frame_with_blank)
    def mouse_call(self, event,x,y,flags,param):
        #global ix,iy,drawing,mode
        consol.log('param', param, 'Find HSV')

        if event == cv2.EVENT_LBUTTONDOWN:
            consol.log_time('Find HSV', 'mouse click')

            frame_hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)



            col = self.get_pixel_col(x, y)


            # fliped on purpose
            hsv = frame_hsv[y][x]
            consol.log('pixel color (hsv)', hsv, 'Find HSV')

            hsv_delta = np.array([15, 50, 50])


            hsv_min = hsv - hsv_delta
            hsv_max = hsv + hsv_delta

            consol.log('max (hsv)', hsv_max, 'Find HSV')
            consol.log('min (hsv)', hsv_min, 'Find HSV')


            self.set_slider(hsv_min, hsv_max)




            consol.log('pixel color', col, 'Find HSV')
            consol.log('pixel xy', [x, y], 'Find HSV')
            consol.log('frame size', [len(self.frame[0]), len(self.frame)], 'Find HSV')
    def nextCommand(self):
        maxs =70
        mins = 50

        #compute inputs

        distance = self.robot.get_euclidean_distance_to_point(self.world.ball.x, self.world.ball.y)
        dot = self.robot.get_dot_to_target(self.world.ball.x, self.world.ball.y)

        ball_catchable = distance < DISTANCE_ERROR and dot > 0.96 #input

        wall_ball = self.ball_distance_from_wall() < 50 #input

        near_center = self.robot.get_euclidean_distance_to_point(self.world.ball.x, self.midY) < 80 #input


        consol.log('state', self.state, 'GrabAll')
        #FSM body

        command = None



        if self.state == 'pick':
            '''
            if wall_ball:

                self.robot.catcher = "closed"

                self.nstate = 'goto center'
            '''
            if ball_catchable:
                command = self.catch()
            else:
                command = self.go_to_asym(self.world.ball.x, self.world.ball.y, forward=True, max_speed = maxs, min_speed=mins, mid_x=True)




        elif self.state == 'goto center':
            if near_center:
                self.nstate = 'pick wall'
                #command = CommandDict.catch()


            elif ball_catchable:
                command = self.catch()

            elif not wall_ball:
                self.nstate = 'pick'
            else:
                command = self.go_to_asym(self.world.ball.x, self.midY, forward=True, max_speed = 90, min_speed=mins)



        elif self.state == 'pick wall':
            consol.log_time('GrabAll', 'wall move')
            if not wall_ball:
                self.nstate = 'pick'
            elif ball_catchable:
                command = self.catch()

            else:
                command = self.go_to_asym(self.world.ball.x, self.world.ball.y, forward=True, max_speed = maxs, min_speed=mins, mid_y= True)



        '''
        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)




        self.state = self.nstate

        return command
Beispiel #32
0
    def mouse_call(self, event,x,y,flags,param):
        #global ix,iy,drawing,mode
        consol.log('param', param, 'Find YUV')

        if event == cv2.EVENT_LBUTTONDOWN:
            consol.log_time('Find YUV', 'mouse click')

            frame_yuv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2YUV)



            col = self.get_pixel_col(x, y)


            # fliped on purpose
            yuv = frame_yuv[y][x]
            consol.log('pixel color (yuv)', yuv, 'Find YUV')

            yuv_delta = np.array([15, 50, 50])


            yuv_min = yuv - yuv_delta
            yuv_max = yuv + yuv_delta

            consol.log('max (yuv)', yuv_max, 'Find YUV')
            consol.log('min (yuv)', yuv_min, 'Find YUV')


            self.set_slider(yuv_min, yuv_max)




            consol.log('pixel color', col, 'Find YUV')
            consol.log('pixel xy', [x, y], 'Find YUV')
            consol.log('frame size', [len(self.frame[0]), len(self.frame)], 'Find YUV')
    def arrived(self, x, y):
        consol.log('distance to destination', self.robot.get_euclidean_distance_to_point(x, y), 'ShootAll')

        return self.robot.get_euclidean_distance_to_point(x, y) < 50