def main():
coords = [0, 0]
left.reset_angle(0)
right.reset_angle(0)
Kp, Ki, Kd, i, last_error, target = 20, 0, 0, 0, 0, 5
conveyor_belt = Motor(Port.A)
directions_made = []
stopwatch = StopWatch()
stopwatch.resume()
driving_forward = False
while True:
if stopwatch.time() >= 100000: # OUR LAST HOPE FOR POINTS!!!!!
robot.stop()
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
break
if lightSensor.refelctivity() <= 500:
backward(1000)
if cSensor.color() == Color.BLUE:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
conveyor_belt.run_time(-100, 1000, Stop.BRAKE, False)
foundvictim()
if LeftSensor.distance() >= 40:
wait(500)
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
leftturn()
directions_made.append("left turn")
robot.drive_time(-1000, 0, 1000)
directions_made.append(("forward", 1000))
elif ForwardSensor.distance() <= 300:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
rightturn()
directions_made.append("right turn")
else:
robot.drive(-1000, 0)
driving_forward, start_time = True, stopwatch.time()
for direction in directions_made:
if direction == "left turn":
robot.stop()
rightturn()
elif direction == "right turn":
robot.stop()
leftturn()
else:
robot.drive_time(-1000, 0, direction[1])
def wait_until_bumped(self, wait_timer=500):
"""Wait until the TouchSensor is bumped
:param wait_timer: Time to wait (milliseconds) to consider a press and release a bump,
defaults to 500
:type wait_timer: int, float, optional
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_pressed()
my_watch.reset()
my_watch.resume()
self.wait_until_released()
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
def wait_until_button_bumped(self, button, channel, wait_timer=500):
"""Waits until a specified button has been bumped
''NOTE: using a list, tuple or dict for buttons will make this function act the same as
wait_until_button_pressed if all of the buttons listed are pressed at once''
:param button: Button or Buttons to wait for
:type button: Button, list, tuple, dict
:param channel: Channel number of the remote
:type channel: int
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_button_pressed(button, channel)
my_watch.reset()
my_watch.resume()
self.wait_until_button_released(button, channel)
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
class FUNCTION_LIBRARY:
def __init__(self, robot, ev3, left_motor, right_motor, medium_motor,
color_sensor_1, color_sensor_2):
#self, DriveBase, Hub
self.driveBase = robot
self.hub = ev3
self.left_motor = left_motor
self.right_motor = right_motor
self.medium_motor = medium_motor
self.left_motor.reset_angle(0)
self.left_motor.reset_angle(0)
self.stopWatch = StopWatch()
self.color_sensor_1 = color_sensor_1
self.color_sensor_2 = color_sensor_2
def shutdown(self):
self.hub.speaker.say(
"Logic error, error error error error error error error error error error errorrr Non halting program detected, shutting down"
)
#self.hub.speaker.say("Shutting down...")
self.hub.speaker.play_notes(['C4/4', 'F3/4', 'F2/4'])
def line_follow_until_black(self,
p=1.2,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
sensor_stop=-1,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_1
if (sensor_stop == -1):
sensor_stop = self.color_sensor_2
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if sensor_stop.reflection() <= BLACK: #
self.driveBase.stop()
break #thrillitup
def line_follow_until_white(self,
p=1.2,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
sensor_stop=-1,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_1
if (sensor_stop == -1):
sensor_stop = self.color_sensor_2 # I NEED TO CREATE NEW CONSTANt
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if sensor_stop.reflection() <= WHITE: #
self.driveBase.stop()
break #thrillitup
def line_follow_for_time(self,
p=1,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
time=10000,
debug=False):
if (sensor_lf == -1):
sensor_lf = self.color_sensor_2
self.stopWatch.reset()
self.stopWatch.resume()
PROPORTIONAL_GAIN = p
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
threshold = (BLACK + WHITE) / 2 #the center of black+white
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if self.stopWatch.time() > time: #
break #STOP THIEF
self.driveBase.stop()
self.hub.speaker.say("I line followed for" + str(floor(time / 1000)) +
"seconds")
def line_follow_for_distance(self,
p=1,
DRIVE_SPEED=100,
BLACK=9,
WHITE=85,
sensor_lf=-1,
distance=10000,
debug=False):
#BLACK = 9 #what is black
#WHITE = 85 #what is white, also what is life (42)
if (sensor_lf == -1):
sensor_lf = self.color_sensor_2
elif (sensor_lf == -2):
sensor_lf = self.color_sensor_1
PROPORTIONAL_GAIN = p
threshold = (BLACK + WHITE) / 2 #the center of black+white
startingDistance = self.driveBase.distance()
while True: #forever, do
if (debug):
print(sensor_lf.reflection()
) #how bright the stuff the color sensor sees is
#Calculate the turn rate from the devation and set the drive base speed and turn rate.
self.driveBase.drive(
DRIVE_SPEED,
PROPORTIONAL_GAIN * (sensor_lf.reflection() - threshold))
#stop condition
if self.driveBase.distance() - startingDistance > distance: #
break #STOP THIEF
self.driveBase.stop()
##self.hub.speaker.say("I have reached " + str(floor(distance/25.4)) + "inches") removed this, pauses robot too long
def mm_to_inch(self, mm):
return mm / 25.4
def inch_to_mm(self, inch):
return inch * 25.4
def ms_to_second(self, ms):
return ms / 1000
def second_to_ms(self, s):
return s * 1000
liftMotor.reset_angle(0)
robot = DriveBase(leftMotor, rightMotor, wheelDiametr, baseWidth)
lineSensor = ColorSensor(Port.S2)
colorSensor = ColorSensor(Port.S3)
crossSensor = ColorSensor(Port.S1)
# PD coefficients
gray = 40
kp = 0.3
kd = 0.08
timer = StopWatch()
timer.reset()
timer.resume()
class PD:
def __init__(self, p, d):
self.deltaOld = None
self.p = p
self.d = d
self.outputPowerMin = 0
self.outputPowerMax = 960 # TODO: подобрать значения для моторов LEGO
self.inputPowerMin = 0
self.inputPowerMax = 100
def error(self, delta):
if self.deltaOld == None:
self.deltaOld = delta
#ultraSensor = UltrasonicSensor(Port.S3)
leftColorSensor = ColorSensor(Port.S1)
rightColorSensor = ColorSensor(Port.S4)
# Button pressed
#up_pressed = Button.Up in brick.buttons()
programRunning = True
# Speed
minSpeed = 100
maxSpeed = 400
# Clock
clockLine = StopWatch()
clockLine.reset()
clockLine.resume()
durationLine = 0
clockDist = StopWatch()
clockDist.reset()
clockDist.resume()
durationDist = 0
# Distance array
dist = []
while programRunning:
print("GO")
# Timer
if clockLine.time() > durationLine:
speed = random.randint(minSpeed, maxSpeed)
# Die passenden Parameter müsst ihr selbst durch Tests heraus finden!
# Tipp: Zu Beginn setzt nur kP = 1 und den Rest auf Null.
# Anschließend könnt ihr andere Parametereinstellungen durchführen.
# kP: Parameter des P-Anteils
# kI: Parameter des I-Anteils
# kD: Parameter des D-Anteils
# Initialisieung einer Liste, um vergangene Abstandsmessungen zu speichern.
# Diese ist notwendig damit am Ende, der durchschnittliche Abstand zum Vordermann bestimmt werden kann
dist = []
# Initialisierung eines Timers, um kontinuierlich den Abstand zum Vordermann zu messen
# und ihn in der Liste zu speichern.
clockDist = StopWatch()
clockDist.reset()
clockDist.resume()
durationDist = 0
programRunning = True
while programRunning:
# 4. ToDo: Implementierung der Linien-Verfolgung
# Sobald eines der beiden Farbsensoren die Linie detektiert, soll der passende Motor stoppen.
# Ansonsten sollen beide Motoren mit der aktuellen Geschwindigkeit laufen.
# Abstandsmessung zum Vordermann (distance) und speichern in der Liste
if clockDist.time() > durationDist:
distance = ultraSensor.distance()
dist.append(distance)
durationDist = 500
clockDist.reset()
robot = DriveBase(leftmotor, rightmotor, 40, 200)
color = ColorSensor(Port.S1)
uart = UARTDevice(Port.S4, 9600, timeout=1000)
direction = 1
#initial waiting phase:
holder = b'a'
motorspeed = 0
minispeed = 0
filename = 'letters.csv'
fobj = open(filename, 'w')
for up in range(20):
print(up)
watch.reset()
watch.resume()
while watch.time() < 200:
robot.drive(20, 0)
robot.drive(0, 0)
for side in range(80):
watchside.reset()
watchside.resume()
while watchside.time() < 150:
minimotor.run(direction * -50)
colorcheck(side, up, direction)
direction *= -1
while True:
colorcheck()
direction = 1
if uart.waiting() != 0:
holder = uart.read(1).decode("utf-8")
#Set up a queue for bricks on the conveyor belt
brickQueue = []
count = 0
reflectMean = 0
reflectMeanNum = 0
wasBrick = 0
meanSize = 10
numStep = 0
redPos = 0
trashPos = 180
currPos = 0
bucketMotor.reset_angle(0)
time.reset()
time.resume()
while True:
hopperMotor.dc(90)
conveyorMotor.run(-35)
reflect = light.reflection()
if count % meanSize:
reflectMeanNum += reflect
else:
reflectMean = reflectMeanNum / meanSize
# Average every 10 readings
reflectMean = reflectMeanNum / meanSize
if reflectMean > 0.5:
if wasBrick == 0:
sendData = bytes('1', 'utf-8')
