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.
def check_wheel_temperatures(dcm, leds, list_wheel_temperature, list_wheel_speed, list_wheel_stiffness): """ Pauses the robot if the wheels are too hot """ if list_wheel_temperature[0].value >= 80 or \ list_wheel_temperature[1].value >= 80 or \ list_wheel_temperature[2].value >= 80: print("\nWheels too hot --> Waiting") leds.fadeListRGB("shoulder_group", [ 0x00FF00FF, ], [ 0., ]) stop_robot(list_wheel_speed) unstiff_wheels(list_wheel_stiffness) # Wait while list_wheel_temperature[0].value >= 45 or \ list_wheel_temperature[1].value >= 45 or \ list_wheel_temperature[2].value >= 45: tools.wait(dcm, 20000) leds.reset("shoulder_group") stiff_wheels(list_wheel_stiffness)
def check_wheel_temperatures(dcm, leds, list_wheel_temperature, list_wheel_speed, list_wheel_stiffness): """ Pauses the robot if the wheels are too hot """ parameters_cables = tools.read_section("config.cfg", "CablesRoutingParameters") Temp_max = int(parameters_cables["Temp_max"][0]) Temp_min = int(parameters_cables["Temp_min"][0]) if list_wheel_temperature[0].value >= Temp_max or \ list_wheel_temperature[1].value >= Temp_max or \ list_wheel_temperature[2].value >= Temp_max: print("\nWheels too hot --> Waiting") leds.fadeListRGB("shoulder_group", [ 0x00FF00FF, ], [ 0., ]) stop_robot(list_wheel_speed) unstiff_wheels(list_wheel_stiffness) # Wait while list_wheel_temperature[0].value >= Temp_min or \ list_wheel_temperature[1].value >= Temp_min or \ list_wheel_temperature[2].value >= Temp_min: tools.wait(dcm, 20000) leds.reset("shoulder_group") stiff_wheels(list_wheel_stiffness)
def unstiff_joints(dcm, mem, wait_time): """ Unstiff all joints except HipPitch, KneePitch and Wheels """ joints = tools.use_section("config.cfg", "JulietteJoints") for joint in joints: joint_hardness = subdevice.JointHardnessActuator(dcm, mem, joint) joint_hardness.qqvalue = 0.0 tools.wait(dcm, wait_time)
def _msg_send_timer(self, *arg, **kw): if len(self.msg_buff_list) > 0: msg_list = self.msg_buff_list.copy() self.msg_buff_list.clear() for msg in msg_list: while len(msg) != 0: send_msg = msg[:900] msg = msg[900:] self._send_msg(send_msg) tools.wait(5)
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 run(self): bumper_right = subdevice.Bumper(self.dcm, self.mem, "FrontRight") bumper_left = subdevice.Bumper(self.dcm, self.mem, "FrontLeft") bumper_back = subdevice.Bumper(self.dcm, self.mem, "Back") list_bumpers = [bumper_right, bumper_left, bumper_back] parameters = tools.read_section("config.cfg", "BumpersActivationsParameters") # If file already exists, reading to know the previous state if os.path.exists(parameters["Log_file_name"][0]): data = open(parameters["Log_file_name"][0], 'r') csv_reader = csv.reader(data) variables_list = [] row_num = 0 for row in csv_reader: data_num = 0 if row_num == 0: for value in row: variables_list.append([value]) else: for value in row: variables_list[data_num].append(value) data_num += 1 row_num += 1 self._bumpers_activations = int(variables_list[0][row_num - 1]) previous_time = float(variables_list[1][row_num - 1]) data.close() data = open(parameters["Log_file_name"][0], 'a') # Else initialize to 0 else: self._bumpers_activations = 0 previous_time = 0. data = open(parameters["Log_file_name"][0], 'w') data.write("Bumpers activations,Time\n0,0\n") data.flush() time_init = time.time() # Loop while not self._stop.is_set(): for bumper in list_bumpers: if bumper.value == 1: self._bumpers_activations += 1 data.write(str(self._bumpers_activations) + "," + \ str(time.time() - time_init + previous_time) + "\n") print("nb of bumpers activations: " + str(self._bumpers_activations) + "\n") data.flush() tools.wait(self.dcm, 2 * self.wait_time_bumpers)
def run(protocol, csvfile): tools.seed_random(23) node_count = 50 state = topology.create_nodes(node_count) mobility.randomize_positions(state, xy_range=1000) mobility.connect_range(state, max_links=150) # create network and start routing software network.apply(state=state, link_command=get_tc_command) software.start(protocol) tools.sleep(30) for step_distance in [50, 100, 150, 200, 250, 300, 350, 400]: print(f'{protocol}: step_distance {step_distance}') traffic_beg = tools.traffic() for n in range(0, 6): #with open(f'graph-{step_distance}-{n}.json', 'w+') as file: # json.dump(state, file, indent=' ') # connect nodes range wait_beg_ms = tools.millis() # update network representation mobility.move_random(state, distance=step_distance) mobility.connect_range(state, max_links=150) # update network network.apply(state=state, link_command=get_tc_command) # Wait until wait seconds are over, else error tools.wait(wait_beg_ms, 10) paths = tools.get_random_paths(state, 2 * 200) paths = tools.filter_paths(state, paths, min_hops=2, path_count=200) ping_result = tools.ping_paths(paths=paths, duration_ms=2000, verbosity='verbose') packets_arrived_pc = 100 * (ping_result.received / ping_result.send) traffic_end = tools.traffic() # add data to csv file extra = (['node_count', 'time_ms', 'step_distance_m', 'n', 'packets_arrived_pc'], [node_count, tools.millis() - wait_beg_ms, step_distance, n, packets_arrived_pc]) tools.csv_update(csvfile, '\t', extra, (traffic_end - traffic_beg).getData(), ping_result.getData()) traffic_beg = traffic_end software.clear() network.clear()
def verify_written_max(self, out): success, error_counter, attempts = False, 0, 5 while not success: maxs = self.pull_maxs() success = self.__successful_update(out, maxs) if success: logging.debug("%s successfully written" % out) return True else: logging.debug( "run check again. written are %s (%s) and out %s" % (set(maxs), maxs, set([out]))) error_counter += 1 wait(self.options["update_effect_time"]) if error_counter >= attempts: logging.warning("Unsuccessful write check (%s attempts)" % attempts) return False return True
def log(threading_flag): while not threading_flag.is_set(): line = "" flag = 0 speed_fr = wheel_fr_speed_sensor.value speed_fl = wheel_fl_speed_sensor.value speed_b = wheel_b_speed_sensor.value for bumper in list_bumpers: if bumper.value == 1: flag += 1 line += str(1) + "," else: line += str(0) + "," if flag > 0: line += str(speed_fr) + "," + \ str(speed_fl) + "," + \ str(speed_b) + "\n" data.write(line) data.flush() tools.wait(dcm, wait_time_bumpers)
def control_forever(self): last_successful_update, warnings_flag = None, False while True: try: last_successful_update, warnings_flag = last_update_verification( last_successful_update, warnings_flag, self) if self._do_basic_control_cycle(): logging.debug("Successful control cycle") last_successful_update, warnings_flag = utcnow(), True wait(self.options["refresh_period"]) else: logging.warning("Unsuccesful control cycle") wait(self.options["refresh_period"]) except: msg = "Unexpected error: %s" % str(sys.exc_info()) logging.critical(msg) wait(self.options["refresh_period"]) logging.critical("CONTROL SCRIPT IS EXITING ABNORMALLY")
def do_control_cycle(self): wait(self.options["refresh_period"]) return self._do_basic_control_cycle()
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 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