Test

main.py

@@ -18,37 +18,43 @@ giroSensor = GyroSensor(Port.S1)
 
 robot = DriveBase(left_motor, right_motor, wheel_diameter=55.5, axle_track=124)
 
-#Metodo para el giro
-def giro(grados):
-    robot.reset()
-    giroSensor.reset_angle(0)
-    robot.turn(grados)
-    while(giroSensor.angle() != grados):
-        diferencia = giroSensor.angle() - grados
-        ajuste = diferencia * -1        
-        for i in range(abs(ajuste)):
-            if(ajuste > 0):
-                robot.turn(1)
-            else: 
-                if (ajuste < 0):
-                    robot.turn(-1)
-
-def sigue_linea_verde():
-    # Valores medios para los colores
-    green_threshold = (70 + 100) / 2  # Valor medio para el color verde claro
-    blue_threshold = (0 + 20) / 2     # Valor medio para el color azul oscuro
 
+# Variables para almacenar los valores de calibración de color.
+green_rgb = (35, 55, 39)
+black_rgb = (8, 4, 19)
+tolerancia = 5
+
+# Función para determinar si un color es verde basado en la calibración.
+def is_green(rgb):
+    if (rgb[0] >= (green_rgb[0]-tolerancia) and rgb[0] <= (green_rgb[0]+tolerancia)) and (rgb[1] >= (green_rgb[1]-tolerancia) and rgb[1] <= (green_rgb[1]+tolerancia)) and (rgb[2] >= (green_rgb[2]-tolerancia) and rgb[2] <= (green_rgb[2]+tolerancia)):
+      return True
+    return False
+
+def is_black(rgb):
+    if (rgb[0] >= (black_rgb[0]-tolerancia) and rgb[0] <= (black_rgb[0]+tolerancia)) and (rgb[1] >= (black_rgb[1]-tolerancia) and rgb[1] <= (black_rgb[1]+tolerancia)) and (rgb[2] >= (black_rgb[2]-tolerancia) and rgb[2] <= (black_rgb[2]+tolerancia)):
+      return True
+    return False
+      
+
+# Contador de líneas negras.
+black_line_count = 0
+
+
+# Función para seguir la línea verde.
+def follow_green_line():
     while True:
-        reflection = colorSensor.reflection()
-        if reflection > green_threshold:  # Si está sobre la línea verde
-            desviacion = reflection - green_threshold
-            turn = 1.2 * desviacion # Correccion hacia la
-        elif reflection < blue_threshold:  # Si está sobre la línea azul
-            desviacion = blue_threshold - reflection
-            turn = -1.2 * desviacion  # Corrección hacia la izquierda
-        else:  # Si está entre la línea verde y la azul
-            turn = 0  # Avanzar recto
-
-        robot.drive(100, turn)
-
-        wait(10)
+        rgb = colorSensor.rgb()
+
+        # Si detecta verde, sigue adelante.
+        if is_green(rgb):
+            robot.drive(100, 0)
+
+        elif is_black(rgb):
+            robot.drive(50,0)
+        # Si detecta otro color (no verde), entonces buscará la línea verde girando.
+        else:
+            robot.drive(0, 50)
+
+# Comienza a seguir la línea verde.
+follow_green_line()
+