diff --git a/py/src/drive.py b/py/src/drive.py
index dd3f031..0d12463 100644
--- a/py/src/drive.py
+++ b/py/src/drive.py
@@ -10,7 +10,7 @@ class Drive():
     def __init__(self, motors: tuple, dimensions: tuple):
         leftMotor, rightMotor = motors
         wheelDiameter, axleTrack = dimensions
-        
+         
 
         self.driveBase = DriveBase(leftMotor, rightMotor, 
                         wheel_diameter=wheelDiameter, axle_track=axleTrack)
@@ -20,14 +20,14 @@ def forward(self, speed):
         self.driveBase.drive(speed, 0)
 
     def backward(self, speed):
-        self.driveBase.drive(speed, 0 )
+        self.driveBase.drive(-abs(speed), 0 )
 
-    def left(self, speed, angle):
+    def left(self, speed):
         self.driveBase.drive(speed, 90)
     
-    def right(self, speed, angle):
-        self.driveBase.drive(speed, -90)
+    def right(self, speed):
+        self.driveBase.turn(90)
 
-    def stop(self, speed):
+    def stop(self, brakeType):
         self.driveBase.drive(0,0)
     
\ No newline at end of file
diff --git a/py/src/main.py b/py/src/main.py
index 0e09bfc..71fcee3 100644
--- a/py/src/main.py
+++ b/py/src/main.py
@@ -7,7 +7,7 @@
 from pybricks.robotics import DriveBase
 from pybricks.media.ev3dev import SoundFile, ImageFile
 
-
+ 
 try:
     from drive import Drive
     from sensors import Sensors
@@ -23,17 +23,15 @@
 # This program requires LEGO EV3 MicroPython v2.0 or higher.
 # Click "Open user guide" on the EV3 extension tab for more information.
 
-dimensions = ( 20,   # Wheel diamter
-               20 ) #Axle track
+dimensions = ( 47,   # Wheel diamter
+               140 ) #Axle track
 
 motors = (Motor(port=Port.D),      # Left motor for the drive base
           Motor(port=Port.C))      # Right motor for the drive base
 
 sensors = (ColorSensor(port=Port.S1),       # Lyssensor for å måle fargen på jorda
-           UltrasonicSensor(port=Port.S4))  # Ultrasonisksensor for å sjekke for potensielle hindringer robotten må navigere unna
+           UltrasonicSensor(port=Port.S2))  # Ultrasonisksensor for å sjekke for potensielle hindringer robotten må navigere unna
 
 robotDrive = Drive(motors, dimensions)  # Robottens kjørefunksjonalitet
-robotSensors = Sensors(sensors)         # Robottens sensorfunksjonalitet
-
-robot = Robot(robotDrive, robotSensors) # EV3 robot
+robot = Robot(robotDrive, *sensors) # EV3 robot
 robot.start()
\ No newline at end of file
diff --git a/py/src/robot.py b/py/src/robot.py
index 6984787..3416a91 100644
--- a/py/src/robot.py
+++ b/py/src/robot.py
@@ -9,20 +9,42 @@
 
 from drive import Drive
 from sensors import Sensors
+from sensors import UltraSonic
 
 class Robot():
-    def __init__(self, drive: Drive, sensors: Sensors):
+    def __init__(self, drive: Drive, colorSensor, ultrasonicSensor):
         self.hub = EV3Brick()
         self.drive = drive
-        self.sensors = sensors
+        self.color = colorSensor
+        self.ultrasonic = ultrasonicSensor
+        
+ 
+    def start(self):
+        self.speed = 150
+        self.drive.forward(self.speed)
 
-    def start(self, duration_ms = 5000):
-        self.drive.forward(50)
-        timer = StopWatch()
-        while timer.time() < duration_ms:
+        while True:
             wait(50)
+
+            UltrasonicSensor = UltraSonic(Port.S2)
+
+            if UltrasonicSensor is None:
+                continue
             
-        self.stop()
+            if UltrasonicSensor.distanceLessThan(100):                
+                self.drive.stop(0)
+                self.hub.speaker.beep()
+                wait(1000)
+                self.drive.backward(self.speed)
+                wait(1000)
+                self.drive.right(self.speed)
+                wait(1000)
+                self.drive.forward(self.speed)
+                wait(1000)
+                self.drive.right(self.speed)
+                wait(1000)
+                self.drive.forward(self.speed)
+    
 
     def stop(self):
         self.drive.forward(0)
diff --git a/py/src/sensors.py b/py/src/sensors.py
index dfe179b..d3f83ae 100644
--- a/py/src/sensors.py
+++ b/py/src/sensors.py
@@ -27,13 +27,19 @@ def __init__(self, sensorPort):
         self.sensor = UltrasonicSensor(port=sensorPort)
 
     def distanceLessThan(self, value):
-        if self.sensor.distance() < value:
-            return True
-        else:
-            return False
-
-
-class  Color():
+        try:
+            return self.sensor.distance() < value
+        except OSError:
+        # kort pause og ett nytt forsøk
+            wait(50)
+            try:
+                return self.sensor.distance() < value
+            except OSError:
+            # Logg/varsle, og anta "for nær" for sikkerhet
+                return True
+
+
+class Color():
     def __init__(self, sensorPort):
         self.sensor = ColorSensor(port=sensorPort)