def __init__(self, dcm, mem, event, file_name):
"""
@dcm : proxy to DCM (object)
@mem : proxy to Motion (object)
@event : event object (object)
@file_name : file to save log (string)
"""
threading.Thread.__init__(self)
self._event = event
self._file_name = file_name
self._robot_on_charging_station = subdevice.ChargingStationSensor(
dcm, mem)
self._battery_current_sensor = subdevice.BatteryChargeSensor(dcm, mem)
self._wheelb_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
self._wheelfr_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
self._wheelfl_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
self._wheelb_speed_sen = subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
self._wheelfr_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFR")
self._wheelfl_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFL")
self._wheelb_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelB")
self._wheelfr_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelFR")
self._wheelfl_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelFL")
self._end = False
def __init__(self, dcm, mem, event, file_name):
"""
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@event : event (object)
@file_name : name of log file (string)
"""
threading.Thread.__init__(self)
self._mem = mem
self._event = event
self._file_name = file_name
self._wheelb_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
self._wheelfr_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
self._wheelfl_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
self._wheelb_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelB")
self._wheelfr_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFR")
self._wheelfl_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFL")
self._end = False
def test_reliability_003(dcm, mem, motion, motion_wake_up, parameters,
direction):
"""
Test : bumper detects step.
Return True if bumper detects obstacles and robot stops
Return False if bumper detects obstacle but robot moves again
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@motion : proxy to ALMotion (object)
@motion_wake_up : robot does it wakeUp
@parameters : dictionnary {"parameter":value} from config file
(dictionnary)
@directions : dictionnary {"direction":value} (dictionnary)
"""
# Objects creation
right_bumper = subdevice.Bumper(dcm, mem, "FrontRight")
left_bumper = subdevice.Bumper(dcm, mem, "FrontLeft")
back_bumper = subdevice.Bumper(dcm, mem, "Back")
wheelfr_speed_act = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheelfl_speed_act = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
log = Log(dcm, mem, direction + "-" + parameters["Speed"][0])
log.start()
print "Go move"
move(
motion, float(parameters["Speed"][0]),
qha_tools.read_section("reliability_003.cfg", "Direction" + direction))
print "Move finish"
flag = 0
while flag < int(parameters["Nbcycles"][0]):
if right_bumper.value or left_bumper.value or back_bumper.value:
print "Bumper"
time.sleep(3) # Time to wait wheel actuators reaction...
if wheelfr_speed_act.value == 0 and wheelfl_speed_act.value == 0:
print "Stop OK"
flag += 1
time.sleep(5) # Robot forget mapping
moveto(
motion, float(parameters["MoveBackDistance"][0]),
qha_tools.read_section("reliability_003.cfg",
"Direction" + direction))
print "Move back OK"
time.sleep(2)
move(
motion, float(parameters["Speed"][0]),
qha_tools.read_section("reliability_003.cfg",
"Direction" + direction))
print "Move"
else:
print flag
break
log.stop()
assert flag == int(parameters["Nbcycles"][0])
def move_robot(dcm, mem, wait_time, list_wheels_to_use):
"""
Makes the robot move.
- The robot uses for each sequence two wheels and lets
the third one unstiffed.
- The speed is the same for the 3 wheels.
- It returns the wheels used for each sequence.
"""
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
# Wheel speed actuators
wheel_fr_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheel_fl_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
wheel_b_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
# Same speed for the 3 wheels
speed_fraction = 0.3
speed = speed_fraction * wheel_fr_speed_actuator.maximum
# To know which wheels are used for each sequence
list_stiffed_wheels = []
# Make the robot move according to the wheels to use
#-------------------------- CASE 1 -------------------------#
if list_wheels_to_use[0] == 1 and list_wheels_to_use[1] == 1:
wheel_fr_speed_actuator.mqvalue = [(-speed, wait_time)]
wheel_fl_speed_actuator.mqvalue = [(speed, wait_time)]
list_stiffed_wheels.append('fr')
list_stiffed_wheels.append('fl')
return list_stiffed_wheels
#-------------------------- CASE 2 -------------------------#
if list_wheels_to_use[1] == 1 and list_wheels_to_use[2] == 1:
wheel_fl_speed_actuator.mqvalue = [(-speed, wait_time)]
wheel_b_speed_actuator.mqvalue = [(speed, wait_time)]
list_stiffed_wheels.append('fl')
list_stiffed_wheels.append('b')
return list_stiffed_wheels
#-------------------------- CASE 3 -------------------------#
if list_wheels_to_use[2] == 1 and list_wheels_to_use[0] == 1:
wheel_b_speed_actuator.mqvalue = [(-speed, wait_time)]
wheel_fr_speed_actuator.mqvalue = [(speed, wait_time)]
list_stiffed_wheels.append('b')
list_stiffed_wheels.append('fr')
return list_stiffed_wheels
def fin():
wheel_fr_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
wheel_fl_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
wheel_b_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
wheel_fr_speed_actuator.qvalue = (0.0, 0)
wheel_fl_speed_actuator.qvalue = (0.0, 0)
wheel_b_speed_actuator.qvalue = (0.0, 0)
print "Robot stopped"
tools.wait(dcm, wait_time)
def log_wheels_actu_sens(request, dcm, mem, system, log_period):
"""
Log wheels' speeds [rad/s] every 0.5s
"""
wheel_fr_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFR")
wheel_fl_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFL")
wheel_b_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
wheel_fr_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheel_fl_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
wheel_b_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
file_extension = "csv"
robot_name = system.robotName()
date = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
log_type = "wheels_speeds"
file_name = "-".join([robot_name, date, log_type])
output = ".".join([file_name, file_extension])
data = open(output, 'w')
data.write("Time (s)" + "," + "wheel FR speed sensor [rad/s]" + "," +
"wheel FR speed actuator [rad/s]" + "," +
"wheel FL speed sensor [rad/s]" + "," +
"wheel FL speed actuator [rad/s]" + "," +
"Wheel B speed sensor [rad/s]" + "," +
"Wheel B speed actuator [rad/s]" + "\n")
threading_flag = threading.Event()
def log(threading_flag):
cpt = 1
time_init = time.time()
while not threading_flag.is_set():
line = ""
if float(format((time.time() - time_init),
'.1f')) == (cpt * log_period):
cpt += 1
line += str(float(format((time.time() - time_init),
'.1f'))) + "," + \
str(wheel_fr_speed_sensor.value) + "," + \
str(wheel_fr_speed_actuator.value) + "," + \
str(wheel_fl_speed_sensor.value) + "," + \
str(wheel_fl_speed_actuator.value) + "," + \
str(wheel_b_speed_sensor.value) + "," + \
str(wheel_b_speed_actuator.value) + "\n"
data.write(line)
data.flush()
log_thread = threading.Thread(target=log, args=(threading_flag, ))
log_thread.start()
def test_wheels_dico(request, result_base_folder, dcm, mem):
"""
Create the appropriate objects for each joint.
It logs the informations into a dictionnary and save the data into a file
after each wheel test.
"""
wheel_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, request.param)
wheel_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, request.param)
logger = qha_tools.Logger()
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([
result_base_folder, wheel_speed_actuator.subdevice_type, "_".join(
[wheel_speed_actuator.short_name,
str(logger.flag)])
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
# creating a dictionnary with all the objects
dico_object = {
"wheelActuator": wheel_speed_actuator,
"wheelSensor": wheel_speed_sensor,
"logger": logger
}
return dico_object
def test_sensors_safety_001(robot_ip, dcm, mem, motion, session,
motion_wake_up, direction, speed):
"""
Docstring
"""
expected = {"ALMotion/MoveFailed": 1}
module_name = "EventChecker_{}_".format(uuid.uuid4())
obstacledetected = EventModule(mem)
module_id = session.registerService(module_name, obstacledetected)
obstacledetected.subscribe(module_name, expected)
# Objects creation
wheelb_speed_act = subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
wheelfr_speed_act = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheelfl_speed_act = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
flag_test = True
log = multi_logger.Logger(
robot_ip,
"multi_logger.cfg",
0.1,
"Direction" + direction + " - " + str(speed) + ".csv")
log.log()
# Movement
move(motion, speed,
qha_tools.read_section("safety_001.cfg", "Direction" + direction)
)
time.sleep(1)
while not obstacledetected.flag:
if wheelb_speed_act.value == 0 and \
wheelfr_speed_act.value == 0 and \
wheelfl_speed_act.value == 0:
flag_test = False
log.stop()
session.unregisterService(module_id)
assert flag_test
def __init__(self, dcm, mem, file_name):
"""
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@file_name : name of log file (string)
"""
threading.Thread.__init__(self)
self._mem = mem
self._file_name = file_name
self._wheelb_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
self._wheelfr_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
self._wheelfl_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
self._rightbumper = subdevice.Bumper(dcm, mem, "FrontRight")
self._leftbumper = subdevice.Bumper(dcm, mem, "FrontLeft")
self._end = False
def move_robot_random(dcm, mem, wait_time, min_fraction, max_fraction,
max_random):
"""
Makes the robot move randomly.
"""
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
# Wheel speed actuators
wheel_fr_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheel_fl_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
wheel_b_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
# Random speed
speed_fraction_fr = float(
format(random.uniform(min_fraction, max_fraction), '.1f'))
speed_fraction_fl = float(
format(random.uniform(min_fraction, max_fraction), '.1f'))
speed_fraction_b = float(
format(random.uniform(min_fraction, max_fraction), '.1f'))
print type(speed_fraction_fr)
print type(speed_fraction_fl)
print type(speed_fraction_b)
speed_fr = speed_fraction_fr * wheel_fr_speed_actuator.maximum
speed_fl = speed_fraction_fl * wheel_fl_speed_actuator.maximum
speed_b = speed_fraction_b * wheel_b_speed_actuator.maximum
alea = int(random.uniform(0, 15))
# Make the robot move randomly
#-------------------------- CASE 1 -------------------------#
if alea <= 5:
timed_commands_fr = [(-speed_fr, wait_time)]
timed_commands_fl = [(speed_fl, wait_time)]
timed_commands_b = [(0.0, wait_time)]
wheel_fr_speed_actuator.mqvalue = timed_commands_fr
wheel_fl_speed_actuator.mqvalue = timed_commands_fl
return [timed_commands_fr, timed_commands_fl, timed_commands_b]
#-------------------------- CASE 2 -------------------------#
if alea > 5 and alea <= 10:
timed_commands_fl = [(-speed_fl, wait_time)]
timed_commands_b = [(speed_b, wait_time)]
timed_commands_fr = [(0.0, wait_time)]
wheel_fl_speed_actuator.mqvalue = timed_commands_fl
wheel_b_speed_actuator.mqvalue = timed_commands_b
return [timed_commands_fr, timed_commands_fl, timed_commands_b]
#-------------------------- CASE 3 -------------------------#
if alea > 10 and alea <= 15:
timed_commands_b = [(-speed_b, wait_time)]
timed_commands_fr = [(speed_fr, wait_time)]
timed_commands_fl = [(0.0, wait_time)]
wheel_b_speed_actuator.mqvalue = timed_commands_b
wheel_fr_speed_actuator.mqvalue = timed_commands_fr
return [timed_commands_fr, timed_commands_fl, timed_commands_b]
def test_move_random(dcm, mem, leds, expressiveness, wait_time,
wait_time_bumpers, min_fraction, max_fraction, max_random,
stop_robot, wake_up_pos_brakes_closed, stiff_robot_wheels,
unstiff_joints, log_wheels_speed, log_bumper_pressions):
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
#------------------------- Wheels ------------------------#
list_wheel_speed_actuator = [
subdevice.WheelSpeedActuator(dcm, mem, "WheelFR"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelFL"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
]
list_wheel_speed_sensor = [
subdevice.WheelSpeedSensor(dcm, mem, "WheelFR"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelFL"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
]
list_wheel_stiffness_actuator = [
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFR"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFL"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelB")
]
list_wheel_temperature = [
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFR"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFL"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelB")
]
#------------------------- Bumpers -----------------------#
list_bumper = [
subdevice.Bumper(dcm, mem, "FrontRight"),
subdevice.Bumper(dcm, mem, "FrontLeft"),
subdevice.Bumper(dcm, mem, "Back")
]
#-------------------------- Leds --------------------------#
# Disable notifications (from disconnected tablet)
expressiveness._setNotificationEnabled(False)
# Switch off eyes and ears leds
leds.off('FaceLeds')
leds.off('EarLeds')
# Switch on shoulder leds
leds.createGroup("shoulder_group", SHOULDER_LEDS)
leds.on("shoulder_group")
# Bumpers activation detection
parameters_bumpers = tools.read_section("config.cfg",
"BumpersActivationsParameters")
# Launch bumper counter (thread)
bumper_detection = mobile_base_utils.BumpersCounting(
dcm, mem, leds, wait_time_bumpers)
bumper_detection.start()
#---------------------------------------------------------#
#--------------------- Initialization --------------------#
#---------------------------------------------------------#
flag_bumpers = False
list_commands = move_robot_random(dcm, mem, wait_time, min_fraction,
max_fraction, max_random)
tools.wait(dcm, 2000)
#---------------------------------------------------------#
#----------------------- Main Loop -----------------------#
#---------------------------------------------------------#
while flag_bumpers == False:
try:
#-------------------- Wall event -------------------#
if (list_bumper[0].value == 1) or \
(list_bumper[1].value == 1) or \
(list_bumper[2].value == 1):
# Robot hit wall --> Stop it
list_wheel_speed_actuator[0].qvalue = (0.0, 0)
list_wheel_speed_actuator[1].qvalue = (0.0, 0)
list_wheel_speed_actuator[2].qvalue = (0.0, 0)
# Going back
list_wheel_speed_actuator[0].qvalue = [
(-1) * list_commands[0][0][0], wait_time
]
list_wheel_speed_actuator[1].qvalue = [
(-1) * list_commands[1][0][0], wait_time
]
list_wheel_speed_actuator[2].qvalue = [
(-1) * list_commands[2][0][0], wait_time
]
tools.wait(dcm, 4000)
# random again
list_commands = move_robot_random(dcm, mem, wait_time,
min_fraction, max_fraction,
max_random)
#tools.wait(dcm, 2000)
#--------------------- Check temperatures --------------------#
check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed_actuator,
list_wheel_stiffness_actuator)
#-------------------- Check test variables -------------------#
if bumper_detection.bumpers_activations == \
int(parameters_bumpers["nb_bumper_activations"][0]):
bumper_detection.stop()
flag_bumpers = True
# Exit test if user interrupt
except KeyboardInterrupt:
print "\n******* User interrupt - ending test *******"
bumper_detection.stop()
break
def run(self):
""" Log """
robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
battery_current = subdevice.BatteryCurrentSensor(self.dcm, self.mem)
back_bumper_sensor = subdevice.Bumper(self.dcm, self.mem, "Back")
wheelfr_speed_actuator = subdevice.WheelSpeedActuator(
self.dcm, self.mem, "WheelFR")
wheelfl_speed_actuator = subdevice.WheelSpeedActuator(
self.dcm, self.mem, "WheelFL")
wheelfr_speed_sensor = subdevice.WheelSpeedSensor(
self.dcm, self.mem, "WheelFR")
wheelfl_speed_sensor = subdevice.WheelSpeedSensor(
self.dcm, self.mem, "WheelFL")
wheelfr_current_sensor = subdevice.WheelCurrentSensor(
self.dcm, self.mem, "WheelFR")
wheelfl_current_sensor = subdevice.WheelCurrentSensor(
self.dcm, self.mem, "WheelFL")
log_file = open(self.file_name, 'w')
log_file.write(
"Time,Detection,Current,BackBumper,WheelFRSpeedActuator," +
"WheelFRSpeedSensor,WheelFRCurrent,WheelFLSpeedActuator," +
"WheelFLSpeedSensor,WheelFLCurrent\n")
plot_server = easy_plot_connection.Server()
time_init = time.time()
while not self._end_plot:
elapsed_time = time.time() - time_init
plot_server.add_point("Detection", elapsed_time,
robot_on_charging_station.value)
plot_server.add_point("Current", elapsed_time,
battery_current.value)
plot_server.add_point("BackBumper", elapsed_time,
back_bumper_sensor.value)
plot_server.add_point("WheelFRSpeedActuator", elapsed_time,
wheelfr_speed_actuator.value)
plot_server.add_point("WheelFRSpeedSensor", elapsed_time,
wheelfr_speed_sensor.value)
line_to_write = ",".join([
str(elapsed_time),
str(robot_on_charging_station.value),
str(battery_current.value),
str(back_bumper_sensor.value),
str(wheelfr_speed_actuator.value),
str(wheelfr_speed_sensor.value),
str(wheelfr_current_sensor.value),
str(wheelfl_speed_actuator.value),
str(wheelfl_speed_sensor.value),
str(wheelfl_current_sensor.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
time.sleep(0.1)
def test_move_random(motion, dcm, mem, leds, expressiveness, wait_time,
wait_time_bumpers, min_fraction, max_fraction, max_random,
stop_robot, wakeup_no_rotation, stiff_robot_wheels,
unstiff_joints, log_wheels_speed, log_bumper_pressions):
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
#------------------------- Wheels ------------------------#
list_wheel_speed_actuator = [
subdevice.WheelSpeedActuator(dcm, mem, "WheelFR"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelFL"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
]
list_wheel_speed_sensor = [
subdevice.WheelSpeedSensor(dcm, mem, "WheelFR"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelFL"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
]
list_wheel_stiffness_actuator = [
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFR"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFL"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelB")
]
list_wheel_temperature = [
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFR"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFL"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelB")
]
#------------------------- Bumpers -----------------------#
list_bumper = [
subdevice.Bumper(dcm, mem, "FrontRight"),
subdevice.Bumper(dcm, mem, "FrontLeft"),
subdevice.Bumper(dcm, mem, "Back")
]
#-------------------------- Leds --------------------------#
# Disable notifications (from disconnected tablet)
expressiveness._setNotificationEnabled(False)
motion.setExternalCollisionProtectionEnabled('All', False)
# Switch off eyes and ears leds
leds.off('FaceLeds')
leds.off('EarLeds')
# Switch on shoulder leds
leds.createGroup("shoulder_group", SHOULDER_LEDS)
leds.on("shoulder_group")
#----------------------------------------------------------#
#--------------------- Test selection ---------------------#
#----------------------------------------------------------#
# Cables crossing detection
parameters_cables = tools.read_section("config.cfg",
"CablesRoutingParameters")
# Bumpers activation detection
parameters_bumpers = tools.read_section("config.cfg",
"BumpersActivationsParameters")
# Get the desired type of test
flag_test = tools.read_section("config.cfg", "TestChoice")
# Launch bumper counter (thread)
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection = mobile_base_utils.BumpersCounting(
dcm, mem, leds, wait_time_bumpers)
bumper_detection.start()
# Launch cable counter (thread)
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection = mobile_base_utils.CablesCrossing(dcm, mem)
cable_detection.start()
#---------------------------------------------------------#
#--------------------- Initialization --------------------#
#---------------------------------------------------------#
flag_bumpers = False
flag_cables = False
list_wheels_to_use = [1, 1, 0]
#tools.wait(dcm, 30000)
used_wheels = move_robot(dcm, mem, wait_time, list_wheels_to_use)
tools.wait(dcm, 2000)
acc_z = subdevice.InertialSensorBase(dcm, mem, "AccelerometerZ")
#---------------------------------------------------------#
#----------------------- Main Loop -----------------------#
#---------------------------------------------------------#
while flag_bumpers == False and flag_cables == False:
try:
#-------------------- Wall event -------------------#
if (math.fabs(list_wheel_speed_sensor[0].value) < 0.10) and \
(math.fabs(list_wheel_speed_sensor[1].value) < 0.10) and \
(math.fabs(list_wheel_speed_sensor[2].value) < 0.10):
# Robot hit wall --> Stop it
list_wheel_speed_actuator[0].qvalue = (0.0, 0)
list_wheel_speed_actuator[1].qvalue = (0.0, 0)
list_wheel_speed_actuator[2].qvalue = (0.0, 0)
# stop_robot(list_wheel_speed_actuator)
# check temperatures
check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed_actuator,
list_wheel_stiffness_actuator)
alea = int(random.uniform(0, 10))
if used_wheels[0] == 'fr' and used_wheels[1] == 'fl':
wheel_stiffed_1 = list_wheel_stiffness_actuator[0]
wheel_stiffed_2 = list_wheel_stiffness_actuator[1]
list_wheel_stiffness_actuator[2].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [0, 1, 1]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [1, 0, 1]
if used_wheels[0] == 'fl' and used_wheels[1] == 'b':
wheel_stiffed_1 = list_wheel_stiffness_actuator[1]
wheel_stiffed_2 = list_wheel_stiffness_actuator[2]
list_wheel_stiffness_actuator[0].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [1, 0, 1]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [1, 1, 0]
if used_wheels[0] == 'b' and used_wheels[1] == 'fr':
wheel_stiffed_1 = list_wheel_stiffness_actuator[2]
wheel_stiffed_2 = list_wheel_stiffness_actuator[0]
list_wheel_stiffness_actuator[1].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [1, 1, 0]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [0, 1, 1]
used_wheels = move_robot(dcm, mem, wait_time,
list_wheels_to_use)
tools.wait(dcm, 2000)
#-------------------- Check test variables -------------------#
if int(flag_test["test_bumpers"][0]) == 1 and \
bumper_detection.bumpers_activations == \
int(parameters_bumpers["nb_bumper_activations"][0]):
bumper_detection.stop()
flag_bumpers = True
if int(flag_test["test_cables_crossing"][0]) == 1 and \
cable_detection.cables_crossing == \
int(parameters_cables["Nb_cables_crossing"][0]):
cable_detection.stop()
flag_cables = True
#-------- Robot has fallen ----------#
if (math.fabs(acc_z.value)) < 2.0:
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection.stop()
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection.stop()
break
# Exit test if user interrupt
except KeyboardInterrupt:
print "\n******* User interrupt - ending test *******"
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection.stop()
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection.stop()
break