Merge pull request #17 from IDATT1004-Team13-H2025/oving-3-follow-line

Oving 3 follow line

oving_3/main.py

@@ -9,6 +9,7 @@
 
 from rtttl_player import RTTTLPlayer
 from random import randint
+import time
 
 
 
@@ -33,13 +34,49 @@
 ]
 
 # Lag spiller og kjør
-player = RTTTLPlayer(ev3, songs[3])
+#player = RTTTLPlayer(ev3, songs[3])
 #ev3.screen.print("Spiller", player.name, "...")
 #player.play()
 
 leftMotor = Motor(port=Port.A)
-rightMotor = Motor(port=Port.B)
+rightMotor = Motor(port=Port.C)
+robotDriveBase = DriveBase(leftMotor, rightMotor, wheel_diameter=56, axle_track=160)
 
-robotDriveBase = DriveBase(leftMotor, rightMotor, wheel_diameter=56, axle_track=110)
+startBtn = TouchSensor(port=Port.S1)
+usSensor = UltrasonicSensor(port=Port.S3)
 
-robotDriveBase.drive(1000, 0)
+leftColorSensor = ColorSensor(port=Port.S2)
+rightColorSensor = ColorSensor(port=Port.S4)
+
+speaker = ev3.speaker
+
+leftSensorThreshold = 9
+rigthSensorThreshold = 9
+
+currentTime = time.time()
+lastCorrectino = currentTime
+lastDirection = 1
+
+while True:
+    currentTime = time.time()
+
+    if currentTime - lastCorrectino > 2:
+        while leftColorSensor.reflection() > 10 or rightColorSensor.reflection() < 10:
+            robotDriveBase.drive(-100, 50 * lastDirection)
+
+        lastCorrectino = time.time()
+        continue
+
+    if leftColorSensor.reflection() < leftSensorThreshold:
+        robotDriveBase.drive(-100, 25)
+        lastCorrectino == currentTime
+        lastDirection = 1
+        continue
+
+    if rightColorSensor.reflection() < rigthSensorThreshold:
+        robotDriveBase.drive(-100, -25)
+        lastCorrectino = currentTime
+        lastDirection = -1
+        continue
+
+    robotDriveBase.drive(-100, 50)