def execute_long_fp(self):
CommMgr.send('X', CommMgr.ARDUINO)
_long_cmd = []
_fCount = 0
for _act in self._actions:
if _act == Action.FORWARD:
_fCount += 1
if _fCount == 9:
#self._robot.move_multiple(_fCount)
_command = 'F' + str(_fCount)
_long_cmd.append(_command)
_fCount = 0
elif _act == Action.RIGHT or _act == Action.LEFT:
if _fCount > 0:
#self._robot.move_multiple(_fCount)
_command = 'F' + str(_fCount)
_long_cmd.append(_command)
_fCount = 0
#self._robot.move(_act)
_long_cmd.append(_act.value)
if _fCount > 0:
#self._robot.move_multiple(_fCount)
_command = 'F' + str(_fCount)
_long_cmd.append(_command)
_fCount = 0
time.sleep(1)
CommMgr.send('{}{}'.format('FULL', ''.join(_long_cmd)),
CommMgr.ANDROID)
time.sleep(1)
self._robot.notify_arduino(''.join(_long_cmd))
self._actions = []
return '{}'.format(''.join(_long_cmd))
def run(self):
if self._robot.is_actual_robot():
print('Awaiting Start Point/Waypoint')
while True:
_spwp = CommMgr.recv()
print(_spwp)
if _spwp[:2] == 'SP':
#spxxyywpxxyy
_sp = (abs(int(_spwp[2:4]) - 19), int(_spwp[4:6]))
print(_sp)
#if self._surface is not None:
# self._explored_map.draw(self._surface)
self._wp = (abs(int(_spwp[8:10]) - 19), int(_spwp[10:12]))
print(self._wp)
self._robot.notify_arduino(Action.CALIBRATE.value)
CommMgr.recv()
self._robot.notify_arduino(Action.RIGHT.value)
CommMgr.recv()
self._robot.notify_arduino(Action.CALIBRATE.value)
CommMgr.recv()
self._robot.notify_arduino(Action.RIGHT.value)
CommMgr.recv()
self._robot.set_pos(_sp[0], _sp[1])
self._explored_map.set_robot_explored()
print(_spwp[-1])
if _spwp[-1] == 'N':
self.turn_robot(Orientation.NORTH.value)
elif _spwp[-1] == 'E':
self.turn_robot(Orientation.EAST.value)
elif _spwp[-1] == 'S':
self.turn_robot(Orientation.SOUTH.value)
elif _spwp[-1] == 'W':
self.turn_robot(Orientation.WEST.value)
break
print('Awaiting EX_START')
while True:
_command = CommMgr.recv()
print(_command)
if _command == 'EX_START':
break
print("Starting Exploration")
self._start_time = int(round(time.time() * 1000))
self._end_time = self._start_time + (self._time_limit * 1000)
if self._robot.is_actual_robot():
self._robot.notify_arduino(
Action.SENSOR.value) #, self._explored_map, False)
self.sense_and_update()
area_explored = self.calculate_area_explored()
print("Explored Area: {}".format(area_explored))
self.do_exploration(self._robot.get_pos())
def notify_android(self, action, explored_map):
if self._actualRobot and action != Action.CALIBRATE:
ArenaUtils.generate_arena_descriptor(explored_map)
_ex_mdf = ArenaUtils.hex_string('map/generated/MapExplored.txt')
_obs_mdf = ArenaUtils.hex_string('map/generated/MapObstacle.txt')
_pos = str(self._pos[0]).zfill(2) + str(self._pos[1]).zfill(2)
_to_android = "***{},{},{}:{}:{}".format(
_pos, Orientation.getText(self._ori), action.value, _ex_mdf,
_obs_mdf)
CommMgr.send(_to_android, CommMgr.ANDROID)
def main(): CommMgr.connect() while True: if CommMgr.recv() == 'EX_START': CommMgr.send(Action.FORWARD.value, CommMgr.ANDROID) time.sleep(1) CommMgr.send(Action.LEFT.value, CommMgr.ANDROID) CommMgr.close()
def main(): robot = Robot((18,1), True) #robot.set_speed(100) #_real_map = Arena(robot) #ArenaUtils.load_arena_from_file(_real_map, 'map/SampleWeek11.txt') _explore_map = Arena(robot) _explore_map.set_allunexplored() CommMgr.connect() _explore = Exploration(_explore_map, robot, 300, 3600) _explore.run() CommMgr.close()
def main():
#command = input('Enter 1: Actual Run, 2: Fastest Path => ')
CommMgr.connect()
#if int(command) == 1:
explore()
#else:
# fp()
command = input('Press ENTER to close connection.... ')
if command == '':
CommMgr.close()
def fp():
robot = Robot((18, 1), True)
_real_map = Arena(robot)
ArenaUtils.load_arena_from_file(_real_map, 'map/17_week10.txt')
print('Awaiting FP_START')
while True:
_command = CommMgr.recv()
if _command == 'FP_START':
CommMgr.send('X', CommMgr.ARDUINO)
break
_go_to_wp_goal = FastestPath(_real_map, robot)
_status = _go_to_wp_goal.do_fastest_path_wp_goal(
(7, 10), ArenaConstant.GOAL_POS.value)
def do_exploration(self, pos):
while True:
self.next_move()
#input_var = input("Enter something to continue: ")
#print ("you entered " + input_var)
#time.sleep(self._robot.get_speed() / 1000)
area_explored = self.calculate_area_explored()
if self._robot.get_pos()[0] == pos[0] and self._robot.get_pos(
)[1] == pos[1]:
if area_explored >= 100:
break
print(area_explored)
if area_explored > self._coverage_limit or int(
round(time.time() * 1000)) > self._end_time:
break
self.go_home()
if self._robot.is_actual_robot():
#self._robot.notify_android(Action.CALIBRATE, self._explored_map)
print('Awaiting FP_START')
while True:
_command = CommMgr.recv()
if _command == 'FP_START':
break
_go_to_wp_goal = FastestPath(self._explored_map, self._robot)
_go_to_wp_goal.do_fastest_path_wp_goal(self._wp,
ArenaConstant.GOAL_POS.value)
def move_robot(self, action):
print(action)
self._action_taken.append(action)
self._robot.move(action)
self._robot.notify_arduino(action) #, self._explored_map, True)
#if self._surface is not None:
# self._explored_map.draw(self._surface)
if action is not Action.CALIBRATE:
self.sense_and_update()
self._robot.notify_android(action, self._explored_map)
else:
_status = CommMgr.recv()
print(_status)
if self._robot.is_actual_robot() and not self.calibration_mode:
self.calibration_mode = True
if self.can_calibrate_on_spot(self._robot.get_ori()):
self.last_calibrate = 0
self.move_robot(Action.CALIBRATE)
#self._robot.notify_arduino(Action.CALIBRATE)#, self._explored_map, False)
else:
self.last_calibrate += 1
if self.last_calibrate >= 3:
_target_calibrate = self.get_calibrate_ori()
if _target_calibrate is not None:
self.last_calibrate = 0
self.calibrate(_target_calibrate)
self.calibration_mode = False
def move_robot(self, action):
print(action)
if not self.calibration_mode:
self._action_taken.append(action)
self._robot.move(action)
self._robot.notify_arduino(action.value) #, self._explored_map, True)
#if self._surface is not None:
# self._explored_map.draw(self._surface)
if action is not Action.CALIBRATE and not self.calibration_mode:
self.sense_and_update()
self._robot.notify_android(action.value, self._explored_map)
else:
if not self.end_calibrate:
_status = CommMgr.recv()
print(_status)
else:
self.sense_and_update()
self._robot.notify_android(action.value, self._explored_map)
if self._robot.is_actual_robot() and not self.calibration_mode:
self.calibration_mode = True
if self.can_calibrate_on_spot(self._robot.get_ori()):
self.last_calibrate = 0
self.move_robot(Action.CALIBRATE)
if self.can_calibrate_on_spot(
Orientation.getPrev(self._robot.get_ori())):
self.calibrate(Orientation.getPrev(self._robot.get_ori()))
self.end_calibrate = False
elif self.can_calibrate_on_spot(
Orientation.getNext(self._robot.get_ori())):
self.calibrate(Orientation.getNext(self._robot.get_ori()))
self.end_calibrate = False
else:
_last_act = self._action_taken[len(self._action_taken) - 1]
if _last_act == Action.UTURN:
self.last_calibrate += 2
else:
self.last_calibrate += 1
if self.last_calibrate >= 5:
_target_calibrate = self.get_calibrate_ori()
if _target_calibrate is not None:
self.last_calibrate = 0
self.calibrate(_target_calibrate)
self.end_calibrate = False
self.calibration_mode = False
def run_exploration(self, robot, map_file):
self.init_objects(robot, map_file)
while True:
# _mouse = pygame.mouse.get_pos()
# if 450 + self._ex_rect.left < _mouse[0] < 450 + self._ex_rect.right and self._ex_rect.top < _mouse[1] < self._ex_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._ex_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._ex_rect, 3)
# if 450 + self._timed_rect.left < _mouse[0] < 450 + self._timed_rect.right and self._timed_rect.top < _mouse[1] < self._timed_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._timed_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._timed_rect, 3)
# if 450 + self._coverage_rect.left < _mouse[0] < 450 + self._coverage_rect.right and self._coverage_rect.top < _mouse[1] < self._coverage_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._coverage_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._coverage_rect, 3)
# if 450 + self._wp_rect.left < _mouse[0] < 450 + self._wp_rect.right and self._wp_rect.top < _mouse[1] < self._wp_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._wp_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._fp_rect, 3)
# if 450 + self._fp_rect.left < _mouse[0] < 450 + self._fp_rect.right and self._fp_rect.top < _mouse[1] < self._fp_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._fp_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._fp_rect, 3)
# if 450 + self._mdf_rect.left < _mouse[0] < 450 + self._mdf_rect.right and self._mdf_rect.top < _mouse[1] < self._mdf_rect.bottom:
# pygame.draw.rect(self._menu, (255,0,0), self._mdf_rect, 3)
# else:
# pygame.draw.rect(self._menu, (0,0,0), self._mdf_rect, 3)
# self._screen.blit(self._menu, (ArenaConstant.ARENA_COL.value * self.BLOCK_SIZE,0))
# pygame.display.update()
keys = pygame.key.get_pressed()
for event in pygame.event.get():
if event.type == QUIT or keys[pygame.K_ESCAPE]:
CommMgr.close()
pygame.quit()
sys.exit()
elif keys[pygame.K_RETURN]:
self.init_objects(robot, map_file)
if self._real_run:
CommMgr.connect()
_explore = Exploration(self._explore_map, self._real_map,
self._robot, 300, 3600,
(self._screen, self._background))
_explore.run()
if self._real_run:
CommMgr.close()
def sense(self, explored_map, real_map=None):
result = [None for x in range(6)]
# if not self._actualRobot:
# result[0] = self._SSFrontLeft.sense(explored_map, real_map)
# result[1] = self._SSFrontCenter.sense(explored_map, real_map)
# result[2] = self._SSFrontRight.sense(explored_map, real_map)
# result[3] = self._SSLeftTop.sense(explored_map, real_map)
# result[4] = self._LSRightCenter.sense(explored_map, real_map)
# result[5] = self._SSRightTop.sense(explored_map, real_map)
# else:
_sensor_str = CommMgr.recv().rstrip()
print(_sensor_str)
result = _sensor_str.split('-')
for i in range(len(result)):
if result[i] == '':
result[i] = 0
else:
result[i] = int(float(result[i]))
self._LSRightCenter.sense_real(
explored_map,
result[0]) #, (10 <= result[0] < 20 and 10 <= result[1] < 20))
self._SSLeftTop.sense_real(
explored_map,
result[1]) #, (10 <= result[0] < 20 and 10 <= result[1] < 20))
self._SSFrontLeft.sense_real(
explored_map, result[2]
) #, (10 <= result[2] < 20 and 10 <= result[3] < 20 and 10 <= result[4] < 20))
self._SSFrontCenter.sense_real(
explored_map, result[3]
) #, (10 <= result[2] < 20 and 10 <= result[3] < 20 and 10 <= result[4] < 20))
self._SSFrontRight.sense_real(
explored_map, result[4]
) #, (10 <= result[2] < 20 and 10 <= result[3] < 20 and 10 <= result[4] < 20))
self._SSRightTop.sense_real(explored_map,
result[5]) #, (10 <= result[5] < 20))
return result
def notify_arduino(self, action):
if self._actualRobot:
#if isinstance(action, str):
# CommMgr.send(action, CommMgr.ARDUINO)
#else:
CommMgr.send(action.value, CommMgr.ARDUINO)
def process_sensor_val(self, explored_map, sensor_val, row_inc, col_inc,
check):
sensor_val = sensor_val + 10
if len(self._id) == 4:
if explored_map.check_valid_coord(
(self._pos[0] + row_inc, self._pos[1] + col_inc)):
if self._id[3] == 'L' and (explored_map.get_block(
(self._pos[0] + row_inc,
self._pos[1] + col_inc)).is_obstacle()):
return
if explored_map.check_valid_coord(
(self._pos[0] + (row_inc * 2), self._pos[1] + (col_inc * 2))):
if self._id[3] == 'L' and (explored_map.get_block(
(self._pos[0] + (row_inc * 2), self._pos[1] +
(col_inc * 2))).is_obstacle()):
return
for i in range(self._range[0], self._range[1] + 1):
_pos = (self._pos[0] + (row_inc * i), self._pos[1] + (col_inc * i))
if not explored_map.check_valid_coord(_pos):
print('{} Processing End'.format(self._id))
return
if explored_map.get_block(_pos).is_obstacle(
): #or explored_map.get_block(_pos).is_explored():
print('{} Processing End'.format(self._id))
return
explored_map.get_block(_pos).set_explored(True)
#if (sensor_val - 10) <= 9:
# return
if (sensor_val // 10) == 2 and check:
CommMgr.send(Action.FORWARD.value, CommMgr.ARDUINO)
print('{} Moving Forward to check'.format(self._id))
_sensor_str = CommMgr.recv().rstrip()
_check = int(float(_sensor_str.split('-')[int(
self._id[2])])) + 10
print('{} check value: {}'.format(self._id, _check))
if _check // 10 == 1:
print('{} sets {} as OBSTACLE => Sensor Value: {}'.format(
self._id, _pos, sensor_val))
explored_map.set_obstacle(_pos, True)
CommMgr.send(Action.BACKWARD.value, CommMgr.ARDUINO)
print('{} Moving Backward'.format(self._id))
CommMgr.recv()
return
else:
print('{} sets {} as CLEARED => Sensor Value: {}'.format(
self._id, _pos, sensor_val))
CommMgr.send(Action.BACKWARD.value, CommMgr.ARDUINO)
print('{} Moving Backward'.format(self._id))
CommMgr.recv()
continue
if (sensor_val // 10) == i:
print('{} sets {} as OBSTACLE => Sensor Value: {}'.format(
self._id, _pos, sensor_val))
explored_map.set_obstacle(_pos, True)
return
else:
print('{} sets {} as CLEARED => Sensor Value: {}'.format(
self._id, _pos, sensor_val))
continue