Follow_Line_Pro_Edition

#!/usr/bin/env pybricks-micropython

from pybricks import ev3brick as brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
                                 SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
from ev3dev2.motor import (OUTPUT_B,OUTPUT_C,LargeMotor,MediumMotor,SpeedPercent,MoveTank,MoveSteering,MoveDifferential,SpeedRPM,MotorSet)
from ev3dev2.wheel import EV3EducationSetTire
from math import pi


"""


Et program som søker etter en hvit linje ved hjelp av rotasjoner og spiralmønster for EV3-educational.robot
Programmet gjør en kombinasjon av høyre/venstre-søk, fullrotasjon-søk og firkantet spiral-søk

"""

COLOR_SENSOR = ColorSensor(Port.S3)
ROBOT_TANK = MoveTank(OUTPUT_B,OUTPUT_C)
RIGHT_MOTOR = Motor(Port.C, Direction.CLOCKWISE)
LEFT_MOTOR = Motor(Port.B, Direction.CLOCKWISE)
#ultrasonic_sensor = UltrasonicSensor(Port.S4)
#gyro = GyroSensor(Port.S2,Direction.CLOCKWISE)
#touch_sensor =  TouchSensor(Port.S1)


TURN_DEG = 50 #Antall grader høyre og venstre sving skal være i høyre/venstre-søk
SPEED = 15 #Hastighet på rotasjoner og kjøring, prosent
INIT_DIST=10 #Distanse de 2 første strekningene i spiralen, i cm
ADD_DIST=10 #Økningen av distansen for hver andre strekning, i cm
LEFT_TURN_FIRST=False #True, start med venstresving. False, start med høyresving.
ROTATE_AND_SPIRAL_SEARCH=True #True, start søket med full rotasjon og spiralsøk. False, start søket med høyre/venstre-søk
SKIP_FIRST_ROTATION=True # Ved første rotasjon og spiral-søk: True, gå rett på spiralsøk. False, utfør en full rotasjon, før spiralsøk

DIAMETER = 56 #Diameter på hjul i mm
CIRCUMFERENCE = pi*DIAMETER/10 #Omkrets på hjul i cm
FIRST_TURN_DEG=TURN_DEG*2 # 180 grader rotasjon på motorene svarer til 90 grader.
NUMBER_OF_ROTATIONS = INIT_DIST/CIRCUMFERENCE #Antall rotasjoner av hjulet for å forflytte bilen INIT_DIST
NUMBER_OF_DEG = NUMBER_OF_ROTATIONS*360 #Antall grader hjulet må rotere for å oppnå ønsket antall rotasjoner, dermed forflytte seg INIT_DIST



def spiral_search():
    global SPEED, INIT_DIST, ADD_DIST, NUMBER_OF_DEG, NUMBER_OF_ROTATIONS, CIRCUMFERENCE
    if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
        return
    while True:
        RIGHT_MOTOR.reset_angle(0)
        while RIGHT_MOTOR.angle()<=NUMBER_OF_DEG: #Kjør rett frem til antall grader er lik NUMBER_OF_DEG
            ROBOT_TANK.on(SPEED,SPEED)
            if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
                return
        ROBOT_TANK.on_for_rotations(SPEED,-SPEED,0.5) #Svinger 90 grader mot høyre
        LEFT_MOTOR.reset_angle(0)
        while LEFT_MOTOR.angle()<=NUMBER_OF_DEG: #Kjør rett frem til antall grader er lik NUMBER_OF_DEG
            ROBOT_TANK.on(SPEED,SPEED)
            if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
                return
        INIT_DIST=INIT_DIST+ADD_DIST #Oppdaterer kjørelengden
        NUMBER_OF_ROTATIONS = INIT_DIST/CIRCUMFERENCE #Oppdaterer antall rotasjoner
        NUMBER_OF_DEG = NUMBER_OF_ROTATIONS*360 #Oppdaterer antall grader
        ROBOT_TANK.on_for_rotations(SPEED,-SPEED,0.5) #Svinger 90 grader mot høyre

def white_search():
    global SPEED, LEFT_TURN_FIRST, ROTATE_AND_SPIRAL_SEARCH, FIRST_TURN_DEG, SKIP_FIRST_ROTATION
    while True:
        while COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Hvis du ser hvit, kjør rett frem
            ROBOT_TANK.on(SPEED,SPEED)

        if LEFT_TURN_FIRST and not ROTATE_AND_SPIRAL_SEARCH: #Sving venstre først
            RIGHT_MOTOR.reset_angle(0)
            while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and RIGHT_MOTOR.angle()<=FIRST_TURN_DEG: #Rotasjon mot venstre basert på FIRST_TURN_DEG, se ved definisjonen av FIRST_TURN_DEG
                ROBOT_TANK.on(-SPEED,SPEED)
            LEFT_MOTOR.reset_angle(0)
            while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and LEFT_MOTOR.angle()<=2*FIRST_TURN_DEG:#Rotasjon mot høyre basert på 2*FIRST_TURN_DEG, se ved definisjonen av FIRST_TURN_DEG
                ROBOT_TANK.on(SPEED,-SPEED)
                LEFT_TURN_FIRST=False
            if COLOR_SENSOR.color()!=Color.WHITE: #Registrer at ingenting ble funnet, utfør ROTATE_AND_SPIRAL_SEARCH + spiralsøk
                ROTATE_AND_SPIRAL_SEARCH=True

        elif not LEFT_TURN_FIRST and not ROTATE_AND_SPIRAL_SEARCH: #Sving høyre først
            LEFT_MOTOR.reset_angle(0)
            while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and LEFT_MOTOR.angle()<=FIRST_TURN_DEG:#Rotasjon mot høyre basert på FIRST_TURN_DEG, se ved definisjonen av FIRST_TURN_DEG
                ROBOT_TANK.on(SPEED,-SPEED)
            RIGHT_MOTOR.reset_angle(0)
            while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and RIGHT_MOTOR.angle()<=2*FIRST_TURN_DEG: #Rotasjon mot venstre, basert på 2*FIRST_TURN_DEG, se ved definisjonen av FIRST_TURN_DEG
                ROBOT_TANK.on(-SPEED,SPEED)
                LEFT_TURN_FIRST=True
            if COLOR_SENSOR.color()!=Color.WHITE: #Registrer at ingenting ble funnet, utfør ROTATE_AND_SPIRAL_SEARCH + spiralsøk
                ROTATE_AND_SPIRAL_SEARCH=True

        elif ROTATE_AND_SPIRAL_SEARCH:
            RIGHT_MOTOR.reset_angle(0)
            LEFT_MOTOR.reset_angle(0)

            if LEFT_TURN_FIRST and not SKIP_FIRST_ROTATION: #Roter 360 grader mot venstre
                while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and RIGHT_MOTOR.angle()<=720:
                    ROBOT_TANK.on(-SPEED,SPEED)

            elif not LEFT_TURN_FIRST and not SKIP_FIRST_ROTATION: #Roter 360 grader mot høyre
                while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and LEFT_MOTOR.angle()<=720:
                    ROBOT_TANK.on(SPEED,-SPEED)

            spiral_search() #Kjør spiralsøk
            ROTATE_AND_SPIRAL_SEARCH=False #Nullstiller, slik at neste søk blir høyre/venstre.
            SKIP_FIRST_ROTATION=False #Nullstiller, slik at roboten alltid søker med 360 rotasjon før spiralsøk fra nå av

white_search()