class Robot:
# US sensors constants.
US_SENSORS = [
{
'name': 'front_bottom',
'trigger_limit': 8,
'sensors': 0
},
{
'name': 'left',
'trigger_limit': 10,
'sensors': 2
},
{
'name': 'back',
'trigger_limit': 8,
'sensors': 4
},
{
'name': 'right',
'trigger_limit': 10,
'sensors': 3
},
{
'name': 'front_top',
'trigger_limit': 8,
'sensors': 1
},
]
# Take in count the obstacle dimension (assuming another robot)
# + the robot size.
OBSTACLES_DIMENSION = 50
def __init__(self):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
self._us = RangeSensor(4)
logging.info('Building the graph map.')
# self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
self._blocking_servo = -1
def reset_kinematics(self):
self._kinematic.up_clamp()
time.sleep(0.2)
self._kinematic.open_clamp()
time.sleep(0.2)
self._kinematic.reset_funny()
time.sleep(0.2)
self._kinematic.push_back()
time.sleep(1)
def wait_motors(self, enable=True, timeout=0):
first_time = time.time()
while not self._motors.is_done():
if enable:
if self._blocking_servo != -1:
i = self._blocking_servo
test = self._us.get_range(i)
print(test)
if test > self.US_SENSORS[i]['trigger_limit']:
self._motors.restart()
self._blocking_servo = -1
else:
continue
us_sensors = self._us.get_ranges()
print(us_sensors)
for i, us in enumerate(us_sensors):
if 'front' in self.US_SENSORS[i][
'name'] or 'back' in self.US_SENSORS[i]['name']:
if us < self.US_SENSORS[i]['trigger_limit']:
self._motors.stop()
self._blocking_servo = i
break
if (time.time() - first_time) > timeout and timeout != 0:
break
time.sleep(0.1)
def take_modules(self, number=1):
for i in range(number):
self._kinematic.open_clamp()
self._kinematic.down_clamp()
time.sleep(DELAY_UP_DOWN_CLAMP)
self._motors.forward(9)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
# self._motors.set_speed(40)
self._motors.backward(7)
if number == 1:
self._motors.forward(6)
self.wait_motors(enable=False, timeout=2)
self._motors.backward(6)
self.wait_motors(enable=False, timeout=2)
self.wait_motors(enable=False)
self._motors.backward(8)
self.wait_motors(enable=False)
#SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
self._kinematic.open_clamp()
time.sleep(0.5)
self._motors.forward(3)
self.wait_motors(enable=False, timeout=2)
self._kinematic.close_clamp()
time.sleep(1)
#---
self._motors.forward(3)
self._kinematic.up_clamp()
self.wait_motors(enable=False, timeout=3)
time.sleep(0.5)
self._kinematic.open_clamp()
time.sleep(1)
self._kinematic.down_clamp()
time.sleep(0.5)
self._kinematic.up_clamp()
def eject_modules(self, number=1):
for i in range(number):
self._kinematic.push_out()
time.sleep(1.5)
self._kinematic.push_back()
time.sleep(1.5)
self._motors.forward(2)
self.wait_motors(timeout=2)
self._motors.backward(8)
self.wait_motors(timeout=2)
def get_kin(self):
return self._kinematic
def get_motors(self):
return self._motors
def finalize(self):
self._motors.stop()
self._kinematic.launch_funny()
time.sleep(0.8)
self._kinematic.reset_funny()
class Robot:
_DELAY_OPEN_ClOSE_CLAMP = 0.7
_DELAY_UP_DOWN_CLAMP = 2.5
_DELAY_IN_OUT_BLOCK = 3
# US sensors constants.
US_SENSORS = [
{
'name': 'front_bottom',
'trigger_limit': 10,
'sensors': [0, 1]
},
{
'name': 'left',
'trigger_limit': 10,
'sensors': [2]
},
{
'name': 'back',
'trigger_limit': 5,
'sensors': [4]
},
{
'name': 'right',
'trigger_limit': 10,
'sensors': [3]
},
{
'name': 'front_top',
'trigger_limit': 10,
'sensors': [0, 1]
},
]
DIMENSION = {'length': 32.5, 'width': 19.2}
# Take in count the obstacle dimension (assuming another robot)
# + the robot size.
OBSTACLES_DIMENSION = 50
def __init__(self, position):
"""
position: init position dict with keys:
- "angle": a X-axis relative angle.
- "point": the first position.
"""
self._position = position
# self._us_sensors = RangeSensor(4)
self._motors = Motors(5)
self._kinematic = Kinematics(6)
logging.info('Building the graph map.')
robot_diagonal = math.sqrt(self.DIMENSION['length'] +
self.DIMENSION['width']**2)
self._graph = build_graph(robot_diagonal)
logging.info('Finished to build the graph map.')
def take_modules(self, number=1, distance=0):
for i in range(number):
self._kinematic.down_clamp()
time.sleep(self._DELAY_UP_DOWN_CLAMP)
self._motors.forward(distance)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
self._kinematic.close_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._motors.backward(distance)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
#SEULEMENT SI ON DECIDE DE RECULER POUR MIEUX PRENDRE LE MODULE ---
self._kinematic.open_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._motors.forward(2.5)
while not self._motors.is_done(self.__done_callback):
time.sleep(0.5)
self._kinematic.close_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
#---
self._kinematic.up_clamp()
time.sleep(self._DELAY_UP_DOWN_CLAMP)
self._kinematic.open_clamp()
def eject_modules(self, number=1):
for i in range(number):
self._kinematic.push_out()
time.sleep(self._DELAY_IN_OUT_BLOCK)
if number == 4 and i == 2:
self._kinematic.open_clamp()
time.sleep(self._DELAY_OPEN_CLOSE_CLAMP)
self._kinematic.push_back()
time.sleep(self._DELAY_IN_OUT_BLOCK)
def move_to(self, target):
"""
target: a dict with keys:
- "angle": a X-axis relative constrain target angle.
- "point": position of the target.
"""
while True:
instructions = self._graph.get_path(self._position, target)
status = self._motors.move_with_instructions(
instructions, self.__move_callback, self.__done_callback)
if status == 'ok':
break
def finalize(self):
self._motors.stop()
self._kinematic.launch_funny()
time.sleep(1)
self._kinematic.reset_funny()
def __move_callback(self):
"""
Return a string giving the state if we need to stop or not the regulation because of
an obstacle.
"""
# ranges = self._us_sensors.get_ranges()
ranges = [20, 20, 20, 20, 20]
for us_data in self.US_SENSORS:
for i, sensor in enumerate(us_data['sensors']):
if ranges[sensor] == 0:
# Zero value means that we are too far from obstacles.
continue
if ranges[sensor] < us_data['trigger_limit']:
logging.warn(
'Motors stopped becauce of the %s%i US sensors at %i cm',
us_data['name'], i, ranges[sensor])
self._motors.stop()
# TODO: avoid recalculation if we have the near same position.
self._graph.reset_obstacles()
self._graph.add_obstacle(self._position, self.DIMENSION,
self.OBSTACLES_DIMENSION,
us_data['name'], ranges[sensor])
return 'obstacle'
return 'continue'
def __done_callback(self, distance_travelled, status='ok'):
"""
Update the position and angle informations of the robots.
"""
self._move_target = None
logging.info('Regulation is done!')
self.__update_robot_position(distance_travelled)
print(self._position)
return status
def __update_robot_position(self, distance_travelled):
left = distance_travelled['left']
right = distance_travelled['right']
if self.__is_opposite_sign(left, right):
# We finished a rotation regulation.
angle = ((right - left) / 2) / Motors.ANGLE_CORRECTION
self._position['angle'] += angle
logging.info('Robot turned with an angle of %i', angle)
else:
# We finished a lead regulation.
angle = self._position['angle']
distance = (left + right) / 2
self._position['point'][0] += distance * math.cos(
math.radians(angle))
self._position['point'][1] += distance * math.sin(
math.radians(angle))
def __is_opposite_sign(self, n, m):
if n > 0 and m > 0:
return False
elif n < 0 and m < 0:
return False
return True