progress

oving_3/main.py

@@ -9,6 +9,7 @@
 
 from rtttl_player import RTTTLPlayer
 from random import randint
+import time
 
 
 
@@ -38,7 +39,7 @@
 #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=110)
 
 startBtn = TouchSensor(port=Port.S1)
@@ -47,11 +48,28 @@
 leftColorSensor = ColorSensor(port=Port.S2)
 rightColorSensor = ColorSensor(port=Port.S4)
 
+BLACK = 9
+WHITE = 85
+threshold = (BLACK + WHITE) / 2
+
+lastCorrectino = time.time()
+lastDirection = 1
+
 while True:
-    if leftColorSensor.color() == Color.BLACK:
-        robotDriveBase.turn(5)
+    if time.time() - lastCorrectino > 1:
+        while leftColorSensor.reflection() > 10 or rightColorSensor.reflection() < 10:
+            robotDriveBase.drive(-100, 50 * lastDirection)
 
-    elif rightColorSensor.color() == Color.BLACK:
-        robotDriveBase.turn(-5)
+    if leftColorSensor.reflection() < 10:
+        robotDriveBase.drive(-100, 50)
+        lastCorrectino == time.time()
+        lastDirection = 1
+        continue
 
-    DriveBase.run(100)
+    elif rightColorSensor.reflection() < 10:
+        robotDriveBase.drive(-100, -50)
+        lastCorrectino = time.time()
+        lastDirection = -1
+        continue
+
+    robotDriveBase.drive(-100, 50)