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 __init__(self, points):
if type(points) is list:
self.points = points
else:
self.points = [points]
self.logger = Logger("Wall")
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 __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))
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])
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()
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)
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()
#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")