def fin(): """Head capacitive sensor test teardown""" # going to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiff robot subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
def test_leg_enslavement(self, dcm, mem, zero_pos, test_leg_dico, test_time, test_limit): """ Test joint enslavement for HipPitch and KneePitch. Error must be lower than a fixed limit. """ # Objects creation hippitch_position_actuator = test_leg_dico["hipActuator"] hippitch_position_sensor = test_leg_dico["hipSensor"] kneepitch_position_actuator = test_leg_dico["kneeActuator"] kneepitch_position_sensor = test_leg_dico["kneeSensor"] logger = test_leg_dico["logger"] # Flag initialization logger.flag = True # Going to initial position subdevice.multiple_set(dcm, mem, zero_pos, wait=True) # Behavior of the tested part hippitch_position_actuator.explore(2 * test_time) kneepitch_position_actuator.explore(2 * test_time, max_to_min=False) # Timer creation just before test loop timer = qha_tools.Timer(dcm, 4 * test_time) # Test loop while timer.is_time_not_out(): hip_act_rad = hippitch_position_actuator.value hip_act_deg = math.degrees(hip_act_rad) hip_sensor_rad = hippitch_position_sensor.value hip_sensor_deg = math.degrees(hip_sensor_rad) knee_act_rad = kneepitch_position_actuator.value knee_act_deg = math.degrees(knee_act_rad) knee_sensor_rad = kneepitch_position_sensor.value knee_sensor_deg = math.degrees(knee_sensor_rad) error_hip = hip_act_deg - hip_sensor_deg error_knee = knee_act_deg - knee_sensor_deg logger.log(("Time", timer.dcm_time() / 1000.), ("HipPitchActuator", hip_act_deg), ("HipPitchSensor", hip_sensor_deg), ("ErrorHip", error_hip), ("KneePitchActuator", knee_act_deg), ("KneePitchSensor", knee_sensor_deg), ("ErrorKnee", error_knee), ("Eps", test_limit), ("-Eps", test_limit * -1.), ("HipPitchActuator+Eps", hip_act_deg + test_limit), ("HipPitchActuator-Eps", hip_act_deg - test_limit), ("KneePitchActuator+Eps", knee_act_deg + test_limit), ("KneePitchActuator-Eps", knee_act_deg - test_limit)) if abs(error_hip) > test_limit or abs(error_knee) > test_limit: logger.flag = False assert logger.flag
def test_joint_enslavement(self, dcm, mem, zero_pos, test_objects_dico, test_time): """ Test joint enslavement. Error must be lower than a fixed limit. @type dcm : object @param dcm : proxy to DCM @type mem : object @param mem : proxy to ALMemory @type zero_pos : dictionnary @param zero_pos : dictionnary {"ALMemory key":value} from config file. @type joint : string @param joint : string describing the current joint. @type test_time : integer @param test_time : time to wait to test a joint enslavement [ms]. """ # Objects creation joint_position_actuator = test_objects_dico["jointActuator"] joint_position_sensor = test_objects_dico["jointSensor"] logger = test_objects_dico["logger"] test_limit = test_objects_dico["joint_test_limit"] # Flag initialization logger.flag = True # Going to initial position subdevice.multiple_set(dcm, mem, zero_pos, wait=True) # Going to min position to have a complete exploration joint_position_actuator.go_to(dcm, "min") # Behavior of the tested part joint_position_actuator.explore(2 * test_time) # Timer creation just before test loop timer = qha_tools.Timer(dcm, 4 * test_time) # Test loop while timer.is_time_not_out(): error = joint_position_actuator.value - joint_position_sensor.value actuator_degrees = math.degrees(joint_position_actuator.value) logger.log(("Time", timer.dcm_time() / 1000.), ("Actuator", actuator_degrees), ("Sensor", math.degrees(joint_position_sensor.value)), ("Error", math.degrees(error)), ("Eps", test_limit), ("-Eps", test_limit * -1.), ("Actuator+Eps", actuator_degrees + test_limit), ("Actuator-Eps", actuator_degrees - test_limit)) if abs(error) > math.radians(test_limit): logger.flag = False assert logger.flag
def fin(): """Hands capacitive sensor test teardown""" # going to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiff robot subdevice.multiple_set(dcm, mem, stiffness_off, wait=True) # setting max to their initial values lwristyaw.position.actuator.maximum = \ [[[lwristyaw_initial_max, dcm.getTime(0)]], "Merge"] rwristyaw.position.actuator.maximum = \ [[[rwristyaw_initial_max, dcm.getTime(0)]], "Merge"]
def wake_up_pos_brakes_closed(request, dcm, mem, wake_up_pos, rest_pos, kill_motion, stiff_robot): """ Fixture which make the robot wakeUp, close brakes. Control if brakes slip. """ subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True) thread_flag = threading.Event() def control(): """ Control if brakes slip""" kneepitch_position_actuator = subdevice.JointPositionActuator( dcm, mem, "KneePitch") kneepitch_position_sensor = subdevice.JointPositionSensor( dcm, mem, "KneePitch") kneepitch_hardness_actuator = subdevice.JointHardnessActuator( dcm, mem, "KneePitch") hippitch_position_actuator = subdevice.JointPositionActuator( dcm, mem, "HipPitch") hippitch_position_sensor = subdevice.JointPositionSensor( dcm, mem, "HipPitch") hippitch_hardness_actuator = subdevice.JointHardnessActuator( dcm, mem, "HipPitch") hippitch_hardness_actuator.qqvalue = 0. kneepitch_hardness_actuator.qqvalue = 0. while not thread_flag.is_set(): if abs(hippitch_position_sensor.value) > 0.1 or\ abs(kneepitch_position_sensor.value) > 0.1: hippitch_hardness_actuator.qqvalue = 1. kneepitch_hardness_actuator.qqvalue = 1. hippitch_position_actuator.qvalue = (0., 1000) kneepitch_position_actuator.qvalue = (0., 1000) tools.wait(dcm, 2100) hippitch_hardness_actuator.qqvalue = 0. kneepitch_hardness_actuator.qqvalue = 0. my_thread = threading.Thread(target=control) my_thread.start() def fin(): """ Stop control and put the robot in rest position at the end of session" """ thread_flag.set() subdevice.multiple_set(dcm, mem, rest_pos, wait=True) request.addfinalizer(fin)
def st_head(request, kill_motion, dcm, mem, stiffness_on, wake_up_pos, rest_pos, stiffness_off): """Test Setup and TearDown for head capacitive sensor false positive""" # stiffing body subdevice.multiple_set(dcm, mem, stiffness_on, wait=True) # going to zero position subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True) def fin(): """Head capacitive sensor test teardown""" # going to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiff robot subdevice.multiple_set(dcm, mem, stiffness_off, wait=True) request.addfinalizer(fin)
def st_hands(request, kill_motion, dcm, mem, stiffness_on, zero_pos, rest_pos, stiffness_off, rwristyaw, lwristyaw): """Test Setup and TearDown for hands capacitive sensor false positive""" # stiffing body subdevice.multiple_set(dcm, mem, stiffness_on, wait=True) # going to zero position subdevice.multiple_set(dcm, mem, zero_pos, wait=True) # defining initial max lwristyaw_initial_max = lwristyaw.position.actuator.maximum rwristyaw_initial_max = rwristyaw.position.actuator.maximum # new max position actuator out of real mechanical stop lwristyaw_new_max = lwristyaw_initial_max + radians(10.0) rwristyaw_new_max = rwristyaw_initial_max + radians(10.0) # setting new max lwristyaw.position.actuator.maximum = \ [[[lwristyaw_new_max, dcm.getTime(0)]], "Merge"] rwristyaw.position.actuator.maximum = \ [[[rwristyaw_new_max, dcm.getTime(0)]], "Merge"] # going to new max position lwristyaw.position.actuator.qvalue = (lwristyaw_new_max, 4000) rwristyaw.position.actuator.qvalue = (rwristyaw_new_max, 4000) def fin(): """Hands capacitive sensor test teardown""" # going to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiff robot subdevice.multiple_set(dcm, mem, stiffness_off, wait=True) # setting max to their initial values lwristyaw.position.actuator.maximum = \ [[[lwristyaw_initial_max, dcm.getTime(0)]], "Merge"] rwristyaw.position.actuator.maximum = \ [[[rwristyaw_initial_max, dcm.getTime(0)]], "Merge"] request.addfinalizer(fin)
def fin(): """Wait between two tests.""" subdevice.multiple_set(dcm, mem, stiffness_on, wait=True) subdevice.multiple_set(dcm, mem, rest_pos, wait=True) subdevice.multiple_set(dcm, mem, stiffness_off, wait=True) logging.info("Waiting " + str(time_to_wait) + " s") time.sleep(float(time_to_wait))
def test_joint_current_limitation(self, dcm, mem, parameters, joint, result_base_folder, rest_pos, stiffness_off, plot, plot_server): # logger initialization log = logging.getLogger('test_joint_current_limitation') # erasing real time plot plot_server.curves_erase() # flags initialization flag_loop = True # test loop stops when this flag is False flag_joint = True # giving test result flag_current_limit_low_exceeded = False # checking that ALMemory key MinMaxChange Allowed = 1 if int(mem.getData("RobotConfig/Head/MinMaxChangeAllowed")) != 1: flag_loop = False log.error("MinMaxChangeAllowed ALMemory key missing") # test parameters test_params = parameters joint_position_actuator = joint.position.actuator joint_position_sensor = joint.position.sensor joint_temperature_sensor = joint.temperature joint_current_sensor = joint.current slav = qha_tools.SlidingAverage(test_params["sa_nb_points"]) logger = qha_tools.Logger() # Going to initial position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiffing all the other joints to avoid leg motors overheat subdevice.multiple_set(dcm, mem, stiffness_off, wait=False) time.sleep(0.1) # stiffing the joint we want to test joint.hardness.qqvalue = 1.0 # keeping initial joint min and max joint_initial_maximum = joint_position_actuator.maximum joint_initial_minimum = joint_position_actuator.minimum # put joint to its initial maximum in 3 seconds if joint_position_actuator.short_name in\ ("HipPitch", "RShoulderRoll", "RElbowRoll"): joint_position_actuator.qvalue = (joint_initial_minimum, 3000) else: joint_position_actuator.qvalue = (joint_initial_maximum, 3000) qha_tools.wait(dcm, 3000) # setting current limitations joint_max_current = joint_current_sensor.maximum joint_min_current = joint_current_sensor.minimum delta_current = joint_max_current - joint_min_current k_sup = test_params["limit_factor_sup"] k_inf = test_params["limit_factor_inf"] current_limit_high = joint_max_current + k_sup * delta_current current_limit_low = joint_max_current - k_inf * delta_current # setting temperature limitatons joint_temperature_min = joint_temperature_sensor.minimum # setting new min and max out of the mechanical stop joint_new_maximum = \ joint_initial_maximum + \ math.radians(test_params["limit_extension"]) joint_new_minimum = \ joint_initial_minimum - \ math.radians(test_params["limit_extension"]) joint_position_actuator.maximum = [[[ joint_new_maximum, dcm.getTime(0) ]], "Merge"] joint_position_actuator.minimum = [[[ joint_new_minimum, dcm.getTime(0) ]], "Merge"] if joint_position_actuator.short_name in\ ("HipPitch", "RShoulderRoll", "RElbowRoll"): joint_position_actuator.qvalue = (joint_new_minimum, 1000) else: joint_position_actuator.qvalue = (joint_new_maximum, 1000) timer = qha_tools.Timer(dcm, test_params["test_time"]) timer_limit = qha_tools.Timer(dcm, test_params["test_time_limit"]) # test loop while flag_loop and timer.is_time_not_out(): try: loop_time = timer.dcm_time() / 1000. joint_temperature = joint_temperature_sensor.value joint_current = joint_current_sensor.value slav.point_add(joint_current) joint_current_sa = slav.calc() if not flag_current_limit_low_exceeded and\ joint_current_sa > current_limit_low: flag_current_limit_low_exceeded = True log.info("Current limit low exceeded") if joint_current_sa > current_limit_high: flag_joint = False log.warning("Current limit high overshoot") if flag_current_limit_low_exceeded and\ joint_current_sa < current_limit_low: flag_joint = False log.warning("Current lower than current limit low") if timer_limit.is_time_out() and not \ flag_current_limit_low_exceeded: flag_joint = False log.info("Timer limit finished and "+\ "current limit low not exceeded") # out of test loop if temperature is higher than min temperature if joint_temperature >= joint_temperature_min: flag_loop = False log.info("Temperature higher than Min temperature") log.info("Test finished") logger.log(("Time", timer.dcm_time() / 1000.), ("Current", joint_current), ("CurrentSA", joint_current_sa), ("CurrentLimitHigh", current_limit_high), ("CurrentLimitLow", current_limit_low), ("MaxAllowedCurrent", joint_max_current), ("Temperature", joint_temperature), ("TemperatureMin", joint_temperature_min), ("Command", joint_position_actuator.value), ("Position", joint_position_sensor.value)) if plot: plot_server.add_point("Current", loop_time, joint_current) plot_server.add_point("CurrentSA", loop_time, joint_current_sa) plot_server.add_point("CurrentLimitHigh", loop_time, current_limit_high) plot_server.add_point("CurrentLimitLow", loop_time, current_limit_low) plot_server.add_point("MaxAllowedCurrent", loop_time, joint_max_current) plot_server.add_point("Temperature", loop_time, joint_temperature) plot_server.add_point("TemperatureMin", loop_time, joint_temperature_min) except KeyboardInterrupt: flag_loop = False # out of test loop log.info("KeyboardInterrupt from user") log.info("OUT OF TEST LOOP") result_file_path = "/".join([ result_base_folder, joint_position_actuator.subdevice_type, joint_position_actuator.short_name + "_" + str(flag_joint) ]) + ".csv" logger.log_file_write(result_file_path) joint_position_actuator.maximum = [[[ joint_initial_maximum, dcm.getTime(0) ]], "Merge"] joint_position_actuator.minimum = [[[ joint_initial_minimum, dcm.getTime(0) ]], "Merge"] plot_server.curves_erase() assert flag_joint assert flag_current_limit_low_exceeded
def main(): """Main function""" parser = argparse.ArgumentParser(description="Log datas from ALMemory") parser.add_argument(dest="robot_ip", help="Robot IP or name") args = parser.parse_args() general = qha_tools.read_section("slip.cfg", "general") robot_ip = args.robot_ip result_folder = general["resultsFolder"][0] # Create result folder if not existing try: os.mkdir(result_folder) except OSError: pass dcm = ALProxy("DCM", robot_ip, 9559) mem = ALProxy("ALMemory", robot_ip, 9559) # Get Body ID body_id = mem.getData("Device/DeviceList/ChestBoard/BodyId") # Kill motion (if not already done) try: mot = ALProxy("ALMotion", robot_ip, 9559) mot.exit() except RuntimeError: pass print "[I] ALMotion is now killed" # Ask the operator to write his name name = raw_input("Please enter your name and press Enter : ") while name == "": name = raw_input("Please enter your name and press Enter : ") nb_of_trials = int(general["numberOfTrials"][0]) stiffness_on = qha_tools.read_section("slip.cfg", "stiffnessOnWithoutWheels") stiffness_off = qha_tools.read_section("slip.cfg", "stiffnessOff") rest_pos = qha_tools.read_section("slip.cfg", "restPosition") force_list = [] for i in range(nb_of_trials): # Set Stiffness on subdevice.multiple_set(dcm, mem, stiffness_on) # Go to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) time.sleep(1) # Set Stiffness off subdevice.multiple_set(dcm, mem, stiffness_off) if (nb_of_trials - i - 1) <= 1: trial_string = "trial" else: trial_string = "trials" force = raw_input("Please push the robot, write the force (in Newton) " "and press Enter. (" + str(nb_of_trials - i - 1) + " " + trial_string + " left) : ") while not is_number(force): force = raw_input("Please push the robot, write the force" "(in Newton) and press Enter. (" + str(nb_of_trials - i - 1) + " " + trial_string + " left) : ") force_list.append(force) print("Thank you !") # At the end of the test, put the robot in initial position # Set Stiffness on subdevice.multiple_set(dcm, mem, stiffness_on) # Go to rest position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) time.sleep(1) # Set Stiffness off subdevice.multiple_set(dcm, mem, stiffness_off) # Write results file file_path = result_folder + "/" + body_id + ".dat" my_file = open(file_path, 'a') string_to_print = name + " " + " ".join(force_list) + "\n" my_file.write(string_to_print)
def fin(): """ Stop control and put the robot in rest position at the end of session" """ thread_flag.set() subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
def fin(): """Method automatically executed at the end of the test.""" dcm.set(["Hardness", "Merge", [[1.0, dcm.getTime(0)]]]) subdevice.multiple_set(dcm, mem, rest_pos, wait=True) dcm.set(["Hardness", "Merge", [[0.1, dcm.getTime(0)]]]) dcm.set(["Hardness", "Merge", [[0., dcm.getTime(1000)]]])
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 test_joint_temperature_protection(self, dcm, mem, parameters, joint, result_base_folder, rest_pos, stiffness_off, plot_server, plot): # logger initialization log = logging.getLogger('MOTOR_LIMITATION_PERF_HW_002') # flags initialization flag_joint = True flag_loop = True flag_max_current_exceeded = False flag_max_temperature_exceeded = False flag_low_limit = False flag_info = False flag_info2 = False flag_info3 = False flag_info4 = False # erasing real time curves if plot: plot_server.curves_erase() # Objects creation test_params = parameters joint_position_actuator = joint.position.actuator joint_position_sensor = joint.position.sensor joint_temperature_sensor = joint.temperature joint_hardness_actuator = joint.hardness joint_current_sensor = joint.current slav = qha_tools.SlidingAverage(test_params["sa_nb_points"]) logger = qha_tools.Logger() log.info("") log.info("*************************************") log.info("Testing : " + str(joint.short_name)) log.info("*************************************") log.info("") # Knowing the board, we can know if the motor is a MCC or DC Brushless joint_board = joint_position_actuator.device # Creating device object to acceed to its error joint_board_object = device.Device(dcm, mem, joint_board) # Going to initial position subdevice.multiple_set(dcm, mem, rest_pos, wait=True) # unstiffing all the other joints to avoid leg motors overheat subdevice.multiple_set(dcm, mem, stiffness_off, wait=False) time.sleep(0.1) # stiffing the joint we want to test joint.hardness.qqvalue = 1.0 # keeping initial joint min and max joint_initial_maximum = joint_position_actuator.maximum joint_initial_minimum = joint_position_actuator.minimum # setting current limitations joint_current_max = joint_current_sensor.maximum # setting temperature limitatons joint_temperature_min = joint_temperature_sensor.minimum joint_temperature_max = joint_temperature_sensor.maximum delta_temperature = joint_temperature_max - joint_temperature_min # setting new min and max out of the mechanical stop if joint_initial_maximum >= 0.0: joint_new_maximum = \ joint_initial_maximum + \ math.radians(test_params["limit_extension"]) else: joint_new_maximum = \ joint_initial_maximum - \ math.radians(test_params["limit_extension"]) if joint_initial_minimum <= 0.0: joint_new_minimum = \ joint_initial_minimum - \ math.radians(test_params["limit_extension"]) else: joint_new_minimum = \ joint_initial_minimum + \ math.radians(test_params["limit_extension"]) joint_position_actuator.maximum = [[[ joint_new_maximum, dcm.getTime(0) ]], "Merge"] joint_position_actuator.minimum = [[[ joint_new_minimum, dcm.getTime(0) ]], "Merge"] # set timer limit timer_limit = qha_tools.Timer(dcm, test_params["test_time_limit"]) # for Brushless motors, max test time is 60 seconds brushless_motors = ("KneePitch", "HipPitch", "HipRoll") if joint_position_actuator.short_name in brushless_motors: timer = qha_tools.Timer(dcm, 60000) else: timer = qha_tools.Timer(dcm, test_params["test_time"]) # set position actuator out of the joint mechanical stop # going out of physical mechanical stop in 5 seconds if joint_position_actuator.short_name in \ ("HipPitch", "RShoulderRoll", "HipRoll"): joint_position_actuator.qvalue = (joint_new_minimum, 5000) else: joint_position_actuator.qvalue = (joint_new_maximum, 5000) flag_first_iteration = True timer_current_decrease = qha_tools.Timer(dcm, 100) while flag_loop is True and timer.is_time_not_out(): try: joint_temperature = joint_temperature_sensor.value joint_current = joint_current_sensor.value slav.point_add(joint_current) joint_position_command = joint_position_actuator.value joint_position = joint_position_sensor.value joint_hardness_value = joint_hardness_actuator.value joint_current_sa = slav.calc() firmware_error = joint_board_object.error dcm_time = timer.dcm_time() / 1000. # Max current adaptation if joint temperature higher than Min # No lower current for DC Brushless motors if joint_board not in \ ("HipBoard", "ThighBoard", "BackPlatformBoard"): if joint_temperature > joint_temperature_min: delta_max = joint_temperature_max - joint_temperature max_allowed_current = ( (delta_max) / (delta_temperature)) *\ joint_current_max else: max_allowed_current = joint_current_max else: max_allowed_current = joint_current_max # max allowed current can not be lower than 0. if max_allowed_current < 0.0: max_allowed_current = 0.0 # defining old max current as first calculated max current if # it is the first loop iteration if flag_first_iteration is True: old_mac = max_allowed_current flag_first_iteration = False # defining current regulation limits current_limit_high = max_allowed_current * \ (1.0 + test_params["limit_factor_sup"]) current_limit_low = max_allowed_current *\ (1.0 - test_params["limit_factor_inf"]) # setting flag to True if 90 percent of max current is exceeded if joint_current_sa > 0.9 * max_allowed_current and not\ flag_max_current_exceeded: flag_max_current_exceeded = True log.info("90 percent of max allowed currend reached") if max_allowed_current != old_mac: timer_current_decrease = qha_tools.Timer(dcm, 100) # averaged current has not to exceed limit high if joint_current_sa > current_limit_high and \ timer_current_decrease.is_time_out() and not flag_info4: flag_joint = False flag_info4 = True log.warning("current high limit exceeded") # once max current is exceeded, current hasn't to be lower than # limit low if flag_max_current_exceeded and \ joint_current_sa < current_limit_low and not \ flag_max_temperature_exceeded and not flag_low_limit: flag_joint = False flag_low_limit = True log.info("current has been lower than low limit") # after time limit, current has to have exceeded max current # if it has not, it is written on time in log file if timer_limit.is_time_out() and not\ flag_max_current_exceeded and flag_info is False: flag_joint = False flag_info = True log.info("current has not exceeded 90 percent of max "+\ "allowed current") # hardware protection if joint_temperature >= joint_temperature_max + 1: flag_loop = False log.warning("temperature too high (max+1 degree)") # if joint temperature higher than a limit value, # joint current must be null after 100ms. if flag_max_temperature_exceeded is False and \ joint_temperature >= joint_temperature_max: flag_max_temperature_exceeded = True timer_max = qha_tools.Timer(dcm, 100) log.info("max temperature exceeded a first time") if flag_max_temperature_exceeded and\ timer_max.is_time_out() and joint_current != 0 and not\ flag_info2: flag_joint = False flag_info2 = True log.critical("max temperature exceeded and current "+\ "is not null") if flag_max_temperature_exceeded and joint_current == 0.0 and\ not flag_info3: flag_info3 = True log.info("current null reached") old_mac = max_allowed_current logger.log( ("Time", dcm_time), ("Hardness", joint_hardness_value), ("Current", joint_current), ("CurrentSA", joint_current_sa), ("MaxAllowedCurrent", max_allowed_current), ("CurrentLimitHigh", current_limit_high), ("CurrentLimitLow", current_limit_low), ("Temperature", joint_temperature), ("TemperatureMin", joint_temperature_min), ("TemperatureMax", joint_temperature_max), ("Command", joint_position_command), ("Position", joint_position), ("FWError", firmware_error)) # for real time plot if plot: plot_server.add_point("Hardness", dcm_time, joint_hardness_value) plot_server.add_point("CurrentSA", dcm_time, joint_current_sa) plot_server.add_point("MaxAllowedCurrent", dcm_time, max_allowed_current) plot_server.add_point("CurrentLimitHigh", dcm_time, current_limit_high) plot_server.add_point("CurrentLimitLow", dcm_time, current_limit_low) plot_server.add_point("Temperature", dcm_time, joint_temperature) plot_server.add_point("TemperatureMin", dcm_time, joint_temperature_min) plot_server.add_point("TemperatureMax", dcm_time, joint_temperature_max) plot_server.add_point("Command", dcm_time, joint_position_command) plot_server.add_point("Position", dcm_time, joint_position) except KeyboardInterrupt: flag_loop = False log.info("KeyboardInterrupt from user") # writing logger results into a csv file result_file_path = "/".join([ result_base_folder, joint_position_actuator.subdevice_type, joint_position_actuator.short_name + "_" + str(flag_joint) ]) + ".csv" logger.log_file_write(result_file_path) # seting min and max joint software limits to their original values joint_position_actuator.maximum = [[[ joint_initial_maximum, dcm.getTime(0) ]], "Merge"] joint_position_actuator.minimum = [[[ joint_initial_minimum, dcm.getTime(0) ]], "Merge"] assert flag_joint