def getNewOrientation(self, action):
if action == Action.RIGHT:
return Orientation.getNext(self._ori)
elif action == Action.LEFT:
return Orientation.getPrev(self._ori)
elif action == Action.UTURN:
return Orientation.getOpp(self._ori)
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 get_calibrate_ori(self):
_cur_ori = self._robot.get_ori()
_check_ori = Orientation.getNext(_cur_ori)
if self.can_calibrate_on_spot(_check_ori):
return _check_ori
_check_ori = Orientation.getPrev(_cur_ori)
if self.can_calibrate_on_spot(_check_ori):
return _check_ori
_check_ori = Orientation.getPrev(_check_ori)
if self.can_calibrate_on_spot(_check_ori):
return _check_ori
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 turn_robot(self, _ori_target):
_num_of_turn = abs(self._robot.get_ori() - _ori_target)
if _num_of_turn > 2:
_num_of_turn = _num_of_turn % 2
if _num_of_turn == 1:
if Orientation.getNext(self._robot.get_ori()) == _ori_target:
self.move_robot(Action.RIGHT)
else:
self.move_robot(Action.LEFT)
elif _num_of_turn == 2:
self.move_robot(Action.UTURN)
def go_home(self):
if self._coverage_limit == 300 and self._time_limit == 3600:
if not self._robot.get_reachedGoal():
while True:
_go_to_goal = FastestPath(
self._explored_map, self._robot,
True) #, self._real_map, self._surface)
_status = _go_to_goal.do_fastest_path(
ArenaConstant.GOAL_POS.value)
if _status != 'T':
break
_return_to_start = FastestPath(
self._explored_map, self._robot,
True) #, self._real_map, self._surface)
_status = _return_to_start.do_fastest_path(
Attribute.START_POS.value)
print(_status)
else:
_backtrace = list(self._action_taken)
while len(_backtrace) != 0:
_act = _backtrace.pop()
if _act == Action.FORWARD:
self.move_robot(Action.BACKWARD)
elif _act == Action.RIGHT:
self.move_robot(Action.LEFT)
elif _act == Action.LEFT:
self.move_robot(Action.RIGHT)
elif _act == Action.BACKWARD:
self.move_robot(Action.FORWARD)
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:
_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
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:
_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
self.turn_robot(Orientation.NORTH.value)
print("Exploration Completed!")
area_explored = self.calculate_area_explored()
print("{:0.2f}% Coverage".format(((area_explored / 300.0) * 100)))
print("{} Blocks".format(area_explored))
print("{} seconds".format(
(int(round(time.time() * 1000)) - self._start_time) / 1000))