SHARE
TWEET

Follow_Line_Pro_Edition

a guest Feb 15th, 2020 100 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env pybricks-micropython
  2.  
  3. from pybricks import ev3brick as brick
  4. from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
  5.                                  InfraredSensor, UltrasonicSensor, GyroSensor)
  6. from pybricks.parameters import (Port, Stop, Direction, Button, Color,
  7.                                  SoundFile, ImageFile, Align)
  8. from pybricks.tools import print, wait, StopWatch
  9. from pybricks.robotics import DriveBase
  10. from ev3dev2.motor import (OUTPUT_B,OUTPUT_C,LargeMotor,MediumMotor,SpeedPercent,MoveTank,MoveSteering,MoveDifferential,SpeedRPM,MotorSet)
  11. from ev3dev2.wheel import EV3EducationSetTire
  12. from math import pi
  13.  
  14.  
  15. """
  16.  
  17.  
  18. Et program som søker etter en hvit linje ved hjelp av rotasjoner og spiralmønster for EV3-educational.robot
  19. Programmet gjør en kombinasjon av høyre/venstre-søk, fullrotasjon-søk og firkantet spiral-søk
  20.  
  21. """
  22.  
  23. COLOR_SENSOR = ColorSensor(Port.S3)
  24. ROBOT_TANK = MoveTank(OUTPUT_B,OUTPUT_C)
  25. RIGHT_MOTOR = Motor(Port.C, Direction.CLOCKWISE)
  26. LEFT_MOTOR = Motor(Port.B, Direction.CLOCKWISE)
  27. #ultrasonic_sensor = UltrasonicSensor(Port.S4)
  28. #gyro = GyroSensor(Port.S2,Direction.CLOCKWISE)
  29. #touch_sensor =  TouchSensor(Port.S1)
  30.  
  31.  
  32. TURN_DEG = 50 #Antall grader høyre og venstre sving skal være i høyre/venstre-søk
  33. SPEED = 15 #Hastighet på rotasjoner og kjøring, prosent
  34. INIT_DIST=10 #Distanse de 2 første strekningene i spiralen, i cm
  35. ADD_DIST=10 #Økningen av distansen for hver andre strekning, i cm
  36. LEFT_TURN_FIRST=False #True, start med venstresving. False, start med høyresving.
  37. 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
  38. 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
  39.  
  40. DIAMETER = 56 #Diameter på hjul i mm
  41. CIRCUMFERENCE = pi*DIAMETER/10 #Omkrets på hjul i cm
  42. FIRST_TURN_DEG=TURN_DEG*2 # 180 grader rotasjon på motorene svarer til 90 grader.
  43. NUMBER_OF_ROTATIONS = INIT_DIST/CIRCUMFERENCE #Antall rotasjoner av hjulet for å forflytte bilen INIT_DIST
  44. NUMBER_OF_DEG = NUMBER_OF_ROTATIONS*360 #Antall grader hjulet må rotere for å oppnå ønsket antall rotasjoner, dermed forflytte seg INIT_DIST
  45.  
  46.  
  47.  
  48. def spiral_search():
  49.     global SPEED, INIT_DIST, ADD_DIST, NUMBER_OF_DEG, NUMBER_OF_ROTATIONS, CIRCUMFERENCE
  50.     if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
  51.         return
  52.     while True:
  53.         RIGHT_MOTOR.reset_angle(0)
  54.         while RIGHT_MOTOR.angle()<=NUMBER_OF_DEG: #Kjør rett frem til antall grader er lik NUMBER_OF_DEG
  55.             ROBOT_TANK.on(SPEED,SPEED)
  56.             if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
  57.                 return
  58.         ROBOT_TANK.on_for_rotations(SPEED,-SPEED,0.5) #Svinger 90 grader mot høyre
  59.         LEFT_MOTOR.reset_angle(0)
  60.         while LEFT_MOTOR.angle()<=NUMBER_OF_DEG: #Kjør rett frem til antall grader er lik NUMBER_OF_DEG
  61.             ROBOT_TANK.on(SPEED,SPEED)
  62.             if COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Avbryt søket hvis farge=hvit
  63.                 return
  64.         INIT_DIST=INIT_DIST+ADD_DIST #Oppdaterer kjørelengden
  65.         NUMBER_OF_ROTATIONS = INIT_DIST/CIRCUMFERENCE #Oppdaterer antall rotasjoner
  66.         NUMBER_OF_DEG = NUMBER_OF_ROTATIONS*360 #Oppdaterer antall grader
  67.         ROBOT_TANK.on_for_rotations(SPEED,-SPEED,0.5) #Svinger 90 grader mot høyre
  68.  
  69. def white_search():
  70.     global SPEED, LEFT_TURN_FIRST, ROTATE_AND_SPIRAL_SEARCH, FIRST_TURN_DEG, SKIP_FIRST_ROTATION
  71.     while True:
  72.         while COLOR_SENSOR.color()==Color.WHITE and COLOR_SENSOR.rgb()[2]>=80: #Hvis du ser hvit, kjør rett frem
  73.             ROBOT_TANK.on(SPEED,SPEED)
  74.  
  75.         if LEFT_TURN_FIRST and not ROTATE_AND_SPIRAL_SEARCH: #Sving venstre først
  76.             RIGHT_MOTOR.reset_angle(0)
  77.             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
  78.                 ROBOT_TANK.on(-SPEED,SPEED)
  79.             LEFT_MOTOR.reset_angle(0)
  80.             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
  81.                 ROBOT_TANK.on(SPEED,-SPEED)
  82.                 LEFT_TURN_FIRST=False
  83.             if COLOR_SENSOR.color()!=Color.WHITE: #Registrer at ingenting ble funnet, utfør ROTATE_AND_SPIRAL_SEARCH + spiralsøk
  84.                 ROTATE_AND_SPIRAL_SEARCH=True
  85.        
  86.         elif not LEFT_TURN_FIRST and not ROTATE_AND_SPIRAL_SEARCH: #Sving høyre først
  87.             LEFT_MOTOR.reset_angle(0)
  88.             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
  89.                 ROBOT_TANK.on(SPEED,-SPEED)
  90.             RIGHT_MOTOR.reset_angle(0)
  91.             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
  92.                 ROBOT_TANK.on(-SPEED,SPEED)
  93.                 LEFT_TURN_FIRST=True
  94.             if COLOR_SENSOR.color()!=Color.WHITE: #Registrer at ingenting ble funnet, utfør ROTATE_AND_SPIRAL_SEARCH + spiralsøk
  95.                 ROTATE_AND_SPIRAL_SEARCH=True
  96.  
  97.         elif ROTATE_AND_SPIRAL_SEARCH:
  98.             RIGHT_MOTOR.reset_angle(0)
  99.             LEFT_MOTOR.reset_angle(0)
  100.  
  101.             if LEFT_TURN_FIRST and not SKIP_FIRST_ROTATION: #Roter 360 grader mot venstre
  102.                 while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and RIGHT_MOTOR.angle()<=720:
  103.                     ROBOT_TANK.on(-SPEED,SPEED)
  104.  
  105.             elif not LEFT_TURN_FIRST and not SKIP_FIRST_ROTATION: #Roter 360 grader mot høyre
  106.                 while (COLOR_SENSOR.color() != Color.WHITE or not COLOR_SENSOR.rgb()[2]>=80) and LEFT_MOTOR.angle()<=720:
  107.                     ROBOT_TANK.on(SPEED,-SPEED)
  108.  
  109.             spiral_search() #Kjør spiralsøk
  110.             ROTATE_AND_SPIRAL_SEARCH=False #Nullstiller, slik at neste søk blir høyre/venstre.
  111.             SKIP_FIRST_ROTATION=False #Nullstiller, slik at roboten alltid søker med 360 rotasjon før spiralsøk fra nå av
  112.  
  113. white_search()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top