18-В1 Розно 1,2,3,4 лабы сети и 1,2,3 роботы

robs/lb1/Розно/ObstacleAvoidance.py

@@ -0,0 +1,83 @@
+from controller import Robot, Compass
+
+# Get reference to the robot.
+robot = Robot()
+
+# get robot's Compass device
+compass = robot.getCompass("compass")
+
+# 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)
+
+# enable the Compass
+compass.enable(timeStep)
+
+# Disable motor PID control mode.
+leftMotor.setPosition(float('inf'))
+rightMotor.setPosition(float('inf'))
+
+# Set ideal motor velocity.
+initialVelocity = maxMotorVelocity
+
+while robot.step(timeStep) != -1:
+    # to read values
+    values = compass.getValues()
+
+    # Read values from four distance sensors and calibrate.
+    outerLeftSensorValue = outerLeftSensor.getValue() / distanceSensorCalibrationConstant
+    centralLeftSensorValue = centralLeftSensor.getValue() / distanceSensorCalibrationConstant
+    centralSensorValue = centralSensor.getValue() / distanceSensorCalibrationConstant
+    centralRightSensorValue = centralRightSensor.getValue() / distanceSensorCalibrationConstant
+    outerRightSensorValue = outerRightSensor.getValue() / distanceSensorCalibrationConstant
+
+    # if there is an obstacle
+    if centralLeftSensorValue != 0.0 or centralSensorValue != 0.0 or centralRightSensorValue != 0.0 or outerRightSensorValue != 0.0 or outerLeftSensorValue != 0.0:
+        # if obstacle in left
+        if centralLeftSensorValue != 0.0 or centralSensorValue != 0.0 or outerLeftSensorValue != 0.0:
+            rightMotor.setVelocity(-initialVelocity)
+            leftMotor.setVelocity(initialVelocity)
+        # if obstacle in right
+        elif centralSensorValue != 0.0 or centralRightSensorValue != 0.0 or outerRightSensorValue != 0.0:
+            leftMotor.setVelocity(-initialVelocity)
+            rightMotor.setVelocity(initialVelocity)
+        # if obstacle in everywhere
+        elif centralLeftSensorValue != 0.0 and outerLeftSensorValue != 0.0 and centralRightSensorValue != 0.0 and outerRightSensorValue != 0.0:
+            leftMotor.setVelocity(-initialVelocity)
+            rightMotor.setVelocity(-initialVelocity)
+    # if there isnt an obstacle
+    else:
+        # displacement of the vision towards the line
+        if (round(values[0], 3) > 0.001) and (
+                centralLeftSensorValue == 0.0 and centralSensorValue == 0.0 and outerLeftSensorValue == 0.0 and centralRightSensorValue == 0.0 and outerRightSensorValue == 0.0):
+            rightMotor.setVelocity(initialVelocity / 1.1)
+            leftMotor.setVelocity(initialVelocity)
+        elif (round(values[0], 3) < 0.001) and (
+                centralLeftSensorValue == 0.0 and centralSensorValue == 0.0 and outerLeftSensorValue == 0.0 and centralRightSensorValue == 0.0 and outerRightSensorValue == 0.0):
+            leftMotor.setVelocity(initialVelocity / 1.1)
+            rightMotor.setVelocity(initialVelocity)
+        # if everything is alright then just move
+        else:
+            leftMotor.setVelocity(initialVelocity)
+            rightMotor.setVelocity(initialVelocity)

robs/lb1/Розно/SquarePath.py

@@ -0,0 +1,67 @@
+"""my_square_path controller."""
+
+"""Sample Webots controller for the square path benchmark."""
+
+from controller import Robot
+import math
+
+# Get pointer to the robot.
+robot = Robot()
+
+# Get pointer to each wheel of our robot.
+leftWheel = robot.getMotor('left wheel')
+rightWheel = robot.getMotor('right wheel')
+
+rightWheelSensor = robot.getPositionSensor('right wheel sensor')
+rightWheelSensor.enable(16)
+
+leftWheelSensor = robot.getPositionSensor('left wheel sensor')
+leftWheelSensor.enable(16)
+
+diamWheel = 0.195
+distWheels = 0.33
+sensorValue = 0
+l = math.pi * diamWheel
+# Max velocity
+MAX_SPEED = 5.24
+
+# Repeat the following 4 times (once for each side).
+for i in range(0, 4):
+
+    leftWheel.setPosition(1000)
+    rightWheel.setPosition(1000)
+    robot.step(16)
+
+    while rightWheelSensor.getValue() * diamWheel / 2.0 < sensorValue + 2.0:
+        if rightWheelSensor.getValue() * diamWheel / 2.0 > sensorValue + 1.9:
+            leftWheel.setVelocity(0.6 * MAX_SPEED)
+            rightWheel.setVelocity(0.6 * MAX_SPEED)
+        robot.step(160)
+
+    if i == 0:
+        leftWheel.setPosition(
+            leftWheelSensor.getValue() + (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi + 0.09)
+        rightWheel.setPosition(
+            rightWheelSensor.getValue() - (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi - 0.04)
+    elif i == 1:
+        leftWheel.setPosition(
+            leftWheelSensor.getValue() + (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi + 0.06)
+        rightWheel.setPosition(
+            rightWheelSensor.getValue() - (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi - 0.05)
+    elif i == 2:
+        leftWheel.setPosition(
+            leftWheelSensor.getValue() + (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi + 0.11)
+        rightWheel.setPosition(rightWheelSensor.getValue() - (math.pi / 2.0 * distWheels / 2.0) / l * 2.0 * math.pi)
+    elif i == 3:
+        continue
+    robot.step(912)
+
+    sensorValue = rightWheelSensor.getValue() * diamWheel / 2
+
+    leftWheel.setVelocity(MAX_SPEED)
+    rightWheel.setVelocity(MAX_SPEED)
+
+# Stop the robot when path is completed, as the robot performance
+# is only computed when the robot has stopped.
+leftWheel.setVelocity(0)
+rightWheel.setVelocity(0)

robs/lb2/СмирновДормидонтовПетровРозно/HighwayDriving.py

@@ -0,0 +1,105 @@
+"""Sample Webots controller for highway driving benchmark."""
+
+from vehicle import Driver
+
+# name of the available distance sensors
+sensorsNames = [
+    'front',
+    'front right 0',
+    'front right 1',
+    'front right 2',
+    'front left 0',
+    'front left 1',
+    'front left 2',
+    'rear',
+    'rear left',
+    'rear right',
+    'right',
+    'left']
+sensors = {}
+
+maxSpeed = 80
+driver = Driver()
+driver.setSteeringAngle(0.0)  # go straight
+
+# get and enable the distance sensors
+for name in sensorsNames:
+    sensors[name] = driver.getDistanceSensor('distance sensor ' + name)
+    sensors[name].enable(10)
+
+# get and enable the GPS
+gps = driver.getGPS('gps')
+gps.enable(10)
+
+# get the camera
+# camera = driver.getCamera('camera')
+# uncomment those lines to enable the camera
+# camera.enable(50)
+# camera.recognitionEnable(50)
+
+while driver.step() != -1:
+    # adjust the speed according to the value returned by the front distance sensor
+    frontDistance = sensors['front'].getValue()
+    frontRange = sensors['front'].getMaxValue()
+    rleftDistance = sensors['rear left'].getValue()
+    rleftRange = sensors['rear left'].getMaxValue()
+    leftDistance = sensors['left'].getValue()
+    leftRange = sensors['left'].getMaxValue()
+    rightDistance = sensors['right'].getValue()
+    rightRange = sensors['right'].getMaxValue()
+    fleft2Distance = sensors['front left 2'].getValue()
+    fleft2Range = sensors['front left 2'].getMaxValue()
+    fright0Distance = sensors['front right 0'].getValue()
+    fright0Range = sensors['front right 0'].getMaxValue()
+    fright1Distance = sensors['front right 1'].getValue()
+    fright1Range = sensors['front right 1'].getMaxValue()
+    fright2Distance = sensors['front right 2'].getValue()
+    fright2Range = sensors['front right 2'].getMaxValue()
+    fleft0Distance = sensors['front left 0'].getValue()
+    fleft0Range = sensors['front left 0'].getMaxValue()
+    fleft1Distance = sensors['front left 1'].getValue()
+    fleft1Range = sensors['front left 1'].getMaxValue()
+    r = rightDistance / rightRange
+    rl = rleftDistance / rleftRange
+    l = leftDistance / leftRange
+    fl2 = fleft2Distance / fleft2Range
+    fl0 = fleft0Distance / fleft0Range
+    fl1 = fleft1Distance / fleft1Range
+    fr0 = fright0Distance / fright0Range
+    fr1 = fright1Distance / fright1Range
+    fr2 = fright2Distance / fright2Range
+    fr = frontDistance / frontRange
+    speed = maxSpeed * frontDistance / frontRange
+    driver.setCruisingSpeed(speed)
+    # print(rl,'|',l,'|',fl2)
+    # brake if we need to reduce the speed
+    speedDiff = driver.getCurrentSpeed() - speed
+    if speedDiff > 0:
+        driver.setBrakeIntensity(min(speedDiff / speed, 1))
+    else:
+        driver.setBrakeIntensity(0)
+
+    if ((l > 0.6) and (fl2 > 0.6)):
+        if (r > 0.5):
+            angle1 = 0.02 * (
+                        -1 + (1 - fl2) + (1 - fl0) + (1 - fl1) - (1 - fr) * 4 - (1 - fr1) - (1 - fr0) - (1 - fr2) - (
+                            1 - r))
+            driver.setSteeringAngle(angle1)
+        else:
+            angle1 = 0.03 * (-1 + (1 - fl2) + (1 - fl0) + (1 - fl1) - (1 - fr) * 4 - (1 - fr1) - (1 - fr0) * 5 - (
+                        1 - fr2) - (1 - r))
+            driver.setSteeringAngle(angle1)
+    else:
+        if ((l > 0.2) or (fl2 > 0.4)):
+            angle1 = 0.025 * (
+                        (1 - l) + (1 - fl2) + (1 - fl0) + (1 - fl1) + (1 - fr) * 4 - (1 - fr2) * 4 - (1 - fr1) * 4 - (
+                            1 - fr0) * 4 - (1 - r) * 2 - rl * 0.25)
+            driver.setSteeringAngle(angle1)
+        elif ((l < 0.2) or (fl2 < 0.4)):
+            angle = 0.025 * (
+                        (1 - l) + (1 - fl2) + (1 - fl1) + (1 - fl0) - (1 - fr2) - (1 - fr1) - (1 - fr0) + (1 - fr) - (
+                            1 - r) + (1 - rl))
+            if (angle < 0.0):
+                driver.setSteeringAngle(0.01)
+            else:
+                driver.setSteeringAngle(angle)