示例#1
0
文件: remain.py 项目: finsqm/IAR
    def __init__(self):
        STATES = ['BEGIN', 'EXPLORE', 'AVOID', 'FOLLOW_WALL', 'STOP', 'GETTING FOOD','GOING HOME']
        self.EXPLORE_SPEED = 6
        self.TURN_SPEED = 3
        self.WALL_SPEED = 3
        # If we are closer than 500 then there is very little distance for the robot to try to get closer 
        # while there is a lot of distance where it is "too close"
        # 700 might be a good value
        self.IDEAL_IR = 700
        self.K1 = 0.0001
        self.K2 = 0.019

        self.state = 'EXPLORING'

        self.connection = open_connection()
        set_counts(self.connection, 0, 0)

        # INTERNAL STATE VARS
        self.error = 0
        self.v_left = self.WALL_SPEED
        self.v_right = self.WALL_SPEED
        
        self.old_counters = [0,0]
        self.ir_sensors = []
        self.distances = []
        # wall is either 'left', 'right' or None
        self.wall = None
        self.x = 0
        self.y = 0
        # current angle
        self.theta = 0
        # angle to home
        self.phi = np.pi
        self.graph = OdometryPlot(use_map = True)
        self.graph.update(self.x, self.y)
        # Time elapsed since journey started
        self.clock = 0
        # Belief that robot is pointing home
        self.pointing_home = False

        self.ambient = read_ambient(self.connection)
        self.graph.fig.canvas.mpl_connect('key_press_event', self.key_pressed)

        self.has_food = False
        self.food_at = None
        self.home = Point(x=0, y=0)
        self.target = self.home
        self.turning_home = False
        self.avoiding = False

        self.avoided_counter = 0
示例#2
0
文件: remain.py 项目: finsqm/IAR
    def update_vars(self):
        try:
            ir_sensors, counters, self.ambient = self.get_readings()
        except ValueError as e:
            logger.debug(e)
            return self.update_vars()
        
        self.x, self.y, self.theta, self.phi = OdometryPlot.calc_odometry(
                counters, self.old_counters, self.x, self.y, self.theta,self.target)   

        self.old_counters = counters

        self.ir_sensors = ir_sensors

        self.distances = self.get_distances(ir_sensors)
示例#3
0
文件: remain.py 项目: finsqm/IAR
class Kheperam(object):

    def __init__(self):
        STATES = ['BEGIN', 'EXPLORE', 'AVOID', 'FOLLOW_WALL', 'STOP', 'GETTING FOOD','GOING HOME']
        self.EXPLORE_SPEED = 6
        self.TURN_SPEED = 3
        self.WALL_SPEED = 3
        # If we are closer than 500 then there is very little distance for the robot to try to get closer 
        # while there is a lot of distance where it is "too close"
        # 700 might be a good value
        self.IDEAL_IR = 700
        self.K1 = 0.0001
        self.K2 = 0.019

        self.state = 'EXPLORING'

        self.connection = open_connection()
        set_counts(self.connection, 0, 0)

        # INTERNAL STATE VARS
        self.error = 0
        self.v_left = self.WALL_SPEED
        self.v_right = self.WALL_SPEED
        
        self.old_counters = [0,0]
        self.ir_sensors = []
        self.distances = []
        # wall is either 'left', 'right' or None
        self.wall = None
        self.x = 0
        self.y = 0
        # current angle
        self.theta = 0
        # angle to home
        self.phi = np.pi
        self.graph = OdometryPlot(use_map = True)
        self.graph.update(self.x, self.y)
        # Time elapsed since journey started
        self.clock = 0
        # Belief that robot is pointing home
        self.pointing_home = False

        self.ambient = read_ambient(self.connection)
        self.graph.fig.canvas.mpl_connect('key_press_event', self.key_pressed)

        self.has_food = False
        self.food_at = None
        self.home = Point(x=0, y=0)
        self.target = self.home
        self.turning_home = False
        self.avoiding = False

        self.avoided_counter = 0

    def key_pressed(self, event):
        self.state = 'GOING HOME'
        self.has_food = True
        self.food_at = Point(x=self.x, y=self.y)
        self.target = self.home
        self.blink()

    def run(self, test=0):
        """
        Main control loop
        """
        START_TIME = time.time()
        
        if test == 0:
            while True:
                CURRENT_TIME = time.time()
                self.clock = CURRENT_TIME - START_TIME
                try:
                    self.update()
                except KeyboardInterrupt:
                    stop(self.connection)
                    break
                except Exception as e:
                    stop(self.connection)
                    raise e
        else:
            # Use for testing
            #while True:
            #    try:
            #        self.update_vars()
            #        self.point_home()
            #    except KeyboardInterrupt:
            #        stop(self.connection)
            #        #sys.exit()
            #        break
            ir = self.update_vars()
            
    def explore(self):
        go(self.connection, self.EXPLORE_SPEED)

    def follow_wall(self, ir_sensors):
        if self.state != 'FOLLOW WALL':
            self.v_left = self.WALL_SPEED
            self.v_right = self.WALL_SPEED
            if self.state == 'EXPLORING':
                self.state = 'FOLLOW WALL' 
        if ir_sensors[0] > ir_sensors[5]:
                if not self.wall:
                    self.wall = 'left'
                sensor = ir_sensors[0]
        else:
                if not self.wall:
                    self.wall = 'right'
                sensor = ir_sensors[5]

        self.old_error = self.error
        self.error = self.IDEAL_IR - sensor
        delta_error = self.error - self.old_error

        # Negative error - too close, drive away
        # Postive error  - too far away, drive closer
        delta_v = (self.K1 * self.error) + (self.K2 * delta_error)
        
        if self.wall == 'left':
            self.v_right = self.v_right + delta_v
        else:
            self.v_left = self.v_left + delta_v


        turn(self.connection,int(self.v_left),self.v_right)


        if sensor < 10:
        # Lost wall, go back to exploring
            self.v_left = self.WALL_SPEED
            self.wall = None
            self.state = 'EXPLORING'
 

    def avoid(self, ir_sensors, distances):
        """call this when an obstacle is detected. It will set the current state to avoiding.
        once the correct sensor readings are found it will leave the state into the generic explore state """
        if not self.avoiding:
            self.avoiding = True
            if sum(distances[0:2]) < sum(distances[3:5]): # We could go back to using a more clever choice here
                # Turn right
                turn(self.connection,self.TURN_SPEED,-self.TURN_SPEED)
            else:
                # Turn left
                turn(self.connection,-self.TURN_SPEED,self.TURN_SPEED)
        if self.logic_or([x > 50  for x in distances[1:4]]):
            self.avoiding = False
            self.avoided_counter = 5

    def update_vars(self):
        try:
            ir_sensors, counters, self.ambient = self.get_readings()
        except ValueError as e:
            logger.debug(e)
            return self.update_vars()
        
        self.x, self.y, self.theta, self.phi = OdometryPlot.calc_odometry(
                counters, self.old_counters, self.x, self.y, self.theta,self.target)   

        self.old_counters = counters

        self.ir_sensors = ir_sensors

        self.distances = self.get_distances(ir_sensors)

    def update(self):
        
        self.update_vars()
        ir_sensors = self.ir_sensors
        distances = self.distances

        self.graph.update(self.x, self.y)

        #if self.avoided_counter > 0:
        #    self.avoided_counter -= 1
        if (self.detect_ahead() or self.avoiding) and not self.turning_home:
            self.avoid(ir_sensors, distances)
            print 'AVOIDING'
        elif self.has_food or self.state == 'GETTING FOOD':
            self.return_home(ir_sensors,distances)
        #elif ir_sensors[0] > 50 or ir_sensors[5] > 50 or self.state == 'FOLLOW WALL':
        elif self.logic_or([x < 100 for x in distances]):
            self.follow_wall(ir_sensors)
        else:
            self.state = 'EXPLORING'
            self.explore()

        logger.info(self.state)
        logger.debug(self.wall)

    def logic_or(self,xs):

        for x in xs:
            if x:
                return True

        return False

    def detect_ahead(self):

        return self.logic_or([x < 50 for x in self.distances[1:4]])

    def point_home(self):
        """
        Turn on the spot until pointing at home.
        """

        self.turning_home = True
        
        error = (self.theta - self.phi) % (2*math.pi)

        if error < (math.pi/16):
            stop(self.connection)
            self.turning_home = False
            raise DoneTurning('We have turned to within the error')

        if error > (math.pi / 2) and error < (3*math.pi/2):
            turn_v = 3
        else:
            turn_v = 2 

        if error > math.pi:
            # turn left
            turn(self.connection,-turn_v,turn_v)
        else:
            # turn right
            turn(self.connection,turn_v,-turn_v)


    def return_home(self, ir_sensors, distances):

        dir_target = 'left' if self.phi > math.pi else 'right'

        #if (self.phi > 3 * math.pi / 4.0 and self.phi < (5*math.pi) / 4.0) or self.phi < math.pi / 4.0 or self.phi > 7 * math.pi / 4.0 :
        #    dir_target = None

        print 'dir_target: ' + str(dir_target)

        if (dir_target == 'left' and (distances[0] < 100 or distances[1] < 100) ) or (dir_target == 'right' and (distances[4] < 100 or distances[5] < 100)):
            self.follow_wall(ir_sensors)
            print 'FOLLOWED WALL WITH FOOD'
            return

        self.wall = None

        distance_error = abs(self.target.x - self.x) + abs(self.target.y - self.y)
        if distance_error < 5 and self.has_food:
            self.blink()
            self.has_food = False
            self.target = self.food_at
            self.state = 'GETTING FOOD'
            return
        elif distance_error < 5:
            self.state = 'EXPLORING'
            return
        try:
            self.point_home()
        except DoneTurning:
            go(self.connection, self.EXPLORE_SPEED)  

    def get_readings(self):

        ir_sensors = read_IR(self.connection) 
        counters = np.array([float(i) for i in read_counts(self.connection)])
        amb = read_ambient(self.connection)
        return ir_sensors, counters, amb

    def stop(self):
        stop(self.connection)
        set_led(self.connection, 0, 0)
        set_led(self.connection, 1, 0)

    def get_distances(self, ir):
        """
        Calculate the distance between robot and object directly ahead.
        Return -1 if no object detected.
        Return 0 if ir sensors at full.

        sensor = 136702 * exp(-0.47852 * distance)
        """
        ln_A = 11.83
        k = -0.48

        distances = (np.log(ir) - ln_A) / k

        for i,sensor in enumerate(ir):
            if sensor > 1000:
                distances[i] = 0
            elif sensor < 10:
                # far away (no reading)
                distances[i] = 1000

        return distances

    def spiral(self):
        logger.debug(self.spiral_count)
        if self.spiral_count < 300:
            turn(self.connection, 10, 2)
            self.spiral_count += 1
        elif self.spiral_count < 600:
            turn(self.connection, 2, 10)
            self.spiral_count += 1 
        else:
            stop(self.connection)
            self.spiral_count = 0

    def blink(self):
        self.stop()
        sleep = 0.25

        set_led(self.connection, 0, 1)
        time.sleep(sleep)
        for i in range(0,2):
            set_led(self.connection, 1, 2)
            set_led(self.connection, 0, 2)
            time.sleep(sleep)

        set_led(self.connection, 1, 0)
        set_led(self.connection, 0, 0)