Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/usr/bin/env pybricks-micropython
- from pybricks import ev3brick as brick
- from pybricks.ev3devices import (Motor, ColorSensor)
- from pybricks.parameters import (Port, Stop, Direction, Color,
- SoundFile)
- from pybricks.tools import print, wait
- from pybricks.robotics import DriveBase
- baseWidth = 150
- wheelDiametr = 82
- rightMotor = Motor(Port.B, Direction.COUNTERCLOCKWISE)
- leftMotor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
- robot = DriveBase(leftMotor, rightMotor, wheelDiametr, baseWidth)
- # Write your program here
- brick.sound.beep()
- lineSensor = ColorSensor(Port.S2)
- colorSensor = ColorSensor(Port.S1)
- crossSensor = ColorSensor(Port.S3)
- class PD:
- def __init__(self, p, d):
- self.deltaOld = None
- self.p = p
- self.d = d
- def error(self, delta):
- if self.deltaOld == None:
- self.deltaOld = delta
- error = self.p*delta + (delta - self.deltaOld) * self.d
- self.deltaOld = delta
- return error
- gray = 14
- kp = 2
- kd = 2.25
- speed = 300
- pd = PD(kp, kd)
- def p_line():
- error = pd.error(lineSensor.reflection() - gray)
- leftMotor.run(speed+error)
- rightMotor.run(speed-error)
- while True:
- p_line()
- print(lineSensor.reflection())
- if colorSensor.color() == Color.RED:
- brick.sound.file(SoundFile.RED, 100)
- elif colorSensor.color() == Color.BLUE:
- brick.sound.file(SoundFile.BLUE, 100)
- elif colorSensor.color() == Color.GREEN:
- brick.sound.file(SoundFile.GREEN, 100)
- elif colorSensor.color() == Color.YELLOW:
- brick.sound.file(SoundFile.YELLOW, 100)
- elif colorSensor.color() == Color.WHITE:
- brick.sound.file(SoundFile.WHITE, 100)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement