def get_robot_device(robot: Robot, name: str, kind: Type[TDevice]) -> TDevice: device: Optional[Device] = None try: # webots 2020b is buggy and always raises TypeError when passed a str, # however we're aiming to be forwards compatible with 2021a, so try this first. device = robot.getDevice(name) except TypeError: pass if device is None: # webots 2020b always returns None when not raising TypeError if kind is Emitter: device = robot.getEmitter(name) elif kind is Receiver: device = robot.getReceiver(name) elif kind is Motor: device = robot.getMotor(name) elif kind is LED: device = robot.getLED(name) elif kind is DistanceSensor: device = robot.getDistanceSensor(name) elif kind is TouchSensor: device = robot.getTouchSensor(name) elif kind is Compass: device = robot.getCompass(name) elif kind is Display: device = robot.getDisplay(name) if not isinstance(device, kind): raise TypeError return device
swamp_colour = b'R\x89\xa7\xff' swamp_colour2 = b'R\x89\xa6\xff' left_motor = robot.getMotor("left wheel motor") right_motor = robot.getMotor("right wheel motor") left_encoder = left_motor.getPositionSensor() right_encoder = right_motor.getPositionSensor() left_heat_sensor = robot.getLightSensor("left_heat_sensor") right_heat_sensor = robot.getLightSensor("right_heat_sensor") gyro = robot.getGyro("gyro") emitter = robot.getEmitter("emitter") front_sensor_l = robot.getDistanceSensor("ps7") front_sensor_r = robot.getDistanceSensor("ps0") left_sensor = robot.getDistanceSensor("ps5") right_sensor = robot.getDistanceSensor("ps2") back_sensor_l = robot.getDistanceSensor("ps3") back_sensor_r = robot.getDistanceSensor("ps4") color = robot.getCamera("colour_sensor") cam = robot.getCamera("camera_centre") cam_right = robot.getCamera("camera_right") cam_left = robot.getCamera("camera_left") # get the time step of the current world.
robot = Robot() timestep = int(robot.getBasicTimeStep()) print ('Move forward and avoid obstabcles.') leftMotor = robot.getMotor('left motor') robot.getLED('left motor led').set(0xFFFF00) leftMotor.setPosition(float('inf')) # Velocity control mode. rightMotor = robot.getMotor('right motor') robot.getLED('right motor led').set(0x00FFFF) rightMotor.setPosition(float('inf')) # Velocity control mode. maxSpeed = leftMotor.getMaxVelocity() cruisingSpeed = 0.5 * maxSpeed leftDistanceSensor = robot.getDistanceSensor('left distance sensor') leftDistanceSensor.enable(timestep) robot.getLED('left distance sensor led').set(0xFF0000) rightDistanceSensor = robot.getDistanceSensor('right distance sensor') rightDistanceSensor.enable(timestep) robot.getLED('right distance sensor led').set(0x00FF00) while robot.step(timestep) != -1: # Invert the TinkerbotsDistanceSensor.lookupTable to have an appoximation of the distance in meters. rightDistance = 128 - rightDistanceSensor.getValue() leftDistance = 128 - leftDistanceSensor.getValue() delta = leftDistance - rightDistance leftSpeed = -cruisingSpeed - delta + 0.5 rightSpeed = cruisingSpeed - delta + 0.5 leftSpeed = max(-maxSpeed, min(maxSpeed, leftSpeed)) rightSpeed = max(-maxSpeed, min(maxSpeed, rightSpeed))
rightMotorBack = robot.getMotor('back right wheel') leftMotorFront.setPosition(float('inf')) rightMotorFront.setPosition(float('inf')) leftMotorBack.setPosition(float('inf')) rightMotorBack.setPosition(float('inf')) # initialize devices ps = [] psNames = [ 'so0', 'so1', 'so2', 'so3', 'so4', 'so5', 'so6', 'so7' ] for i in range(8): ps.append(robot.getDistanceSensor(psNames[i])) ps[i].enable(TIME_STEP) while robot.step(TIME_STEP) != -1: # read sensors outputs psValues = [] for i in range(8): psValues.append(ps[i].getValue()) # print(psValues[i]) # detect obstacles right_obstacle = psValues[0] > 70.0 or psValues[1] > 70.0 or psValues[2] > 70.0 left_obstacle = psValues[5] > 70.0 or psValues[6] > 70.0 or psValues[7] > 70.0 front_obstacle = psValues[3] > 50.0 or psValues[4] > 50.0 # print(right_obstacle)
def __init__(self, webot: Robot, sensor_name: str) -> None: self.webot_sensor = webot.getDistanceSensor(sensor_name) self.webot_sensor.enable(int(webot.getBasicTimeStep()))
LShoulderPitch = robot.getMotor("LShoulderPitch") LShoulderRoll = robot.getMotor("LShoulderRoll") LElbowRoll = robot.getMotor("LElbowRoll") # get and enable position sensors LShoulderPitchS = robot.getPositionSensor("LShoulderPitchS") LShoulderRollS = robot.getPositionSensor("LShoulderRollS") LElbowRollS = robot.getPositionSensor("LElbowRollS") LShoulderPitchS.enable(timestep) LShoulderRollS.enable(timestep) LElbowRollS.enable(timestep) # get and enable sonars SonarR = robot.getDistanceSensor("Sonar/Right") SonarL = robot.getDistanceSensor("Sonar/Left") SonarR.enable(timestep) SonarL.enable(timestep) #test movement LShoulderPitch.setVelocity(1.0) LShoulderRoll.setVelocity(1.0) LElbowRoll.setVelocity(1.0) def getPosition(): return [round(LShoulderPitchS.getValue(),2),round(LShoulderRollS.getValue(),2), round(LElbowRollS.getValue(),2)]
# create the Robot instance. robot = Robot() # See Thymio specific Webots names: https://cyberbotics.com/doc/guide/thymio2#thymio2-wbt # See "robot" specific functions: https://cyberbotics.com/doc/reference/robot?tab=python#getlightsensor # Motors leftMotor = robot.getMotor('motor.left') rightMotor = robot.getMotor('motor.right') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0) rightMotor.setVelocity(0) # see functions of DistanceSensor: https://cyberbotics.com/doc/reference/distancesensor#wb_distance_sensor_enable # sensors sens0 = robot.getDistanceSensor("prox.horizontal.0") sens1 = robot.getDistanceSensor("prox.horizontal.1") sens2 = robot.getDistanceSensor("prox.horizontal.2") sens3 = robot.getDistanceSensor("prox.horizontal.3") sens4 = robot.getDistanceSensor("prox.horizontal.4") # enable distance sensor measurements in sampling period of TIME_STEP sens0.enable(TIME_STEP) sens1.enable(TIME_STEP) sens2.enable(TIME_STEP) sens3.enable(TIME_STEP) sens4.enable(TIME_STEP) # Wall following algorithm while robot.step(TIME_STEP) != -1: # get the values read by the sensor
import struct # Create a robot instance robot = Robot() # Declare constants TIMESTEP = int(robot.getBasicTimeStep()) COMM_CHANNEL = 14 ROBOT_SPEED = 5.0 # Sensors setup ds = [] dsNames = ['ds0', 'ds1'] for name in dsNames: ds.append(robot.getDistanceSensor(name)) for i in range(2): ds[i].enable(TIMESTEP) # Motors setup wheels = [] wheelsNames = ['left wheel motor', 'right wheel motor'] for name in wheelsNames: wheels.append(robot.getMotor(name)) for i in range(2): wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) # Emitter setup
# get the time step of the current world. timestep = 32 max_speed = 7 #Created motor instances left_motor = robot.getDevice('motor_1') right_motor = robot.getDevice('motor_2') left_motor.setPosition(float('inf')) left_motor.setVelocity(0.0) right_motor.setPosition(float('inf')) right_motor.setVelocity(0.0) left_ir = robot.getDistanceSensor('ir_1') left_ir.enable(timestep) right_ir = robot.getDistanceSensor('ir_2') right_ir.enable(timestep) centre_ir = robot.getDistanceSensor('ir_3') centre_ir.enable(timestep) r_ir = robot.getDistanceSensor('ir_4') r_ir.enable(timestep) l_ir = robot.getDistanceSensor('ir_5') l_ir.enable(timestep) count = 0 while robot.step(timestep) != -1:
# create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # led = robot.getLED('ledname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) emitter = robot.getEmitter("emitter") sensorFLL = robot.getDistanceSensor("FLL") sensorFL = robot.getDistanceSensor("FL") sensorFR = robot.getDistanceSensor("FR") sensorFRR = robot.getDistanceSensor("FRR") lidar = robot.getLidar("lidar") sensorFLL.enable(timestep) sensorFL.enable(timestep) sensorFR.enable(timestep) sensorFRR.enable(timestep) leftWheel = robot.getMotor("left_motor") rightWheel = robot.getMotor("right_motor") leftOdo = robot.getPositionSensor("left_odo")
# create the Robot instance. robot = Robot() # get the wheels wheels = [] wheelNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for name in wheelNames: motor = robot.getMotor(name) motor.setPosition(float('inf')) motor.setVelocity(-1.5) wheels.append(motor) ds = [] dsNames = ['ds_left', 'ds_right'] for name in dsNames: sensor = robot.getDistanceSensor(name) sensor.enable(TIME_STEP) ds.append(sensor) # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) def goForward(): for motor in wheels: motor.setVelocity(-3)
from controller import Robot import os def callback(data): global velocity global message message = 'Received velocity value: ' + str(data.data) velocity = data.data robot = Robot() timeStep = int(robot.getBasicTimeStep()) left = robot.getMotor('motor.left') right = robot.getMotor('motor.right') sensor = robot.getDistanceSensor( 'prox.horizontal.2') # front central proximity sensor sensor.enable(timeStep) left.setPosition(float('inf')) # turn on velocity control for both motors right.setPosition(float('inf')) velocity = 0 left.setVelocity(velocity) right.setVelocity(velocity) message = '' print('Initializing ROS: connecting to ' + os.environ['ROS_MASTER_URI']) robot.step(timeStep) rospy.init_node('listener', anonymous=True) print('Subscribing to "motor" topic') robot.step(timeStep) rospy.Subscriber('motor', Float64, callback) pub = rospy.Publisher('sensor', Float64, queue_size=10) print('Running the control loop')
timeStep = int(robot.getBasicTimeStep()) # Constants of the e-puck motors and distance sensors. maxMotorVelocity = 4.2 num_left_dist_sensors = 4 num_right_dist_sensors = 4 right_threshold = [75, 75, 75, 75] left_threshold = [75, 75, 75, 75] # Get left and right wheel motors. leftMotor = robot.getMotor("left wheel motor") rightMotor = robot.getMotor("right wheel motor") # Get frontal distance sensors. dist_left_sensors = [ robot.getDistanceSensor('ps' + str(x)) for x in range(num_left_dist_sensors) ] # distance sensors list(map((lambda s: s.enable(timeStep)), dist_left_sensors)) # Enable all distance sensors dist_right_sensors = [ robot.getDistanceSensor('ps' + str(x)) for x in range(num_right_dist_sensors, 8) ] # distance sensors list(map((lambda t: t.enable(timeStep)), dist_right_sensors)) # Enable all distance sensors # Disable motor PID control mode. leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf'))
class ScribbleBot: def __init__(self): self.robot = Robot() # Define and enable distance sensors self.ds = [ self.robot.getDistanceSensor(name) for name in ['ds_left', 'ds_right'] ] for sensor in self.ds: sensor.enable(TIME_STEP) # Define motors self.front_left_motor = self.robot.getMotor('wheel1') self.front_right_motor = self.robot.getMotor('wheel2') self.back_left_motor = self.robot.getMotor('wheel3') self.back_right_motor = self.robot.getMotor('wheel4') # Group motors for easy access self.motors = [ self.front_left_motor, self.front_right_motor, self.back_left_motor, self.back_right_motor ] # Enable motors for motor in self.motors: motor.setPosition(float('inf')) motor.setVelocity(0.0) # Define encoders self.enc_front_left = self.robot.getPositionSensor( 'front left position') self.enc_front_right = self.robot.getPositionSensor( 'front right position') self.enc_back_left = self.robot.getPositionSensor('back left position') self.enc_back_right = self.robot.getPositionSensor( 'back right position') # Enable encoders self.enc_front_left.enable(TIME_STEP) self.enc_front_right.enable(TIME_STEP) self.enc_back_left.enable(TIME_STEP) self.enc_back_right.enable(TIME_STEP) # Line generator self.pen = self.robot.getPen('pen') self.pen.write(True) # Positions of encoders self.position_front_left = 0 self.position_back_left = 0 self.position_front_right = 0 self.position_back_right = 0 # Internal velocities, one per side self.left_vel = 0 self.right_vel = 0 # List for distance sensor values self.ds_values = [] def write_motors(self): """Write the current velocities into motor objects""" self.front_left_motor.setVelocity(self.left_vel) self.back_left_motor.setVelocity(self.left_vel) self.front_right_motor.setVelocity(self.right_vel) self.back_right_motor.setVelocity(self.right_vel) def stop_motors(self): """Helper util to stop motors""" self.left_vel = 0 self.right_vel = 0 self.write_motors() def capture_position(self): """Capture current encoder values in position variables""" # Left positions self.position_front_left = self.enc_front_left.getValue() self.position_back_left = self.enc_back_left.getValue() # Right positions self.position_front_right = self.enc_front_right.getValue() self.position_back_right = self.enc_back_right.getValue() def check_left_moved(self, distance: float) -> bool: """ Manages a left side distance move. If either motor has achieved distance required, it is manually stopped. If both have, function returns `True` """ front_moved = abs(self.enc_front_left.getValue() - self.position_front_left) > distance back_moved = abs(self.enc_back_left.getValue() - self.position_back_left) > distance if front_moved: self.front_left_motor.setVelocity(0) if back_moved: self.back_left_motor.setVelocity(0) both = front_moved and back_moved if both: self.left_vel = 0 return both def check_right_moved(self, distance: float) -> bool: """ Manages a right side distance move. If either motor has achieved distance required, it is manually stopped. If both have, function returns `True` """ front_moved = abs(self.enc_front_right.getValue() - self.position_front_right) > distance back_moved = abs(self.enc_back_right.getValue() - self.position_back_right) > distance if front_moved: self.front_right_motor.setVelocity(0) if back_moved: self.back_right_motor.setVelocity(0) both = front_moved and back_moved if both: self.right_vel = 0 return both def make_turn(self, left_dist: float, right_dist: float): """ Makes a turn based on velocities and encoder positions. Blocking. Velocities must be set outside function call. Each wheel is moved to corresponding side distance and stopped """ self.write_motors() self.capture_position() while self.robot.step(TIME_STEP) != -1: left_moved = self.check_left_moved(left_dist) right_moved = self.check_right_moved(right_dist) if left_moved and right_moved: break def make_left_turn(self, distance: float): """ Make a left turn by a distance. Blocking. Velocities must be set outside function call. `distance` pertains to right wheel, left wheel distance is calculated from this and velocity ratio for perfect arcing turn """ ratio = self.left_vel / self.right_vel self.make_turn(ratio * distance, distance) def make_right_turn(self, distance: float): """ Make a right turn by a distance. Blocking. Velocities must be set outside function call. `distance` pertains to left wheel, right wheel distance is calculated from this and velocity ratio for perfect arcing turn """ ratio = self.right_vel / self.left_vel self.make_turn(distance, ratio * distance) def move_dist(self, distance: float): """ Move a distance forward. Blocking. Velocities must be set outside function call """ self.write_motors() self.capture_position() while self.robot.step(TIME_STEP) != -1: left_moved = self.check_left_moved(distance) right_moved = self.check_right_moved(distance) if left_moved and right_moved: break def load_distance_values(self): """Load current distance values from `self.ds` into `self.ds_values`""" self.ds_values = [] for ds in self.ds: self.ds_values.append(ds.getValue()) def init_sensors(self): """Initialize sensors to avoid error values like NaN""" for i in range(0, 5): if self.robot.step(TIME_STEP) == -1: break for ds in self.ds: ds.getValue() self.enc_front_left.getValue() self.enc_front_right.getValue() self.enc_back_left.getValue() self.enc_back_right.getValue() def do_the_wave(self): """Do the waaaaave""" # Entry turn to the right self.left_vel = 3 self.right_vel = 0.1 self.make_right_turn(6) # Arcing main turn to the left self.left_vel = 1 self.right_vel = 3.5 self.make_left_turn(15.8) # Exiting turn to the right self.left_vel = 3 self.right_vel = 0.5 self.make_right_turn(4.5) # Aligning turn to the right, slow and precise self.left_vel = 1 self.right_vel = 0.2 self.make_right_turn(1.94) def search_for_wall(self): """Drive forward until wall is found""" print('Finding end wall...') self.left_vel = 3 self.right_vel = 3 self.write_motors() while self.robot.step(TIME_STEP) != -1: self.load_distance_values() # print('FL: {} FR: {}'.format(self.ds_values[DS_L], self.ds_values[DS_R])) if self.ds_values[DS_L] < DS_WALL_STOP and self.ds_values[ DS_R] < DS_WALL_STOP: break def setup(self): """Pre-movement initializations. Runs once""" print('Initializing...') self.init_sensors() self.left_vel = 3 self.right_vel = 3 self.write_motors() def main(self): """Main code. Usually a loop or series of loops""" print('Running...') self.move_dist(7) for i in range(4): self.do_the_wave() self.search_for_wall() def cleanup(self): """Post-movement finishing code""" self.stop_motors() print('Finished drawing.') def run(self): """Primary running method""" self.setup() self.main() self.cleanup()
# You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) leftMotor = robot.getMotor('left wheel motor') rightMotor = robot.getMotor('right wheel motor') #Инициализация ИК датчиков IRSensorNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7'] IRSensors = [] for sensor in IRSensorNames: IRSensors.append(robot.getDistanceSensor(sensor)) #---------- #Инициализация датчиков линии GroundSensorNames = ['gs0', 'gs1', 'gs2'] GroundSensors = [] for sensor in GroundSensorNames: GroundSensors.append(robot.getDistanceSensor(sensor)) #---------- #включаем все сенсоры for sensor in GroundSensors: sensor.enable(TIME_STEP) for sensor in IRSensors: sensor.enable(TIME_STEP) #----------
# create the Robot instance. robot = Robot() #See Thymio specific Webots names: https://cyberbotics.com/doc/guide/thymio2#thymio2-wbt #See "robot" specific functions: https://cyberbotics.com/doc/reference/robot?tab=python#getlightsensor #Motors leftMotor = robot.getMotor('motor.left') rightMotor = robot.getMotor('motor.right') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0) rightMotor.setVelocity(0) #see functions of DistanceSensor: https://cyberbotics.com/doc/reference/distancesensor#wb_distance_sensor_enable #sensors sens2 = robot.getDistanceSensor("prox.horizontal.2") sens4 = robot.getDistanceSensor("prox.horizontal.4") #enable distance sensor measurements in sampling period of TIME_STEP sens2.enable(TIME_STEP) sens4.enable(TIME_STEP) class ThymioRobot: def __init__(self, id, gender, startPos, sensor_front, sensor_right): self.id = id self.gender = gender self.startPos = startPos self.sensor_front = sensor_front self.sensor_right = sensor_right # Choose random gender ##############
class SuperRoomba: def __init__(self): print('Initializing robot...') # Create the Robot instance. self.robot = Robot() # distance sensors self.ps = [] ps_names = [ 'ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7' ] for i in range(8): self.ps.append(self.robot.getDistanceSensor(ps_names[i])) self.ps[i].enable(TIME_STEP) self.leds = [] led_names = [ 'led0', 'led1', 'led2', 'led3', 'led4', 'led5', 'led6', 'led7', 'led8', 'led9' ] for i in range(10): self.leds.append(self.robot.getLED(led_names[i])) self.compass = self.robot.getCompass('compass') self.compass.enable(TIME_STEP) self.leftMotor = self.robot.getMotor('left wheel motor') self.rightMotor = self.robot.getMotor('right wheel motor') self.left_encoder = self.robot.getPositionSensor('left wheel sensor') self.right_encoder = self.robot.getPositionSensor('right wheel sensor') self.left_encoder.enable(TIME_STEP) self.right_encoder.enable(TIME_STEP) self.leftMotor.setPosition(float('inf')) self.rightMotor.setPosition(float('inf')) self.leftMotor.setVelocity(0.0) self.rightMotor.setVelocity(0.0) self.touch_sensor = self.robot.getTouchSensor('touch sensor') self.touch_sensor.enable(TIME_STEP) self.left_vel = None self.right_vel = None # Current absolute direction self.facing = None self.facing_angle = None self.turning = None # start values for encoder tracking self.left_pos_start = None self.right_pos_start = None # Led for search animation self.search_led = 0 self.led_timer = 0 self.initialize_sensors() # Current movement state self.state = None self.best_path = None self.path_logger = PathLogger() if use_file and run == 3: self.path_logger.load(last_best_path) self.best_path = iter(self.path_logger) # Load initial samples in each sensor window self.ps_windows = [] for i in range(0, 8): self.ps_windows.append(list(repeat(self.ps[i].getValue(), WINDOW_SIZE))) def initialize_sensors(self): # Initialize sensors for i in range(3): if self.robot.step(TIME_STEP) == -1: break for x in range(8): self.ps[x].getValue() self.compass.getValues() self.left_encoder.getValue() self.right_encoder.getValue() self.touch_sensor.getValue() def write_motors(self): self.leftMotor.setVelocity(self.left_vel) self.rightMotor.setVelocity(self.right_vel) def stop_motors(self): self.left_vel = 0 self.right_vel = 0 self.write_motors() def full_forward(self): self.left_vel = FORWARD_SPEED self.right_vel = FORWARD_SPEED self.write_motors() def full_reverse(self): self.left_vel = -FORWARD_SPEED self.right_vel = -FORWARD_SPEED self.write_motors() @staticmethod def rot_dist(current, target): dist = abs(target - current) return min(dist, TWO_PI - dist) def in_range(self, value, target, variance, angle=False): if angle: return self.rot_dist(value, target) <= variance return target - variance <= value <= target + variance @staticmethod def get_turn_direction(current, target): target += math.pi current += math.pi right = (target - current) % TWO_PI left = (current - target) % TWO_PI if right < left: return Direction.RIGHT return Direction.LEFT def ps_refresh(self): for i in range(0, WINDOW_SIZE): if self.robot.step(TIME_STEP) == -1: break for x in range(8): window = self.ps_windows[x] window.insert(0, self.ps[x].getValue()) window.pop() def get_compass(self): x, y, z = self.compass.getValues() return math.atan2(x, z) def capture_position(self): self.left_pos_start = self.left_encoder.getValue() self.right_pos_start = self.right_encoder.getValue() def move_distance_blocking(self, dist): self.capture_position() while self.robot.step(TIME_STEP) != -1: left_pos = self.left_encoder.getValue() right_pos = self.right_encoder.getValue() # print('Moving -> {}%'.format(int(abs(right_pos - self.right_pos_start) / dist * 100))) right_done = abs(right_pos - self.right_pos_start) > dist left_done = abs(left_pos - self.left_pos_start) > dist if right_done: self.right_vel = 0 if left_done: self.left_vel = 0 self.write_motors() if right_done and left_done: return True return False def turn_towards_blocking(self, target): if self.get_turn_direction(self.get_compass(), target) == Direction.RIGHT: self.left_vel = HALF_SPEED self.right_vel = -HALF_SPEED self.turning = Direction.RIGHT else: self.left_vel = -HALF_SPEED self.right_vel = HALF_SPEED self.turning = Direction.LEFT self.write_motors() start = self.get_compass() total_dist = self.rot_dist(start, target) while self.robot.step(TIME_STEP) != -1: angle = self.get_compass() # print('Turning -> {}%'.format(int(self.rot_dist(angle, target) / total_dist * 100))) if self.in_range(angle, target, 0.15, angle=True): self.left_vel = CREEP_SPEED if self.turning == Direction.RIGHT else -CREEP_SPEED self.right_vel = -CREEP_SPEED if self.turning == Direction.RIGHT else CREEP_SPEED self.write_motors() if self.in_range(angle, target, 0.02, angle=True): return True return False def intersection_align(self): self.full_forward() print('Aligning with intersection...') self.move_distance_blocking(DIST_ALIGN) print('Aligned.') self.ps_refresh() def enter_passage(self): self.full_forward() print('Entering passage...') self.move_distance_blocking(DIST_ENTER) print('Entered.') self.ps_refresh() def turn_around(self): print('Turning around') self.blink_reverse(LED_BLINK_TIME) # angle_mid = dir_angles[Direction((self.facing.value - 1) % 4)] # self.turn_towards_blocking(angle_mid) # self.full_forward() # self.move_distance_blocking(DIST_WALL_ALIGN) self.facing = Direction((self.facing.value - 2) % 4) self.turn_towards_blocking(dir_angles[self.facing]) def async_delay(self, delay): time_start = time.time() while self.robot.step(TIME_STEP) != -1: if (time.time() - time_start) > delay: break def set_led_range(self, start, stop): for i in range(start, stop): self.leds[i].set(1) def clear_led_range(self, start, stop): for i in range(start, stop): self.leds[i].set(0) def blink_right(self, delay): self.set_led_range(1, 4) self.async_delay(delay) self.clear_led_range(1, 4) def blink_left(self, delay): self.set_led_range(5, 8) self.async_delay(delay) self.clear_led_range(5, 8) def blink_forward(self, delay): self.set_led_range(0, 2) self.leds[7].set(1) self.async_delay(delay) self.clear_led_range(0, 2) self.leds[7].set(0) def blink_reverse(self, delay): self.set_led_range(3, 6) self.async_delay(delay) self.clear_led_range(3, 6) def turn_right(self): print('Turning right') self.blink_right(LED_BLINK_TIME) self.facing = Direction((self.facing.value + 1) % 4) self.turn_towards_blocking(dir_angles[self.facing]) self.enter_passage() def turn_left(self): print('Turning left') self.blink_left(LED_BLINK_TIME) self.facing = Direction((self.facing.value - 1) % 4) self.turn_towards_blocking(dir_angles[self.facing]) self.enter_passage() def setup(self): # Turn for first decision self.facing = Direction.NORTH self.turn_towards_blocking(dir_angles[self.facing]) self.led_timer = time.time() self.state = State.DECIDE def mainloop(self): while self.robot.step(TIME_STEP) != -1: # Window all the proximity sensors for moving averages ps_values = [] for i in range(8): window = self.ps_windows[i] window.insert(0, self.ps[i].getValue()) window.pop() ps_values.append(sum(window) / WINDOW_SIZE) # Pull front sensors un-windowed so no delay ps_front_left = self.ps[PS_F_L].getValue() ps_front_right = self.ps[PS_F_R].getValue() angle = self.get_compass() if self.state == State.DECIDE: self.stop_motors() # print(ps_values) # if (use_file and ps_values[PS_R] < PS_MIN) or (not use_file and ps_values[PS_L] < PS_MIN): # # self.intersection_align() # self.intersection_align() # twice # self.intersection_align() # twice # # if (use_file and ps_values[PS_L] < PS_MIN) or (not use_file and ps_values[PS_R] < PS_MIN): # self.intersection_align() # twice if use_file: if run == 3: next_dir = next(self.best_path) if next_dir == self.facing: print('Proceeding {}'.format(next_dir.name.title())) self.enter_passage() else: print('Turning {}'.format(next_dir.name.title())) self.facing = next_dir self.turn_towards_blocking(dir_angles[self.facing]) self.enter_passage() else: # Give priority to left if ps_values[PS_L] < PS_MIN: self.turn_left() else: if ps_front_left < PS_FRONT_STOP and ps_front_right < PS_FRONT_STOP: print('Going forward') self.blink_forward(LED_BLINK_TIME) self.enter_passage() else: if ps_values[PS_R] < PS_MIN: self.turn_right() else: self.turn_around() if run == 2: self.path_logger.add(self.facing) else: # Give priority to right if ps_values[PS_R] < PS_MIN: self.turn_right() else: # print('Not turning right because: {}'.format(ps_values[PS_R])) # self.ps_refresh() # print('After refreshing its {}'.format(ps_values[PS_R])) if ps_front_left < PS_FRONT_STOP and ps_front_right < PS_FRONT_STOP: print('Going forward') self.blink_forward(LED_BLINK_TIME) self.enter_passage() else: if ps_values[PS_L] < PS_MIN: self.turn_left() else: self.turn_around() if run == 2: self.path_logger.add(self.facing) self.leds[LED_BODY].set(0) self.full_forward() self.state = State.SEARCH elif self.state == State.SEARCH: # print('L: {} R: {}'.format(self.ps[PS_F_L].getValue(), self.ps[PS_F_R].getValue())) if (time.time() - self.led_timer) > 0.1: self.led_timer = time.time() self.leds[self.search_led].set(0) self.search_led = (self.search_led + 1) % 8 self.leds[self.search_led].set(1) if self.get_turn_direction(angle, dir_angles[self.facing]) == Direction.LEFT: self.right_vel += 0.025 self.left_vel -= 0.025 else: self.left_vel += 0.025 self.right_vel -= 0.025 self.write_motors() if self.touch_sensor.getValue(): self.leds[self.search_led].set(0) self.state = State.FINISH front_hit = self.ps[PS_F_L].getValue() > PS_FRONT_STOP or self.ps[PS_F_R].getValue() > PS_FRONT_STOP if front_hit: print('Encountered wall') self.leds[LED_BODY].set(1) self.stop_motors() self.leds[self.search_led].set(0) self.state = State.DECIDE continue if use_file: turn_found = ps_values[PS_L] < PS_MIN else: turn_found = ps_values[PS_R] < PS_MIN # either_turn_found = ps_values[PS_L] < PS_MIN or ps_values[PS_R] < PS_MIN # both_turns_found = ps_values[PS_L] < PS_MIN and ps_values[PS_R] < PS_MIN # if use_file: # soft_intersection = ps_values[PS_R] < PS_MIN # else: # soft_intersection = ps_values[PS_L] < PS_MIN if turn_found: print('Encountered turn') self.leds[LED_BODY].set(1) self.intersection_align() self.leds[self.search_led].set(0) self.state = State.DECIDE # if soft_intersection: # self.path_logger.add(self.facing) elif self.state == State.FINISH: print('Finished.') print('Path log: {}'.format(self.path_logger.chars())) self.stop_motors() break def cleanup(self): with open('data.json', 'w') as f: data = { 'run': run + 1, 'best_path': self.path_logger.chars() } json.dump(data, f) def run(self): self.setup() self.mainloop() self.cleanup()
"shoulder_lift_joint_sensor") shoulder_pan_joint_sensor = robot.getPositionSensor( "shoulder_pan_joint_sensor") wrist_1_joint_sensor = robot.getPositionSensor("wrist_1_joint_sensor") wrist_2_joint_sensor = robot.getPositionSensor("wrist_2_joint_sensor") wrist_3_joint_sensor = robot.getPositionSensor("wrist_3_joint_sensor") #enabling sensors (each sensor needs to be enabled and you need to specify an update delay) elbow_joint_sensor.enable(TIME_STEP) shoulder_lift_joint_sensor.enable(TIME_STEP) shoulder_pan_joint_sensor.enable(TIME_STEP) wrist_1_joint_sensor.enable(TIME_STEP) wrist_2_joint_sensor.enable(TIME_STEP) wrist_3_joint_sensor.enable(TIME_STEP) dist_sensor = robot.getDistanceSensor("distance sensor") dist_sensor.enable(TIME_STEP) # cam_sensor = robot.getCamera("camera") # cam_sensor.enable(TIME_STEP) # img = cam_sensor.saveImage('test_img.png',50) # print(img) k = 0 while robot.step(TIME_STEP) != -1: # dist_sensor_val = dist_sensor.getValue() # print(dist_sensor_val) print("The elbow_joint_sensor reads: %.2f radian" % elbow_joint_sensor.getValue())
def __init__(self, timestep: int, bot: Robot): # self.light_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87, 1.58784] # self.distance_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87] # self.wheel_actuators_positions = [0.0, 3.14159] # self.bump_sensors_positions = [1.27, 0.77, 0.0, 5.21, 4.21, 3.14159, 2.37, 1.87] self.__devices_labels = EPuckDevicesLabels( self.N_DISTANCE_SENSORS, self.N_LIGHT_SENSORS, self.N_BUMPERS, self.N_LEDS, self.N_WHEELS ) #Retrieve device references self.__led_actuators = actuators_array( self.__devices_labels.led_actuators_labels, timestep, lambda name: bot.getLED(name) ) self.__wheel_actuators = actuators_array( self.__devices_labels.wheel_actuators_labels, timestep, lambda name: bot.getMotor(name), actuators_positions=self.WHEEL_ACTUATORS_POSITION ) self.__distance_sensors = sensor_array( self.__devices_labels.distance_sensors_labels, timestep, lambda name: bot.getDistanceSensor(name), sensors_positions=self.DISTANCE_SENSORS_POSITION ) self.__light_sensors = sensor_array( self.__devices_labels.light_sensors_labels, timestep, lambda name: bot.getLightSensor(name), sensors_positions=self.LIGHT_SENSORS_POSITION ) # self.__bumpers = sensor_array( # self.__devices_labels.bump_sensors_labels, # timestep, # lambda name: bot.getTouchSensor(name), # sensors_positions = self.bump_sensors_positions # ) self.__gps = sensor_array( self.__devices_labels.gps_sensors_labels, timestep, lambda name: bot.getGPS(name), sensors_positions=[0.0] ) self.__emitters = actuators_array( self.__devices_labels.emitter_labels, timestep, lambda name: bot.getEmitter(name), actuators_positions=[0.0] ) self.__receivers = sensor_array( self.__devices_labels.receiver_labels, timestep, lambda name: bot.getReceiver(name), sensors_positions=[0.0] )
robot = Robot() # Get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # Get pointer to the robot wheels motors. leftWheel = robot.getMotor('left wheel') rightWheel = robot.getMotor('right wheel') # We will use the velocity parameter of the wheels, so we need to # set the target position to infinity. leftWheel.setPosition(float('inf')) rightWheel.setPosition(float('inf')) # Get and enable the distance sensors. frontSensor = robot.getDistanceSensor("so3") frontSensor.enable(timestep) frontleftSensor = robot.getDistanceSensor("so1") frontleftSensor.enable(timestep) sideSensor = robot.getDistanceSensor("so0") sideSensor.enable(timestep) backsideSensor = robot.getDistanceSensor("so15") backsideSensor.enable(timestep) maxdist = 0.5 # Move forward until we are 50 cm away from the wall. leftWheel.setVelocity(MAX_SPEED) rightWheel.setVelocity(MAX_SPEED) while robot.step(timestep) != -1: if getDistance(frontSensor) < 0.25: break
MAX_SPEED = 6.28 # максимальная скорость robot = Robot() timestep = int(robot.getBasicTimeStep()) leftMotor = robot.getMotor('left wheel motor') # подключение левого rightMotor = robot.getMotor('right wheel motor') # и правого мотора leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) rightFor = robot.getDistanceSensor('ps0') # подключаем датчики расстояния: rightFor.enable(timestep) # правый и передний правый, right = robot.getDistanceSensor('ps2') # а также активируем их right.enable(timestep) # счётчики для выполнения определённых действий cntForward = 0 # для движения вперёд после потери препятствия cntLeft = 0 # для поворота налево cntRight = 0 # для поворота направо cntForwardAfterRotation = 0 # для движения вперёд после поворота # коэффициенты скорости моторов leftSpeed = 0.0 rightSpeed = 0.0 while robot.step(timestep) != -1: rf = rightFor.getValue() # получение данных
import math # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # You should insert a getDevice-like function in order to get the # instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep) #enable distance sensors frontDistanceSensor = robot.getDistanceSensor('front_ds') leftDistanceSensor = robot.getDistanceSensor('left_ds') rightDistanceSensor = robot.getDistanceSensor('right_ds') frontDistanceSensor.enable(timestep) leftDistanceSensor.enable(timestep) rightDistanceSensor.enable(timestep) # enable camera and recognition camera = robot.getCamera('camera1') camera.enable(timestep) camera.recognitionEnable(timestep) #enable imu imu = robot.getInertialUnit('inertial unit') imu.enable(timestep)
emitter = robot.getEmitter("emitter") gps = robot.getGPS("gps") gps.enable(timeStep) left_heat_sensor = robot.getLightSensor("left_heat_sensor") right_heat_sensor = robot.getLightSensor("right_heat_sensor") left_heat_sensor.enable(timeStep) right_heat_sensor.enable(timeStep) leftSensors = [] rightSensors = [] frontSensors = [] frontSensors.append(robot.getDistanceSensor("ps7")) frontSensors[0].enable(timeStep) frontSensors.append(robot.getDistanceSensor("ps0")) frontSensors[1].enable(timeStep) rightSensors.append(robot.getDistanceSensor("ps1")) rightSensors[0].enable(timeStep) rightSensors.append(robot.getDistanceSensor("ps2")) rightSensors[1].enable(timeStep) leftSensors.append(robot.getDistanceSensor("ps5")) leftSensors[0].enable(timeStep) leftSensors.append(robot.getDistanceSensor("ps6")) leftSensors[1].enable(timeStep) # [left wheel speed, right wheel speed]
motors = [] encoders = [] for i, motor_name in enumerate(["left", "right"]): motor = robot.getMotor('{} wheel motor'.format(motor_name)) enc = robot.getPositionSensor('{} wheel encoder'.format(motor_name)) motor.setPosition(float('inf')) motor.setVelocity(0.0) enc.enable(timestep) motors.append(motor) encoders.append(enc) # distance sensors: distance_sensors = [] for i in range(2): sensor = robot.getDistanceSensor("ds{:}".format(i)) sensor.enable(timestep) distance_sensors.append(sensor) camera = robot.getCamera("camera") camera.enable(timestep) max_vel = motors[1].getMaxVelocity() print("Max velocity is {:.3f} rad/s".format(max_vel)) speed_vectors = [1, -1] ds_to_vel = 0.004 speed_offset = 0.3 * max_vel # Main loop:
wheels = [] wheelsNames = ['wheel1', 'wheel2'] for i in range(2): wheels.append(robot.getMotor(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) robot_type = 1 communication = robot.getReceiver("Receiver") Receiver.enable(communication, TIME_STEP) ds0 = robot.getDistanceSensor("ds0") ds1 = robot.getDistanceSensor("ds1") ds0.enable(TIME_STEP) ds1.enable(TIME_STEP) # Main loop: while robot.step(TIME_STEP) != -1: # Just receiver stuff... if (Receiver.getQueueLength(communication) > 0) : # /* read current packet's data */ new_message= receiver.getData() message = struct.unpack("ddd", new_mssage) if (message_printed != 1) :
"right ultrasonic sensor" ] IRSensorNames = [ "rear left infrared sensor", "left infrared sensor", "front left infrared sensor", "front infrared sensor", "front right infrared sensor", "right infrared sensor", "rear right infrared sensor", "rear infrared sensor", "ground left infrared sensor", "ground front left infrared sensor", "ground front right infrared sensor", "ground right infrared sensor" ] USSensors = [] IRSensors = [] #добавляем все ультразвуковые сенсоры в список for sensor in USSensorNames: USSensors.append(robot.getDistanceSensor(sensor)) #добавляем все ИК сенсоры в список for sensor in IRSensorNames: IRSensors.append(robot.getDistanceSensor(sensor)) #включаем все сенсоры for sensor in USSensors: sensor.enable(TIME_STEP) for sensor in IRSensors: sensor.enable(TIME_STEP) #включаем моторы, устанавливаем начальную скорость = 0 leftMotor = robot.getMotor('left wheel motor') rightMotor = robot.getMotor('right wheel motor') leftMotor.setPosition(float('inf'))
# Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. from controller import Robot robot = Robot() timestep = int(robot.getBasicTimeStep()) print('Move forward until an obstabcle is detected.') robot.getLED('motor led').set(0xFF0000) robot.getLED('distance sensor led').set(0x00FF00) motor = robot.getMotor('motor') motor.setPosition(float('inf')) # Velocity control mode. motor.setVelocity(0.5 * motor.getMaxVelocity()) distanceSensor = robot.getDistanceSensor('distance sensor') distanceSensor.enable(timestep) while robot.step(timestep) != -1: distance = distanceSensor.getValue() if distance < 100: print('Obstacle detected. Stop the motor.') motor.setVelocity(0) robot.step(timestep) break
from controller import Robot SPEED = 1.0 robot = Robot() timestep = int(robot.getBasicTimeStep()) left_motor = robot.getMotor('left wheel motor') right_motor = robot.getMotor('right wheel motor') left_motor.setPosition(float('inf')) right_motor.setPosition(float('inf')) left_motor.setVelocity(0.0) right_motor.setVelocity(0.0) ds_c = robot.getDistanceSensor('floor sensor') ds_c.enable(timestep) iter = 0 flag = True while robot.step(timestep) != -1: iter += 1 value = ds_c.getValue() if value < 101: left_motor.setVelocity(-SPEED) right_motor.setVelocity(SPEED) continue if iter % 10 == 0: if flag: SPEED += 0.1 else: SPEED -= 0.2 if SPEED == 10 or SPEED == 2: flag = not flag
"""e-puck_go_forward controller.""" # You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot robot = Robot() MAX_SPEED = 5.24 DELAY = 70 timestep = int(robot.getBasicTimeStep()) left_wheel = robot.getMotor('left wheel') right_wheel = robot.getMotor('right wheel') front_ss = robot.getDistanceSensor('so4') front_ss.enable(timestep) leftSensors = [] for i in range(3): sensor = robot.getDistanceSensor('so' + str(i)) leftSensors.append(sensor) leftSensors[i].enable(timestep) rightSensors = [] for i in range(3): sensor = robot.getDistanceSensor('so' + str(5 + i)) rightSensors.append(sensor) rightSensors[i].enable(timestep)
# Get reference to the robot. robot = Robot() # Get simulation step length. timeStep = int(robot.getBasicTimeStep()) # Constants of the Thymio II motors and distance sensors. maxMotorVelocity = 9.53 distanceSensorCalibrationConstant = 360 # Get left and right wheel motors. leftMotor = robot.getMotor("motor.left") rightMotor = robot.getMotor("motor.right") # Get frontal distance sensors. outerLeftSensor = robot.getDistanceSensor("prox.horizontal.0") centralLeftSensor = robot.getDistanceSensor("prox.horizontal.1") centralSensor = robot.getDistanceSensor("prox.horizontal.2") centralRightSensor = robot.getDistanceSensor("prox.horizontal.3") outerRightSensor = robot.getDistanceSensor("prox.horizontal.4") # Enable distance sensors. outerLeftSensor.enable(timeStep) centralLeftSensor.enable(timeStep) centralSensor.enable(timeStep) centralRightSensor.enable(timeStep) outerRightSensor.enable(timeStep) # Disable motor PID control mode. leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf'))