SHARE
TWEET

Untitled

a guest Jan 27th, 2020 84 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, ColorSensor)
  5. from pybricks.parameters import (Port, Stop, Direction, Color,
  6.                                  SoundFile)
  7. from pybricks.tools import print, wait
  8. from pybricks.robotics import DriveBase
  9.  
  10. baseWidth = 150
  11. wheelDiametr = 82
  12.  
  13. rightMotor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
  14. leftMotor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
  15.  
  16. robot = DriveBase(leftMotor, rightMotor, wheelDiametr, baseWidth)
  17.  
  18. # Write your program here
  19. brick.sound.beep()
  20.  
  21. lineSensor = ColorSensor(Port.S2)
  22. colorSensor = ColorSensor(Port.S1)
  23. crossSensor = ColorSensor(Port.S3)
  24.  
  25. class PD:
  26.     def __init__(self, p, d):
  27.         self.deltaOld = None
  28.         self.p = p
  29.         self.d = d
  30.  
  31.     def error(self, delta):
  32.         if self.deltaOld == None:
  33.             self.deltaOld = delta
  34.        
  35.         error = self.p*delta + (delta - self.deltaOld) * self.d
  36.         self.deltaOld = delta
  37.         return error
  38.  
  39. gray = 14
  40. kp = 2
  41. kd = 2.25
  42. speed = 300
  43. pd = PD(kp, kd)
  44.  
  45. def p_line():
  46.    
  47.     error = pd.error(lineSensor.reflection() - gray)
  48.     leftMotor.run(speed+error)
  49.     rightMotor.run(speed-error)
  50.  
  51.  
  52. while True:
  53.    
  54.     p_line()
  55.     print(lineSensor.reflection())
  56.     if colorSensor.color() == Color.RED:
  57.         brick.sound.file(SoundFile.RED, 100)
  58.     elif colorSensor.color() == Color.BLUE:
  59.         brick.sound.file(SoundFile.BLUE, 100)
  60.     elif colorSensor.color() == Color.GREEN:
  61.         brick.sound.file(SoundFile.GREEN, 100)
  62.     elif colorSensor.color() == Color.YELLOW:
  63.         brick.sound.file(SoundFile.YELLOW, 100)
  64.     elif colorSensor.color() == Color.WHITE:
  65.         brick.sound.file(SoundFile.WHITE, 100)
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