def move_robot(motion): """ Move robot with various speed (minimal, nominal and maximal) """ parameters_direction = tools.read_section("config.cfg", "DirectionParameters") distance = int(parameters_direction["distance"][0]) parameters_speed = tools.read_section("config.cfg", "Speed") min = int(parameters_speed["min_speed"][0]) nom = int(parameters_speed["nom_speed"][0]) max = int(parameters_speed["max_speed"][0]) question_continue('start') if min == 1: print 'minimal velocity' motion.moveToward(0.05, 0.0, 0.0) time.sleep(6.0) motion.stopMove() if nom == 1: question_continue('nominal velocity') print 'nomimal velocity' motion.moveToward(0.3, 0.0, 0.0) time.sleep(6.0) motion.stopMove() if max == 1: question_continue('maximal velocity') print 'maximal velocity' motion.moveToward(1.0, 0.0, 0.0) time.sleep(5.0) motion.stopMove() motion.rest()
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 zero_pos(): """ Fixture which retrieves zero joints position from a configuration file. """ return tools.read_section( PATH + "/global_test_configuration/" "juliette_positions.cfg", "zero")
def run(self): """Log cables crossing""" gyro_x = subdevice.InertialSensorBase(self.dcm, self.mem, "GyroscopeX") gyro_y = subdevice.InertialSensorBase(self.dcm, self.mem, "GyroscopeY") parameters = tools.read_section("config.cfg", "CablesRoutingParameters") # 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._cables_crossing = 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._cables_crossing = 0 previous_time = 0. data = open(parameters["Log_file_name"][0], 'w') data.write("Cable Crossing,Time\n0,0\n") data.flush() time_init = time.time() while not self._stop.is_set(): if gyro_x.value < \ float(parameters["Minimum_CableDetection"][0]) or \ gyro_x.value > \ float(parameters["Maximum_CableDetection"][0]) or\ gyro_y.value < \ float(parameters["Minimum_CableDetection"][0]) or \ gyro_y.value > \ float(parameters["Maximum_CableDetection"][0]): self._cables_crossing += 1 data.write(str(self._cables_crossing) + "," +\ str(time.time() - time_init + previous_time) + "\n") print("\nnb of cables crossing: " + str(self._cables_crossing)) data.flush() time.sleep(02) time.sleep(0.01)
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 move_robot(motion): parameters_rotation = tools.read_section("config.cfg", "Rotation") nb_tour = int(parameters_rotation['nb_tour'][0]) nb_test = int(parameters_rotation['nb_test'][0]) question_continue('start') for test in range(0, nb_test): motion.moveTo(0, 0, math.pi * (nb_tour * 2)) question_continue('reverse') motion.moveTo(0, 0, -math.pi * (nb_tour * 2)) test += 1 question_continue('restart') motion.rest()
def move_robot(motion, mem): """ Move Robot and stop when a fall id detected """ parameters_slope = tools.read_section("config.cfg", "Stop_Slope") distance = float(parameters_slope["distance"][0]) dir_x = int(parameters_slope["dir_X"][0]) dir_y = int(parameters_slope["dir_Y"][0]) dir_diag_l = int(parameters_slope["dir_diagL"][0]) dir_diag_r = int(parameters_slope["dir_diagR"][0]) stiffness = int(parameters_slope["stiff_ON"][0]) wait = int(parameters_slope["wait_time"][0]) question_continue('start') if dir_x == 1: question_continue('forward') motion.moveTo(distance, 0, 0) slope_detection(motion, stiffness, mem, wait) question_continue('behind') motion.moveTo(-distance, 0, 0) slope_detection(motion, stiffness, mem, wait) if dir_y == 1: question_continue('left') motion.moveTo(0, distance, 0) slope_detection(motion, stiffness, mem, wait) question_continue('rigth') motion.moveTo(0, -distance, 0) slope_detection(motion, stiffness, mem, wait) if dir_diag_l == 1: question_continue('diagonale front left') motion.moveTo(distance, distance, 0) slope_detection(motion, stiffness, mem, wait) question_continue('diagonale back left') motion.moveTo(-distance, -distance, 0) slope_detection(motion, stiffness, mem, wait) if dir_diag_r == 1: question_continue('diagonale front right') motion.moveTo(distance, -distance, 0) slope_detection(motion, stiffness, mem, wait) question_continue('diagonale back right') motion.moveTo(-distance, distance, 0) slope_detection(motion, stiffness, mem, wait) motion.rest()
def move_robot(motion): """ Move robot on abstacles:- rubber band - string - clips - news paper - dust """ parameters_direction = tools.read_section("config.cfg", "Obstacle") distance = float(parameters_direction["distance"][0]) nb_passage = int(parameters_direction["nb_passage"][0]) dir_x = int(parameters_direction["dir_X"][0]) dir_y = int(parameters_direction["dir_Y"][0]) dir_diag_l = int(parameters_direction["dir_diagL"][0]) dir_diag_r = int(parameters_direction["dir_diagR"][0]) rotate = int(parameters_direction["rotate"][0]) question_continue('start') if rotate == 1: print 'rotate' motion.moveTo(0, 0, math.pi * 2) if dir_x == 1: question_continue('forward') motion.moveTo(distance, 0, 0) question_continue('behind') motion.moveTo(-distance, 0, 0) if dir_y == 1: question_continue('left') motion.moveTo(0, distance, 0) question_continue('rigth') motion.moveTo(0, -distance, 0) if dir_diag_l == 1: question_continue('diagonale front left') motion.moveTo(distance, distance, 0) question_continue('diagonale back left') motion.moveTo(-distance, -distance, 0) if dir_diag_r == 1: question_continue('diagonale front right') motion.moveTo(distance, -distance, 0) question_continue('diagonale back right') motion.moveTo(-distance, distance, 0) motion.rest()
def move_robot(motion): """ Move robot on slope without falling """ parameters_direction = tools.read_section("config.cfg", "Slope") distance = float(parameters_direction["distance"][0]) nb_passage = int(parameters_direction["nb_passage"][0]) dir_x = int(parameters_direction["dir_X"][0]) dir_y = int(parameters_direction["dir_Y"][0]) dir_diag_l = int(parameters_direction["dir_diagL"][0]) dir_diag_r = int(parameters_direction["dir_diagR"][0]) question_continue('start') if dir_x == 1: question_continue('forward') motion.moveTo(distance, 0, 0) question_continue('behind') motion.moveTo(-distance, 0, 0) if dir_y == 1: question_continue('left') motion.moveTo(0, distance, 0) question_continue('rigth') motion.moveTo(0, -distance, 0) if dir_diag_l == 1: question_continue('diagonale front left ') motion.moveTo(distance, distance, 0) question_continue('diagonale back left') motion.moveTo(-distance, -distance, 0) if dir_diag_r == 1: question_continue('diagonale front right') motion.moveTo(distance, -distance, 0) question_continue('diagonale back right') motion.moveTo(-distance, distance, 0) motion.rest()
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
def wake_up_pos(): """ Fixture which retrieves wakeUp joints position from a configuration file. """ return tools.read_section("juliette_positions.cfg", "wakeUp")
def move_robot(motion): Move_x = 0 Move_y = 0 diag_1 = 0 diag_2 = 0 parameters_direction = tools.read_section("config.cfg", "DirectionParameters") distance = int(parameters_direction["distance"][0]) nb_passage = int(parameters_direction["nb_passage"][0]) dir_x = int(parameters_direction["dir_X"][0]) dir_y = int(parameters_direction["dir_Y"][0]) dir_diag_l = int(parameters_direction["dir_diagL"][0]) dir_diag_r = int(parameters_direction["dir_diagR"][0]) question_continue('start') if dir_x == 1: print 'move to X' for move_x in range(nb_passage): question_continue('forward') motion.moveTo(distance, 0, 0) question_continue('reverse') motion.moveTo(-distance, 0, 0) if move_x == nb_passage: move_x = 0 question_continue('') pass if dir_y == 1: question_continue('to Y') print 'move to Y' for move_y in range(nb_passage): question_continue('left') motion.moveTo(0, distance, 0) question_continue('right') motion.moveTo(0, -distance, 0) Move_y = +1 if move_y == nb_passage: move_y = 0 pass if dir_diag_l == 1: question_continue('to diagonale') print 'move to first diagonale' for diag_1 in range(nb_passage): question_continue('diagonale front left') motion.moveTo(distance, distance, 0) question_continue('diagonale back left') motion.moveTo(-distance, -distance, 0) diag_1 = +1 if diag_1 == nb_passage: diag_1 = 0 pass if dir_diag_r == 1: question_continue('to diagonale') print 'move to second diagonale' for diag_2 in range(nb_passage): question_continue('diagonale front right') motion.moveTo(distance, -distance, 0) question_continue('diagonale back right') motion.moveTo(-distance, distance, 0) diag_2 = +1 if diag_2 == nb_passage: diag_2 = 0 pass motion.rest()