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 run(self):
"""
Log only when robot moves
"""
# Objects creation
robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
battery_current = subdevice.BatteryCurrentSensor(self.dcm, self.mem)
test_timer = 0.
time_delete = 0.
detection_flag = True
log_file = open(self.log_file_name, 'w')
log_file.write("Time,RobotOnChargingStation,BatteryCurrent\n")
log_detection = open(self.log_detection_file_name, 'w')
log_detection.write("TimeLostDetection,TimeFindDetection\n")
while self._flag_test:
if not self._cycling_stop_flag:
test_timer = self.timer.dcm_time() - time_delete
line_to_write = ",".join([
str(test_timer / 1000),
str(robot_on_charging_station.value),
str(battery_current.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
if detection_flag == True and \
robot_on_charging_station.value == 0:
print "Detection lost : " + str(test_timer / 1000)
detection_flag = False
log_detection.write(str(test_timer / 1000))
log_detection.flush()
if detection_flag == False and \
robot_on_charging_station.value == 1:
print "Detection found : " + str(test_timer / 1000)
detection_flag = True
log_detection.write("," + str(test_timer / 1000) + "\n")
log_detection.flush()
if self._cycling_stop_flag:
time_delete = self.timer.dcm_time() - test_timer
if detection_flag == False:
log_detection.write("," + str(test_timer / 1000) + "\n")
log_detection.flush()
print("\n\nRobot moved during " + str(test_timer / 1000) +
"seconds\n\n")
def __init__(self, dcm, mem):
"""
Initialisation de la class
"""
self.dcm = dcm
self.mem = mem
self.parameters = qha_tools.read_section("test_config.cfg",
"DockCyclingParameters")
self.wheel_motion = subdevice.WheelsMotion(
self.dcm, self.mem, float(self.parameters["speed"][0]))
self.robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
self.back_bumper_sensor = subdevice.Bumper(dcm, mem, "Back")
self.nb_cycles = self.parameters["nb_cycles"][0]
self.log_file = open(self.parameters["cycling_cvs_name"][0], 'w')
self.check_temperature_thread = threading.Thread(
target=self._check_temperature_thread, args=())
self.check_bumper_thread = threading.Thread(
target=self._check_bumper_thread, args=())
self.check_bumper = True
self.check_bumper_pause = True
self.check_temperature = None
self.wheel_hot = None
self.bumper_state = None
def test_damage(dcm, mem, kill_motion):
"""
Test robot docking/undocking to check damages
"""
time.sleep(10)
# Test parameters
parameters = qha_tools.read_section("test_pod.cfg",
"DockCyclingParameters")
# Objects creation
motion = subdevice.WheelsMotion(dcm, mem, float(parameters["speed"][0]))
robot_on_charging_station = subdevice.ChargingStationSensor(dcm, mem)
wheelfr_temperature_sensor = subdevice.WheelTemperatureSensor(
dcm, mem, "WheelFR")
wheelfl_temperature_sensor = subdevice.WheelTemperatureSensor(
dcm, mem, "WheelFL")
back_bumper_sensor = subdevice.Bumper(dcm, mem, "Back")
# Internal flags
cycles_done = 0
cycles_with_bumper_ok = 0
list_bumper_nok = []
unlock_bumper_status = 0
bumper_blocked_flag = False
detection = 1
loose_connexion_flag = 0
stop_cycling_flag = False
# Flag initialization
flag_detection = True
flag_bumper = True
flag_keep_connexion = True
timer = qha_tools.Timer(dcm, 10)
log_file = open(parameters["cycling_cvs_name"][0], 'w')
log_file.write(
"CyclesDone,CyclesDoneWithBumperOk," +
"Detection,LooseConnection,UnlockBumperStatus,LockBumperStatus\n")
plot_log = Plot(dcm, mem, parameters["easy_plot_csv_name"][0])
plot_log.start()
# Cyclage
# If the robot is not on the pod or bumper not activated, test don't start
if robot_on_charging_station.value == 0:
print "Put the robot on the pod\n"
stop_cycling_flag = True
flag_detection = False
flag_bumper = False
flag_keep_connexion = False
while stop_cycling_flag == False:
# Robot moves front
cycles_done += 1
motion.move_x(float(parameters["distance_front"][0]))
qha_tools.wait(dcm, int(parameters["time_wait_out_the_pod"][0]) * 1000)
unlock_bumper_status = back_bumper_sensor.value
# Verification of bumper
if back_bumper_sensor.value == 1:
bumper_blocked_flag = True
else:
bumper_blocked_flag = False
# Robot moves back
motion.move_x(float(parameters["distance_back"][0]))
motion.stiff_wheels(["WheelFR", "WheelFL", "WheelB"],
float(parameters["stiffness_wheels_value"][0]))
qha_tools.wait(
dcm,
float(parameters["time_wait_before_verify_detection"][0]) * 1000)
# Verification of connexion
t_init = timer.dcm_time()
test_time = 0
while robot_on_charging_station.value == 1 and\
test_time < int(parameters["time_wait_on_the_pod"][0]) * 1000:
detection = 1
loose_connexion_flag = 0
test_time = timer.dcm_time() - t_init
# If no detection
if test_time == 0:
detection = 0
# If connexion is lost
elif test_time < int(parameters["time_wait_on_the_pod"][0]) * 1000:
loose_connexion_flag = 1
flag_keep_connexion = False
# Verification of bumper
if back_bumper_sensor.value == 1 and bumper_blocked_flag == False:
cycles_with_bumper_ok += 1
else:
list_bumper_nok.append(cycles_done)
# Log data
line_to_write = ",".join([
str(cycles_done),
str(cycles_with_bumper_ok),
str(detection),
str(loose_connexion_flag),
str(unlock_bumper_status),
str(back_bumper_sensor.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
# Wait if temperature of wheels too hot
while wheelfr_temperature_sensor.value > \
int(parameters["wheels_temperature_limit"][0]) or\
wheelfl_temperature_sensor.value > \
int(parameters["wheels_temperature_limit"][0]):
qha_tools.wait(dcm, int(parameters["time_wait_wheels cooled"][0]))
# End if nb_cycles is reached
if cycles_done == int(parameters["nb_cycles"][0]):
stop_cycling_flag = True
if len(list_bumper_nok) > cycles_done / 100:
flag_bumper = False
log_file.close()
plot_log.stop()
print("Cycles done = " + str(cycles_done))
print("Cycles done with bumper ok = " + str(cycles_with_bumper_ok))
assert flag_detection
assert flag_bumper
assert flag_keep_connexion
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_pod_damage(dcm, mem, wake_up_pos, kill_motion, stiff_robot,
unstiff_parts):
"""
Test robot moving in the pod to check damages
"""
# Objects creation
robot_on_charging_station = subdevice.ChargingStationSensor(dcm, mem)
kneepitch_temperature = subdevice.JointTemperature(
dcm, mem, "KneePitch")
hippitch_temperature = subdevice.JointTemperature(
dcm, mem, "HipPitch")
hiproll_temperature = subdevice.JointTemperature(
dcm, mem, "HipRoll")
# Test parameters
parameters = qha_tools.read_section("test_pod.cfg", "DynamicTestParameters")
motion = subdevice.WheelsMotion(dcm, mem, 0.15)
motion.stiff_wheels(
["WheelFR", "WheelFL", "WheelB"],
int(parameters["stiffness_wheels_value"][0])
)
cycling_flag = 0
cycling_step = 0
# Going to initial position
subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True)
# Flag initialization
flag = True
# Use plot class
plot_log = Plot(dcm, mem, parameters["easy_plot_csv_name"][0])
plot_log.start()
timer = qha_tools.Timer(dcm, int(parameters["test_time"][0]) * 1000)
# Use detection during movement class
log_during_movement = DetectionDuringMovement(
dcm, mem, timer,
parameters["log_during_movement_csv_name"][0],
parameters["lost_detection_csv_name"][0]
)
log_during_movement.start()
if robot_on_charging_station.value == 0:
print "Put the robot on the robot.\nVerify detection.\n"
flag = False
while timer.is_time_not_out() and flag == True:
# Verify if robot moves
if cycling_step == 3:
log_during_movement.set_cycling_stop_flag_true()
if cycling_flag == 0:
if kneepitch_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "KneePitch cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
kneepitch_cycling(dcm, mem,
int(parameters["max_joint_temperature"][0]))
else:
cycling_step += 1
cycling_flag = 1
elif cycling_flag == 1:
if hippitch_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "HipPitch cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
hippitch_cycling(
dcm, mem,
int(parameters["max_joint_temperature"][0])
)
else:
cycling_step += 1
cycling_flag = 2
else:
if hiproll_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "HipRoll cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
hiproll_cycling(
dcm, mem,
int(parameters["max_joint_temperature"][0])
)
else:
cycling_step += 1
cycling_flag = 0
plot_log.stop()
log_during_movement.stop()
assert flag
def run(self):
""" Log """
robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
battery_current = subdevice.BatteryCurrentSensor(self.dcm, self.mem)
kneepitch_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "KneePitch")
hippitch_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "HipPitch")
hiproll_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "HipRoll")
kneepitch_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "KneePitch")
hippitch_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "HipPitch")
hiproll_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "HipRoll")
kneepitch_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "KneePitch")
hippitch_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "HipPitch")
hiproll_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "HipRoll")
log_file = open(self.file_name, 'w')
log_file.write(
"Time,Detection,Current,KneePitchPositionActuator," +
"KneePitchPositionSensor,KneePitchTemperature," +
"HipPitchPositionActuator,HipPitchPositionSensor," +
"HipPitchTemperature,HipRollPositionActuator," +
"HipRollPositionSensor,HipRollTemperature,WheelBStiffness," +
"WheelFRStiffness,WheelFLStiffness\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("KneePitchPositionActuator", elapsed_time,
kneepitch_position_actuator)
plot_server.add_point("KneePitchPositionSensor", elapsed_time,
kneepitch_position_sensor)
plot_server.add_point("KneePitchTemperature", elapsed_time,
kneepitch_temperature)
plot_server.add_point("HipPitchPositionActuator", elapsed_time,
hippitch_position_actuator)
plot_server.add_point("HipPitchPositionSensor", elapsed_time,
hippitch_position_sensor)
plot_server.add_point("HipPitchTemperature", elapsed_time,
hippitch_temperature)
plot_server.add_point("HipRollPositionActuator", elapsed_time,
hiproll_position_actuator)
plot_server.add_point("HipRollPositionSensor", elapsed_time,
hiproll_position_sensor)
plot_server.add_point("HipRollTemperature", elapsed_time,
hiproll_temperature)
line_to_write = ",".join([
str(elapsed_time),
str(robot_on_charging_station.value),
str(battery_current.value),
str(kneepitch_position_actuator.value),
str(kneepitch_position_sensor.value),
str(kneepitch_temperature.value),
str(hippitch_position_actuator.value),
str(hippitch_position_sensor.value),
str(hippitch_temperature.value),
str(hiproll_position_actuator.value),
str(hiproll_position_sensor.value),
str(hiproll_temperature.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
time.sleep(0.1)