robot = Robot() # max_wheel_speed = 1000 # num_dist_sensors = 8 # encoder_resolution = 159.23 # for wheel encoders # tempo = 0.5 # Upper velocity bound = Fraction of the robot's maximum velocity = 1000 = 1 wheel revolution/sec # wheel_diameter = 4.1 # centimeters # axle_length = 5.3 # centimeters #TIME_STEP = 64 TIME_STEP = int(robot.getBasicTimeStep()) MAX_SPEED = float(6.28) #ROBOT_ANGULAR_SPEED_IN_DEGREES = float(360) ROBOT_ANGULAR_SPEED_IN_DEGREES = float(283.588111888) port = robot.getCustomData() print("Port: ", port) # initialize sensors # ps = [] # psNames = [ # 'ps0', 'ps1', 'ps2', 'ps3', # 'ps4', 'ps5', 'ps6', 'ps7' # ] # for i in range(8): # ps.append(robot.getDevice(psNames[i])) # ps[i].enable(TIME_STEP) # initialize and set motor devices leftMotor = robot.getDevice('left wheel')
# Enable touch sensor to detect collisions: Tsensor = robot.getDevice("main_sensor") Tsensor.enable(200) # Sampling period in milliseconds as parameter left_wheel = robot.getDevice("wheel_left_joint") right_wheel = robot.getDevice("wheel_right_joint") left_wheel.setPosition(float('inf')) right_wheel.setPosition(float('inf')) radius = 0.0985 distance_between_wheels = 0.404 collided = False while robot.step(timestep) != -1: data = robot.getCustomData() if len(data) == 0: continue customData = [float(x) for x in data.split(',')] left_wheel_speed, right_wheel_speed = speeds_to_wheels( customData[0], customData[2], radius, distance_between_wheels) left_wheel.setVelocity(left_wheel_speed) right_wheel.setVelocity(right_wheel_speed) # Readings from the touch sensor: collision = Tsensor.getValue( ) # Value is 1 if there is a collision and 0 otherwise if math.isnan(collision) and not collided: collision = '0' elif collision > 0 or collided: collision = '1' collided = True
class Robot: """ Primary API for access to robot parts. This robot requires that the consumer manage the progession of time manually by calling the `sleep` method. """ def __init__(self, quiet: bool = False, init: bool = True) -> None: self._initialised = False self._quiet = quiet self.webot = WebotsRobot() # returns a float, but should always actually be an integer value self._timestep = int(self.webot.getBasicTimeStep()) self.mode = environ.get("SR_ROBOT_MODE", "dev") self.zone = int(environ.get("SR_ROBOT_ZONE", 0)) self.arena = "A" self.usbkey = path.normpath(path.join(environ["SR_ROBOT_FILE"], "../")) # Lock used to guard access to Webot's time stepping machinery, allowing # us to safely advance simulation time from *either* the competitor's # code (in the form of our `sleep` method) or from our background # thread, but not both. self._step_lock = Lock() if init: self.init() self.wait_start() @classmethod def setup(cls) -> 'Robot': return cls(init=False) def init(self) -> None: self._init_devs() self._initialised = True self.display_info() def _get_user_code_info(self) -> Optional[str]: user_version_path = path.join(self.usbkey, '.user-rev') if path.exists(user_version_path): with open(user_version_path) as f: return f.read().strip() return None def display_info(self) -> None: user_code_version = self._get_user_code_info() parts = [ "Zone: {}".format(self.zone), "Mode: {}".format(self.mode), ] if user_code_version: parts.append("User code: {}".format(user_code_version)) print("Robot Initialized. {}.".format(", ".join(parts))) # noqa:T001 def webots_step_and_should_continue(self, duration_ms: int) -> bool: """ Run a webots step of the given duration in milliseconds. Returns whether or not the simulation should continue (based on Webots telling us whether or not the simulation is about to end). """ if duration_ms <= 0: raise ValueError( "Duration must be greater than zero, not {!r}".format(duration_ms), ) with self._step_lock: # We use Webots in synchronous mode (specifically # `synchronization` is left at its default value of `TRUE`). In # that mode, Webots returns -1 from step to indicate that the # simulation is terminating, or 0 otherwise. result = self.webot.step(duration_ms) return result != -1 def wait_start(self) -> None: "Wait for the start signal to happen" if self.mode not in ["comp", "dev"]: raise Exception( "mode of '%s' is not supported -- must be 'comp' or 'dev'" % self.mode, ) if self.zone < 0 or self.zone > 3: raise Exception( "zone must be in range 0-3 inclusive -- value of %i is invalid" % self.zone, ) if self.arena not in ["A", "B"]: raise Exception("arena must be A or B") print("Waiting for start signal.") # noqa:T001 if self.mode == 'comp': # Interact with the supervisor "robot" to wait for the start of the match. self.webot.setCustomData('ready') while ( self.webot.getCustomData() != 'start' and self.webots_step_and_should_continue(self._timestep) ): pass def _init_devs(self) -> None: "Initialise the attributes for accessing devices" # Motor boards self._init_motors() # Ruggeduinos self._init_ruggeduinos() # Camera self._init_camera() def _init_motors(self) -> None: self.motors = motor.init_motor_array(self.webot) def _init_ruggeduinos(self) -> None: self.ruggeduinos = ruggeduino.init_ruggeduino_array(self.webot) def _init_camera(self) -> None: # See comment in Camera.see for why we need to pass the step lock here. self.camera = camera.Camera(self.webot, self._step_lock) self.see = self.camera.see def time(self) -> float: """ Roughly equivalent to `time.time` but for simulation time. """ return self.webot.getTime() def sleep(self, secs: float) -> None: """ Roughly equivalent to `time.sleep` but accounting for simulation time. """ # Checks that secs is positive or zero if secs < 0: raise ValueError('sleep length must be non-negative') # Ensure the time delay is a valid step increment, while also ensuring # that small values remain nonzero. n_steps = math.ceil((secs * 1000) / self._timestep) duration_ms = n_steps * self._timestep # Assume that we're in the main thread here, so we don't really need to # do any cleanup if Webots tells us the simulation is terminating. When # webots kills the process all the proper tidyup will happen anyway. self.webots_step_and_should_continue(duration_ms)
#!/usr/bin/env python3 from controller import Robot, Lidar # Initialize the robot robot = Robot() timestep = int(robot.getBasicTimeStep()) # Initialize motors motor_left = robot.getDevice('wheel_left_joint') motor_right = robot.getDevice('wheel_right_joint') lidar = robot.getDevice('Hokuyo URG-04LX-UG01') position = robot.getDevice('gps') poi_list = [] poi_string_list = robot.getCustomData().split() for i in range(10): poi_element = [ float(poi_string_list[2 * i]), float(poi_string_list[2 * i + 1]) ] poi_list.append(poi_element) position.enable(timestep) lidar.enable(timestep) lidar_size = lidar.getHorizontalResolution() lidar_max_range = lidar.getMaxRange() motor_left.setPosition(float('inf')) motor_right.setPosition(float('inf'))