示例#1
0
 def __init__(self, obstacle_points=set(), clear_rectangles=set()):
   self.logger = Logger("WallMap")
   self.obstacle_points = obstacle_points
   self.clear_rectangles = clear_rectangles
   self.walls = set()
   self.x_min = -10
   self.x_max = 10
   self.y_min = -10
   self.y_max = 10
   self.count_since_last_refresh = 0
示例#2
0
 def __init__(self, points):
   if type(points) is list:
     self.points = points
   else:
     self.points = [points]
   
   self.logger = Logger("Wall")
示例#3
0
    def __init__(self, port="/dev/tty.HC-05-DevB", baud=9600, enabled=True):
        self.port = port
        self.baud = baud
        self.bluetooth = None
        self.enabled = enabled
        self.connected = False
        self.time_packet = [time.time()]
        self.data_stream = ""

        self.logger = Logger("Communicator")
        if self.enabled:
            try:
                self.initiate_bluetooth()
            except:
                self.logger.log("Communicator: Bluetooth Not Connected")
                self.logger.log(sys.exc_info())
示例#4
0
 def __init__(self, image, x=0.0, y=0.0, angle=0):
     pygame.sprite.Sprite.__init__(self)
     self.image = image.convert_alpha()
     self.size = self.image.get_size()
     self.left_encoder_counts = 0
     self.right_encoder_counts = 0
     self.location = (x, y)
     #^separate things above here into just the draw_robot method
     self.angle = angle
     self.dataPackets = [InfoPacket(angle=90),
                         InfoPacket(angle=90)
                         ]  #angles here should be changed to 0?
     self.communicator = Communicator(enabled=False)
     self.communicator.initiate_bluetooth()
     self.state = State.stop
     self.communicator.transmit_info(self.state)
     self.logger = Logger("Robot")
     self.max_dist = 150
     self.last_index = -1
     self.rectifier = None
     self.last_state_switch_time = int(round(time.time() * 1000))
示例#5
0
class Robot:
    def __init__(self, image, x=0.0, y=0.0, angle=0):
        pygame.sprite.Sprite.__init__(self)
        self.image = image.convert_alpha()
        self.size = self.image.get_size()
        self.left_encoder_counts = 0
        self.right_encoder_counts = 0
        self.location = (x, y)
        #^separate things above here into just the draw_robot method
        self.angle = angle
        self.dataPackets = [InfoPacket(angle=90),
                            InfoPacket(angle=90)
                            ]  #angles here should be changed to 0?
        self.communicator = Communicator(enabled=False)
        self.communicator.initiate_bluetooth()
        self.state = State.stop
        self.communicator.transmit_info(self.state)
        self.logger = Logger("Robot")
        self.max_dist = 150
        self.last_index = -1
        self.rectifier = None
        self.last_state_switch_time = int(round(time.time() * 1000))

    def add_data(self):
        new_packet = self.communicator.recieve_info(self.state)
        if new_packet != None:
            self.dataPackets.append(new_packet)
            self._update_location()
            self.logger.log(self.dataPackets[-1])
            self.logger.log('adding data to robot')
            return self.generate_points()  #returns two points

    def quitProgram(self):
        self.communicator.transmit_info(State.stop)
        self.communicator.deactivate_bluetooth()

    def _update_location(self):
        if self.rectifier is None and len(
                self.dataPackets
        ) >= 3:  # when we get the first new data, initialize
            last_packet = self.dataPackets[-1]
            self.rectifier = Rectifier(
                start_angle=last_packet.rotation,
                start_l_encoder=last_packet.left_encoder_counts,
                start_r_encoder=last_packet.right_encoder_counts)

        self.logger.log('rectifier logs:')
        last_packet = self.dataPackets[-1]
        self.logger.log(f'last packet: {last_packet}')
        last_packet.rotation = self.rectifier.offset_angle(
            last_packet.rotation)
        last_packet.left_encoder_counts = self.rectifier.offset_l_encoder(
            last_packet.left_encoder_counts)
        last_packet.right_encoder_counts = self.rectifier.offset_r_encoder(
            last_packet.right_encoder_counts)

        self.logger.log(last_packet.rotation, last_packet.left_encoder_counts,
                        last_packet.right_encoder_counts)

        delta_x, delta_y, angle = self._calculate_delta_location_change()
        self.logger.log("delta_x, delta_y, delta_angle:", delta_x, delta_y,
                        angle)
        self.location = (self.location[0] + delta_x,
                         self.location[1] + delta_y)
        self.logger.log('new position:', self.location[0], self.location[1],
                        self.angle)

    def _calculate_delta_location_change(self):
        differenceR = self.dataPackets[
            -1].right_encoder_counts - self.dataPackets[
                -2].right_encoder_counts  #Find the difference between last transmittion
        differenceL = self.dataPackets[
            -1].left_encoder_counts - self.dataPackets[-2].left_encoder_counts
        difference_average = (differenceL + differenceR) / 2
        if self.dataPackets[-1].state == State.turn_left or self.dataPackets[
                -1].state == State.turn_right:  #Or the difference between last angle if rotation
            delta_x = 0
            delta_y = 0
            angle = self.dataPackets[-1].rotation
        elif self.dataPackets[-1].state == State.forward or self.dataPackets[
                -1].state == State.reverse:
            delta_r_cm = difference_average * WheelInfo.WHEEL_CM_PER_COUNT
            angle = self.dataPackets[-1].rotation

            #if it's going reverse, calculate it like it's going forward backward
            if self.dataPackets[-1].state == State.reverse:
                angle + 180

            self.logger.log("angle: " + str(angle))
            angle_radians = math.radians(angle)
            self.logger.log("angle_radians: " + str(angle_radians))

            #if we adjusted the angle value bc we're going backward, undo that when we return delta_angle
            if self.dataPackets[-1].state == State.reverse:
                angle - 180

            #transform the data with the angle
            delta_x = delta_r_cm * math.cos(math.radians(angle))
            delta_y = delta_r_cm * math.sin(math.radians(angle))
        else:
            delta_x = 0
            delta_y = 0
            #angle = 0
            angle = self.dataPackets[-1].rotation  #experimental

        return delta_x, delta_y, angle

    def generate_points(self):
        if self.dataPackets[-1] != None:
            distForward = self.dataPackets[-1].front_distance  #in cm
            distRight = self.dataPackets[-1].right_distance  #in cm

            if distForward >= 0 and distForward < self.max_dist and distRight >= 0 and distRight < self.max_dist:
                forward = (distForward * math.cos(math.radians(self.angle)),
                           distForward * math.sin(math.radians(self.angle)))
                right = (distRight * math.cos(math.radians(self.angle - 90)),
                         distRight * math.sin(math.radians(self.angle - 90)))
                return (forward, right)
            elif distForward >= 0 and distForward < self.max_dist:
                return ((distForward * math.cos(math.radians(self.angle)),
                         distForward * math.sin(math.radians(self.angle))))
            elif distRight >= 0 and distRight < self.max_dist:
                return ((distRight * math.cos(math.radians(self.angle - 90)),
                         distRight * math.sin(math.radians(self.angle - 90))))
            else:
                return ()

    def get_state_from_encoder(self, r, l):
        difference = r - l
        if math.abs(difference) < 2:
            if r > 0 and l > 0:
                return State.forward
            else:
                return State.reverse
        elif r > l:
            return State.turn_left
        elif l > r:
            return State.turn_right
        else:
            raise Exception("get_state_from_encoder method()")

    def front_is_clear(self):
        # self.logger.log("Rahel needs to work on this method BIG SMH")
        return self.dataPackets[-1].front_distance > 20

    def right_is_clear(self):
        return self.dataPackets[-1].right_distance > 20

    def change_state(self, new_state=State.stop):
        self.state = new_state
        self.communicator.transmit_info(self.state)
        self.logger.log("State changed to: " + State.all_states[self.state])

    def sweep(self):
        self.logger.log("sweep method")
        if int(round(time.time() * 1000)) - self.last_state_switch_time > 250:
            self.logger.log("sweep method testing")
            self.last_state_switch_time = int(round(time.time() * 1000))
            if self.front_is_clear():
                self.state = State.forward
            else:
                self.state = State.turn_right

        self.communicator.transmit_info(self.state)
        self.logger.log("State changed to: " + State.all_states[self.state])
示例#6
0
clock = pygame.time.Clock()

ROBOT = pygame.image.load('./ground_station/assets/Robot.png')
COMPASS = pygame.image.load('./ground_station/assets/Compass.png')
buttonFont = pygame.font.Font("./ground_station/assets/OpenSans-Regular.ttf",
                              25)
buttonFont.set_bold(True)
textFont = pygame.font.Font("./ground_station/assets/OpenSans-Regular.ttf", 35)
textFont.set_bold(True)

#information about the robot and location
mode = Mode.manual

#objects that we need
robot = Robot(image=ROBOT)
logger = Logger("BotDisplay")
wall_map = WallMap()


def main():
    global robot, mode, screen, screen_width, screen_height
    start_time = int(round(time.time() * 1000))
    previous_time = start_time
    while True:
        #Check for user input on the keyboard and OSX operations
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                quitProgram()
            elif event.type == pygame.KEYDOWN:
                if event.key == pygame.K_ESCAPE:
                    quitProgram()
示例#7
0
class WallMap:
  def __init__(self, obstacle_points=set(), clear_rectangles=set()):
    self.logger = Logger("WallMap")
    self.obstacle_points = obstacle_points
    self.clear_rectangles = clear_rectangles
    self.walls = set()
    self.x_min = -10
    self.x_max = 10
    self.y_min = -10
    self.y_max = 10
    self.count_since_last_refresh = 0
  
  def add_obstacle_point(self, x, y):
    self.obstacle_points.add((x, y))
    if x > self.x_max - 10:
      self.x_max = x + 10
    if x < self.x_min + 10:
      self.x_min = x - 10
    
    if y > self.y_max - 10:
      self.y_max = y + 10
    if y < self.y_min + 10:
      self.y_min = y - 10
    
    if self.count_since_last_refresh >= 1:
      self.refresh_walls()
      self.count_since_last_refresh = 0
    else:
      self.count_since_last_refresh += 1
      
  def refresh_walls(self):
    for point in self.obstacle_points:
      if len(self.walls) == 0:
        self.walls.add(Wall(point))
        continue

      walls_to_add = []
      for wall in self.walls:
        distance = wall.calculate_distance(point)
        if distance < 5:
          walls_to_add.append(wall)
      if len(walls_to_add) == 1: #if it is close enough to another wall, add it to that wall
        walls_to_add[0].add_point(point)
      else: #otherwise make a new wall with this point
        total_points = []
        for adding_wall in walls_to_add: #combine walls if necessary
          total_points.extend(adding_wall.points)
          self.walls.discard(adding_wall)
        self.walls.add(Wall(total_points))
    self.obstacle_points = set()

  def draw_map(self, screen, x_min, x_max, y_min, y_max, robot):
                            # 0,   0,   1560, 1123
    #parameters are given as actual pixel dimensions, not from 0 to 1

    print('x_max:', x_max)
    print('y_max:', y_max)
    screen_width = x_max - x_min
    screen_height = y_max - y_min

    map_width = self.x_max - self.x_min
    map_height = self.y_max - self.y_min

    x_add_num = -1 * self.x_min
    x_scale = screen_width / map_width

    y_add_num = -1 * self.y_min
    y_scale = screen_height / map_height

    map_ratio = map_height / map_width
    screen_ratio = screen_height / screen_width

    if map_ratio > screen_ratio: # the map is taller than the screen, relatively
      y_scale = x_scale
    else:
      x_scale = y_scale
    
    # map_mid_x = self.x_min + map_width / 2
    # map_mid_y = self.y_min + map_height / 2

    # map_mid_x += x_add_num
    # map_mid_x *= x_scale

    # map_mid_y += y_add_num
    # map_mid_y *= y_scale

    # screen_mid_x = x_min + screen_width / 2
    # screen_mid_y = y_min + screen_height / 2

    x_screen_adjustment = 0
    y_screen_adjustment = 0

    # x_screen_adjustment = screen_mid_x - map_mid_x
    # y_screen_adjustment = screen_mid_y - map_mid_y

    # robot_x = (robot.location[0] - robot.size[0]/2 + x_add_num) * x_scale + x_screen_adjustment
    # robot_y = (robot.location[1] - robot.size[1]/2 + y_add_num) * y_scale + y_screen_adjustment

    # if robot_x > self.x_max - 10:
    #   self.x_max = robot_x + 10
    # if robot_x < self.x_min + 10:
    #   self.x_min = robot_x - 10
    
    # if robot_y > self.y_max - 10:
    #   self.y_max = robot_y + 10
    # if robot_y < self.y_min + 10:
    #   self.y_min = robot_y - 10
      
    # screen.blit(pygame.transform.rotate(robot.image, robot.angle), (robot_x, robot_y))
    for wall in self.walls:
      wall.draw_wall(screen=screen, y_max=y_max, x_add_num=x_add_num, x_scale=x_scale, x_screen_adjustment=x_screen_adjustment, y_add_num=y_add_num, y_scale=y_scale, y_screen_adjustment=y_screen_adjustment)
      for x, y in wall.points:
        self.logger.log('x 0:', x)
        x += x_add_num
        self.logger.log('x 1:', x)
        x *= x_scale
        self.logger.log('x 2:', x)
        x += x_screen_adjustment
        self.logger.log('x 3:', x)

        self.logger.log('y 0:', y)
        y += y_add_num
        self.logger.log('y 1:', y)
        y *= y_scale
        self.logger.log('y 2:', y)
        y += y_screen_adjustment
        self.logger.log('y 3:', y)

        print('y_max', y_max)

        center = (x, y_max - y) #correct the order (x, y) to (r, c)
        pygame.draw.circle(surface=screen, color=Colors.BLUE, center=center, radius=5)
示例#8
0
class Communicator:
    def __init__(self, port="/dev/tty.HC-05-DevB", baud=9600, enabled=True):
        self.port = port
        self.baud = baud
        self.bluetooth = None
        self.enabled = enabled
        self.connected = False
        self.time_packet = [time.time()]
        self.data_stream = ""

        self.logger = Logger("Communicator")
        if self.enabled:
            try:
                self.initiate_bluetooth()
            except:
                self.logger.log("Communicator: Bluetooth Not Connected")
                self.logger.log(sys.exc_info())

    def initiate_bluetooth(self):
        if self.enabled:
            self.bluetooth = serial.Serial(
                self.port, self.baud,
                timeout=0)  #Start communications with the bluetooth unit
            self.bluetooth.flushInput(
            )  #This gives the bluetooth a little kick
            self.connected = False

#Timestamp 12321, State -1 to 4, Distance Sensor Front Double, Distance Sensor Right Double, Left Encoder Value Int, Right Encoder Value Int, Total Angle Double

    def recieve_info(self, old_state=0):
        if not (self.enabled):
            return None

        try:
            input_data = self.bluetooth.readline().decode()
        except:
            print('bluetooth is not connected, error in receive_info method')
            self.logger.log(sys.exc_info())
            return None
        #self.logger.logDataPacket('input_data:', input_data)
        if (input_data == None or input_data.strip() == ""):
            return None

        self.data_stream += input_data.strip()

        while self.data_stream.strip() != "":
            if self.data_stream[0] == ":":  #If first character is a colon
                if self.data_stream.count(":") >= 2:
                    self.data_stream = self.data_stream[1:]
                    data_packet_list = self.data_stream[0:self.data_stream.
                                                        index(":")]
                    self.data_stream = self.data_stream[len(data_packet_list) +
                                                        1:]
                    return self.create_info_packet(data_packet_list.split(","))
                else:
                    return None
            else:
                self.data_stream = self.data_stream[
                    self.data_stream[self.data_stream.index(':')]:]
                self.logger.log("self.data_stream", self.data_stream)
                print("first character is", int(self.data_stream[0]))

        return None
        #   #incoming data is separated with commas and represents these values in order: Millis, state, front distance, right distance, left encoder total, right encoder total, angle total

    def create_info_packet(self, data):
        print("data:", data)
        newdata = []
        for element in data:
            newdata.append(element.strip())
        self.time_packet.append(time.time() * 1000)
        # print(newdata)
        # self.logger.logDataPacket(newdata)
        self.connected = True

        return InfoPacket(newdata[0], newdata[1], newdata[2], newdata[3],
                          newdata[4], newdata[5])

    def transmit_info(self, state=0):
        self.connection_check(500)
        if self.enabled:
            self.previousState = state
            try:
                self.bluetooth.write(str.encode(
                    str(state
                        )))  #These need to be bytes not unicode, plus a number
            except:
                print(
                    'error transmitting info, bluetooth was disconnected from transmit_info method'
                )
                self.logger.log(sys.exc_info())

    def deactivate_bluetooth(self):
        if self.enabled:
            self.bluetooth.close()

    def connection_check(self, benchmark):
        if self.enabled:
            if len(self.time_packet) == 0:
                self.connected = False

            if (time.time() * 1000) - self.time_packet[-1] > benchmark:
                self.connected = False


#Ideal Use
#c = Communicator()
#c.initiate_bluetooth()
#while(True):
#c.transmit_info(robot.state)
#robot.addInfo(c.recieve_info())
#c.deactivate_bluetooth()
示例#9
0
#to work, this class must be moved out of the "tests" folder and into the "ground_station" folder

from Resources import Logger

logger1 = Logger(fromClass="Class1")
logger2 = Logger(fromClass="Class2")

logger1.log("Hello")
logger2.log("Goodbye")