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')