from pybricks.ev3devices import Motor, UltrasonicSensor, GyroSensor
from pybricks.robotics import DriveBase
from pybricks.parameters import Port, Direction
import time as t
# Klasse for å kjøre roboten fremover, bakover, rotere og rette opp roboten
class Drive:
# Konstruktør for klassen Drive
# Tar inn venstre og høyre motor, ultrasonisk sensor og gyro-sensor
def __init__(self):
self.left_motor = Motor(Port.D)
self.right_motor = Motor(Port.A)
self.ultrasonic_sensor = UltrasonicSensor(Port.S3)
self.gyro_sensor = GyroSensor(Port.S2, Direction.CLOCKWISE)
self.robot = DriveBase(self.left_motor, self.right_motor, 40, 100)
self.ultrasonic_distance = 50 # Avstand fra bakken til ultrasonisk sensor
# Metode for å kjøre roboten fremover
# Tar inn tid som parameter og kjører roboten fremover i den tiden
# Hvis tiden er mindre enn 0 eller lik, kjører roboten kontinuerlig fremover
def drive_forward(self, time):
if time > 0:
self.robot.drive(100, 0)
t.sleep(time)
self.robot.stop()
else:
self.robot.drive(100, 0)
# Metode for å kjøre roboten bakover
def drive_backward(self, time):
if time > 0:
self.robot.drive(-100, 0)
t.sleep(time)
self.robot.stop()
else:
self.robot.drive(-100, 0)
# Method for aligning the robot
def align(self):
self.robot.stop()
# Hvis roboten ser et fall, snur den seg til den ser bakken igjen
while True:
if self.ultrasonic_sensor.distance() > self.ultrasonic_distance:
self.left_motor.run(50)
self.right_motor.run(-50)
# Når roboten ser bakken igjen, henter den vinkelen til gyro-sensoren
if self.ultrasonic_sensor.distance() < self.ultrasonic_distance:
angle = self.gyro_sensor.angle()
new_angle = angle
print(angle)
# Roboten snur seg til den ser rett mot fallkanten
while angle < new_angle + 24:
angleCurrent = new_angle - angle
self.left_motor.run(-50)
self.right_motor.run(50)
new_angle = self.gyro_sensor.angle()
print(angleCurrent)
self.drive_backward(1)
self.robot.stop()
break
# Metode for å rotere roboten 180 grader
def rotate_180(self):
angle_current = 0
angle_start = self.gyro_sensor.angle()
while (angle_current < 180):
self.left_motor.run(-200)
self.right_motor.run(200)
angle_current = angle_start - self.gyro_sensor.angle()
self.left_motor.stop()
self.right_motor.stop()