Esempio n. 1
0
class Navigation():
    def __init__(self):

        # this is only changed once the ball is kicked
        self.switch_goal = False
        self.prev_state = state.none
        self.prev_ball = -1
        self.prev_goal = -1

        # CONDITIONS
        self.ball_in_dribbler = False
        self.ball_found = False
        self.goal_found = False
        self.goal_aligned = False

        # STATES
        self.state = state.none
        self.search = search.none

        # OBJECTS
        self.drive_sys = Drive()
        self.dribbler = Dribbler()
        self.field = Field()
        self.kicker = Kicker()

        # MOTOR pwm
        self.left_motor = 0
        self.right_motor = 0

        # range/headings
        self.obs_range = [None, None, None]
        self.obs_heading = [None, None, None]
        self.ball_range = None
        self.ball_heading = None
        self.goal_range = None
        self.goal_heading = None

    ####################
    # GENERIC FUNCTIONS
    #

    # input: left and right motor pwms
    # output: pwm value that gets larger the sharper the robot is turning
    # WORK IN PROGRESS
    def choose_drib_speed(self):

        # find ratio: -1 is spinning on spot, 1 is going straight
        ratio = 0
        if (self.left_motor == 0 or self.right_motor == 0):
            ratio = 0

        else:
            ratio = min(self.left_motor, self.right_motor) / max(
                self.left_motor, self.right_motor)

        # convert ratio to: 0 is straight, 2 is spinning on spot
        ratio = -ratio + 1

        # if motor is going forward (ratio = 0), dribbler is at 50 pwm
        # if motor is doing a full spin (ratio = 2), dribbler is at 90 pwm
        speed = 60 + (ratio * 100)

        return check_speed(speed)

    # input: object heading/range
    # output: drive and center towards object, with speed slowing down based on proximity
    # example: object at 100cm, -16deg:
    #          left_motor: 50 + 100/8 - -16 = 78.5
    #          right_motor: 50 + 100/8 + -16 = 46.5
    def goto_object_simple(self, speed, ran, head):

        self.right_motor = check_speed((MIN_SPEED * speed) + (ran / 5) -
                                       (head))
        self.left_motor = check_speed((MIN_SPEED * speed) + (ran / 5) + (head))

        self.drive_sys.drive(self.left_motor, self.right_motor)

    # input: object target
    # output: drive towards object and avoid obstacles
    prev_heading = -999

    def goto_object(self, speed=1, object='ball'):
        '''
        def smooth(heading):
            if self.prev_heading == -999:
                self.prev_heading = heading
                
            else:
                if abs(self.prev_heading - heading) > 5:
                    self.prev_heading -= np.sign(self.prev_heading)*5
                else:
                    self.prev_heading = heading
                
            return self.prev_heading'''

        if (self.obs_range[0] is not None
                and self.obs_range[0] <= self.ball_range):
            self.field.change_state(self.state)
            target_heading = self.field.update(
                self.obs_range, self.obs_heading, self.ball_range,
                self.ball_heading, self.goal_range, self.goal_heading)

            if object == 'ball':
                target_range = self.ball_range

            elif object == 'goal':
                target_range = self.goal_range

            else:
                return

        else:
            if object == 'ball':
                target_range = self.ball_range
                target_heading = self.ball_heading

            elif object == 'goal':
                target_range = self.goal_range
                target_heading = self.goal_heading

        self.goto_object_simple(speed, target_range, target_heading)

    def stop(self):
        self.drive_sys.stop()
        self.dribbler.stop()
        GPIO.cleanup()

    ####################
    # UPDATE FUNCTION
    #

    # input: vision feed values
    # output: true if goal needs to be switched
    def update(self, obs, ball, goal):

        # return true if goal needs to be switched, so vision can be changed from main function
        if (self.switch_goal):
            self.switch_goal = False
            return True

        # VISION INFO
        self.obs_range = [
            cap_ran(obs[0][0]),
            cap_ran(obs[1][0]),
            cap_ran(obs[2][0])
        ]
        self.obs_heading = [obs[0][1], obs[1][1], obs[2][1]]
        self.ball_range = cap_ran(ball[0])
        self.ball_heading = ball[1]
        self.goal_range = cap_ran(goal[0])
        self.goal_heading = goal[1]

        # ACT BASED ON VISION INFO
        self.decide_conditions()
        self.decide_state()
        self.decide_action()

        return False

    ##################
    # DECIDE FUNCTIONS
    # decides what to do based on the data from update()

    # input: vision data
    # output: condition bool variables
    dribbler_counter = 0

    def decide_conditions(self):

        # if ball is seen
        if (self.ball_range is not None):
            self.ball_found = True
            self.prev_ball = np.sign(self.ball_heading)

            # if ball is within dribbler range/angle
            if (self.ball_range < BALL_IN_DRIBBLER_DIST and within_angle(
                    self.ball_heading, BALL_IN_DRIBBLER_ANGLE)):
                self.ball_in_dribbler = True
            else:
                self.ball_in_dribbler = False

        else:
            self.ball_found = False

        # if goal is seen
        if (self.goal_range is not None):
            self.goal_found = True
            self.prev_goal = np.sign(self.goal_heading)

            #if goal is within shooting range/angle
            if (self.goal_range < GOAL_DIST
                    and within_angle(self.goal_heading, GOAL_ANGLE)):
                self.goal_aligned = True
            else:
                self.goal_aligned = False
        else:
            self.goal_found = False

    def change_state(self, state):
        self.state = state

    # input: condition variables
    # output: state value
    def decide_state(self):

        if (not self.ball_found and not self.ball_in_dribbler):
            self.state = state.ball_search

        elif (self.ball_found and not self.ball_in_dribbler):
            self.state = state.go_to_ball

        elif (self.ball_found and self.ball_in_dribbler
              and not self.goal_found):
            self.state = state.goal_search

            if (self.prev_state != self.state):
                self.drive_sys.stop()
                time.sleep(0.25)

        elif (self.ball_found and self.ball_in_dribbler and self.goal_found
              and not self.goal_aligned):
            self.state = state.go_to_goal

        elif (self.ball_found and self.ball_in_dribbler and self.goal_found
              and self.goal_aligned):
            self.state = state.shoot_ball

        else:
            self.state = state.ball_search

        self.prev_state = self.state

        # print(self.state)

    # input: state/search value
    # output: appropriate function
    def decide_action(self):

        if (self.state == state.ball_search):
            self.ball_search()

        else:

            self.search_lock = False
            if (self.state == state.go_to_ball):
                self.go_to_ball()

            if (self.state == state.goal_search):
                self.goal_search()

            if (self.state == state.go_to_goal):
                self.go_to_goal()

            if (self.state == state.shoot_ball):
                self.shoot_ball()

        # if state == none
            else:
                pass

    ##################
    # ACTION FUNCTIONS
    #

    # input: search value
    # output: appropriate ball searching method
    search_lock = False

    def ball_search(self):

        if (not self.search_lock):
            if (self.search == search.none):
                self.search = search.spin

            if (self.search == search.spin):
                # will not be true until until spin is complete
                #print("searching for ball")
                if (self.drive_sys.spin(speed=30,
                                        direction=self.prev_ball,
                                        radius=0,
                                        cycles=10)):
                    self.search = search.go_to_obstacle

        if (self.search == search.go_to_obstacle):
            self.search_lock = True
            if (self.obs_range[0] is None):
                #print("searching for obstacle")
                self.drive_sys.spin(speed=30,
                                    direction=self.prev_ball,
                                    radius=0,
                                    cycles=None)
            else:
                if (self.obs_range[0] > OBSTACLE_DIST):
                    #print("goi8ng to obstacle")
                    self.goto_object_simple(1, self.obs_range[0],
                                            self.obs_heading[0])
                else:

                    self.search = search.circle_obstacle

        if (self.search == search.circle_obstacle):
            #print("circling obstacle")
            # will not be true until circle is complete
            if (self.circle_obstacle(adjust=100)):
                #print("CIRCLE OBSTACLE COMPLETE")
                # if after all that the ball is not found, restart the cycle again
                self.search = search.none
                self.search_lock = False

    orientate = True
    spin_out_timer = 0
    right_wheel = 40
    left_wheel = 30
    prev_state = 0
    adjust_counter = 0

    # input: largest obstacle range/heading
    # output: circling obstacle
    def circle_obstacle(self, adjust=10):

        # count how many times the robot has to adjust based on if it saw the object
        if (self.obs_range[0] is None and self.prev_state is not None):
            self.adjust_counter += 1

        self.prev_state = self.obs_range[0]

        # if it has reached the limit, reset everything for the next time the function is called
        if (self.adjust_counter >= adjust):
            self.orientate = True
            self.spin_out_timer = 0
            self.right_wheel = 40
            self.left_wheel = 30
            self.prev_state = 0
            self.adjust_counter = 0
            self.drive_sys.stop()
            return True

        # if the robot needs to oreintate itself
        if (self.orientate and self.obs_range[0] is not None):
            self.drive_sys.drive(-30, 30)
            self.spin_out_timer = 0

        # else if it needs to circle
        else:
            self.orientate = False

            if self.obs_range[0] is not None:
                self.spin_out_timer = 0
                self.right_wheel = 40
                self.left_wheel = 30

            if (self.obs_range[0] is None):
                self.spin_out_timer += 1
                self.right_wheel += 0.4
                self.left_wheel -= 0.4

            # spin in
            if (self.spin_out_timer >= 10):
                self.drive_sys.drive(self.right_wheel, self.left_wheel)

            # spin out
            if (self.spin_out_timer < 10):
                self.drive_sys.drive(-40, 40)

        return False

    # input: ball range/heading, ball_in_dribbler variable
    # output: driving to ball, turning dribbler on if in range
    #         WILL DRIVE STRAIGHT TOWARDS BALL, IGNORING OPBSTACLES
    def go_to_ball(self):
        # self.goto_object_simple(1, self.ball_range, self.ball_heading)
        # REPLACE ^ WITH:
        self.goto_object(speed=1, object='ball')

        if (self.ball_range < 20):
            self.dribbler.start(95)
        else:
            self.dribbler.stop()

    # input:
    # output: turning in a circle without losing ball
    def goal_search(self):
        self.dribbler.start(90)
        self.drive_sys.spin(speed=20,
                            direction=self.prev_goal,
                            radius=10,
                            cycles=None)

    def go_to_goal(self):
        self.dribbler.start(95)
        self.goto_object_simple(0.65, self.goal_range, self.goal_heading)
        # REPLACE ^ WITH:
        # self.goto_object(speed=0.65, object='goal')

    def shoot_ball(self):

        self.dribbler.stop()
        self.kicker.kick()
        self.drive_sys.stop()
        self.switch_goal = True
Esempio n. 2
0
class Navigation:

    def __init__(self):

        self.circle_obstacle = False
        self.goto_ball = False
        self.ball_captured = False
        self.goto_obstacle = False
        self.goto_goal = False
        self.ball_found = False
        self.goal_found = False
        self.goal_clear = False
        self.spin = False
        self.search_obs = False
        self.timer = Timer()

        self.target_side = -1
        self.ball_side = -1
        self.goal_side = -1
        self.wall_side = -1
        self.obs_side = -1
        self.speed_mod = 1

        self.no_target_count = 0
        self.circle_count = 0

        self.obs_r = None
        self.obs_h = None
        self.ball_r = None
        self.ball_h = None
        self.goal_r = None
        self.goal_h = None
        self.wall_r = None
        self.wall_h = None
        self.target_r = None
        self.target_h = None
        self.target_found = False

        self.drive_sys = Drive()
        self.dribbler = Dribbler()
        self.kicker = Kicker()

    obs_buffer = [None, None]
    obs_buf_count = 0
    def update(self, obs, ball, goal, wall):

        if obs[0] is None:
            self.obs_buf_count += 1
            
            if(self.obs_buf_count > 2):
                self.obs_buffer = [None, None]
                self.obs_r = None
                self.obs_h = None
                
            else:
                self.obs_r = cap_range(self.obs_buffer[0])
                self.obs_h = self.obs_buffer[1]
            
        else:
            self.obs_buf_count = 0
            self.obs_buffer = obs
        
        self.ball_r = cap_range(ball[0])
        self.ball_h = ball[1]
        self.goal_r = cap_range(goal[0])
        self.goal_h = goal[1]
        self.wall_r = cap_range(wall[0])
        self.wall_h = wall[1] 

        self.conditions()
        self.state()
        ml, mr, drib, kick = self.decide()

        self.drive_sys.drive(ml, mr)
        self.dribbler.start(drib)
        if kick:
            self.kicker.kick()
            self.goal_clear = False

        return kick

    def conditions(self):

        if self.ball_r is not None:
            self.ball_side = np.sign(self.ball_h)
            self.ball_found = True
            self.no_target_count = 0

            if(within_angle(self.ball_h, MAX_BALL_ANGLE) and self.ball_r < MIN_BALL_DIST):
                self.ball_captured = True
            else:
                self.ball_captured = False

        else:
            self.ball_found = False

        if self.goal_r is not None:
            self.goal_side = np.sign(self.goal_h)
            self.goal_found = True

            if(within_angle(self.goal_h, MAX_GOAL_ANGLE) and self.goal_r < MIN_GOAL_DIST and self.target_clear()):
                self.goal_clear = True
            else:
                self.goal_clear = False

        else:
            self.goal_found = False

        if self.ball_captured:
            self.target_r = self.goal_r
            self.target_h = self.goal_h
            self.target_side = self.goal_side
            if self.goal_found:
                self.target_found = True
            else:
                self.target_found = False

        else:
            self.target_r = self.ball_r
            self.target_h = self.ball_h
            self.target_side = self.ball_side
            if self.ball_found:
                self.target_found = True
            else:
                self.target_found = False
                
        if self.wall_r is not None:
            self.wall_side = np.sign(self.wall_h)
        
        if self.obs_h is not None:
            self.obs_side = np.sign(self.obs_h)            
            
    counter = 0
    def state(self):

        if not self.target_found:
            
            print(self.counter)
            
            if self.counter < 50 * self.speed_mod:
                print("looking for ball")
                self.counter += 1
                self.spin = True

            elif self.counter >= 50 * self.speed_mod:
                self.spin = False
                # spin until first obs found
            
                if not self.search_obs:
                    print(self.obs_r)
                    
                    if(self.obs_r is None):
                        self.spin = True
                        print("looking for obs")
                    else:
                        self.spin = False
                        if(self.obs_r > MIN_OBS_DIST):
                            print("going to obs")
                            self.goto_obstacle = True
                        else:
                            print("spinning to circle obs")
                            self.goto_obstacle = False
                            self.spin = True
                            self.target_side = -self.wall_side
                            self.search_obs = True  
        
                else:                    
                                    
                    if(self.counter >= 100 * self.speed_mod):
                        print("circling obs")
                        self.spin = False
                        self.counter += 1
                        self.circle_obstacle = True

                        if(self.counter >= 150 * self.speed_mod):
                            print("circle obs done, spin out")
                            self.counter += 1
                            self.circle_obstacle = False
                            self.spin = True

                            if(self.counter >= 200 * self.speed_mod):
                                self.counter = 0
                                print("done")
                                self.spin = False
                                self.circle_obstacle = False
                                self.search_obs = False                            

            # target seen
            else:
                print("going to target")
                self.counter = 0

            # if obstacle is too close
            if self.obs_r is not None and self.obs_r < 25:
                self.spin = True
                self.target_side = -self.obs_side


    def decide(self):

        ml = 0
        mr = 0
        drib = 0
        kick = False

        # most complex situations at the top, least complex at the bottom
        if self.circle_obstacle and self.target_found:
            ml1, mr1 = turn(self.target_side, 45, 90)
            ml2, mr2 = self.goto_target()

            if ml1 > mr1:
                ml, mr = min(ml2, ml1), max(mr2, mr1)
            else:
                ml, mr = max(ml2, ml1), min(mr2, mr1)

        elif self.target_found:
            ml, mr = self.goto_target()

        elif self.circle_obstacle:
            ml, mr = turn(self.target_side, 45, 90)

        elif self.goto_obstacle:
            ml, mr = rb_to_pwm(self.obs_r, self.obs_h)

        elif self.spin:
            ml, mr = spin(self.target_side, 40)

        # dribbler adn speed adjusts
        if self.ball_r is not None and self.ball_r < 25:
            drib = 80

            if self.ball_captured:
                if self.spin:
                    ml *= 0.5
                    mr *= 0.5
                    self.speed_mod = 2
                    drib = 100

                if self.circle_obstacle:
                    ml *= 0.65
                    mr *= 0.65
                    self.speed_mod = 1.5
                    drib = 85

                else:
                    ml *= 1
                    mr *= 1
                    self.speed_mod = 1
                    drib = 75

        else:
            drib = 0
            self.speed_mod = 1

        if self.goal_clear:
            drib = 0
            kick = True
            ml = 0
            mr = 0

        return ml, mr, drib, kick

    '''
    def target_clear_many(self):

        def clear(obs_h, obs_r, thresh):
            if self.target_r - 10 < obs_r:
                return True
            else:
                if (obs_h - thresh <= self.target_h <= obs_h + thresh):
                    return False
                return True

        if self.target_h is None:
            return False

        for i in range(3):
            if self.obs_h[i] is None:
                return True
            if not clear(self.obs_h[i], self.obs_r[i], 16):
                return False
            return True'''

    def target_clear(self):
        if self.obs_r is None:
            return True

        if self.target_r is None:
            return False

        if self.target_r - 20 < self.obs_r:
            return True
        else:
            if (self.obs_h - 15 <= self.target_h <= self.obs_h + 15):
                return False
            return True

    def target_between(self):

        closest_left_h = -60
        closest_left_r = 0
        closest_right_h = 60
        closest_right_r = 0
        ret = [None, None], [None, None]

        for i in range(3):
            if self.obs_h[i] is None:
                continue

            if closest_left_h < self.obs_h[i] < self.target_h:
                closest_left_h = self.obs_h[i]
                closest_left_r = self.obs_r[i]

            elif closest_right_h > self.obs_h[i] > self.target_h:
                closest_right_h = self.obs_h[i]
                closest_right_r = self.obs_r[i]

        if closest_left_h != -60:
            ret[0][:] = [closest_left_r, closest_left_h]

        if closest_right_h != 60:
            ret[1][:] = [closest_right_r, closest_right_h]

        return ret

    '''
    def goto_target(self):
          
        left_obs, right_obs = self.target_between()

        if left_obs[0] is None and right_obs[0] is None:
            #print("clear")
            return self.goto_target_simple()
            

        if left_obs[0] is not None and right_obs[0] is not None:
            
            #print("between")
            # ball inbetween left and right obs
            gap = abs(np.sqrt(left_obs[0]**2 + right_obs[0]**2 - 2*left_obs[0]*right_obs[0]*np.cos(left_obs[1] - right_obs[1])))
            dist_dif = - left_obs[0] + right_obs[0]

            m1l, m1r = self.goto_target_simple()
            m2l, m2r = 0, 0

            if gap > 30:
                # right obs below left obs
                if dist_dif <= 0:
                    m2l, m2r = rb_to_pwm(left_obs[0], left_obs[1] * (10 + abs(dist_dif)) / 10)
                else:
                    m2l, m2r = rb_to_pwm(right_obs[0], right_obs[1] * (10 + abs(dist_dif)) / 10)
            else:
                if dist_dif <= 0:
                    m2l, m2r = rb_to_pwm(right_obs[0], right_obs[1] + 16)
                else:
                    m2l, m2r = rb_to_pwm(left_obs[0], left_obs[1] - 16)

            ml = (m1l - m2l) / 2
            mr = (m1r - m2r) / 2

            return ml, mr

        if left_obs[0] is None:
            # ball to the right of right obs
            #print("right")
            heading = max((right_obs[1] - (251 - right_obs[0])), self.target_h)
            return rb_to_pwm(self.target_r, heading)
            

        if right_obs[0] is None:
            print("left")
            # ball to the left of left object
            heading = max(left_obs[1] - (251 - left_obs[0]), self.target_h)
            return rb_to_pwm(self.target_r, heading)
            
        else:
            print("uh oh")'''
            
    def goto_target(self):
        
        
        def avoid(heading, dist):
            left = heading - ((251 - dist) * (175 / 250))
            right = heading + ((251 - dist) * (175 / 250))
            return left, right
        
        if self.obs_r is None or self.obs_r - 10 > self.target_r:
            return self.goto_target_simple()
        
        else:
            left_avoid, right_avoid = avoid(self.obs_h, self.obs_r)
            
            if self.target_h <= left_avoid or self.target_h >= right_avoid:
                return rb_to_pwm(self.target_r, self.target_h)
            
            elif left_avoid < self.target_h <= self.obs_h:
                return rb_to_pwm(self.obs_r, left_avoid)
            
            elif right_avoid > self.target_h > self.obs_h:
                return rb_to_pwm(self.obs_r, right_avoid)
            
            else:
                return 0, 0                
            


    def goto_target_simple(self):
        return rb_to_pwm(self.target_r, self.target_h)
    
    def stop(self):
        self.drive_sys.stop()
        self.dribbler.stop()
        GPIO.cleanup()