class state_machine: """Finite state machine class""" global TASK_SEQ def __init__(self): """Initialize the finite state machine class""" self.handlers = {} self.start_state = None self.end_states = [] self.btn = Button() def add_state(self, name, handler, end_state=0): """Adds a state to the finite state machine""" name = name.upper() self.handlers[name] = handler if end_state: self.end_states.append(name) def set_start(self, name): """Sets the state as start state""" self.start_state = name.upper() def run(self, cargo): """Starts the finite state machine""" # make sure the state machine is set up correctly try: handler = self.handlers[self.start_state] except: raise Exception("Must call .set_start() before run()") if not self.end_states: raise Exception("At least one state must be an end_state") # add dummy char to task sequence TASK_SEQ = "S" + cargo while (True): # If button is pressed then stop if self.btn.any(): exit() (new_state, cargo) = handler(cargo) # if cargo != "": # print(cargo) # check if a end states is reaches if new_state.upper() in self.end_states: # print("Reached", new_state) line_follower.stop() play_music.sound() break else: # if new state is control take next task in task_sequence if new_state == "ctrl": TASK_SEQ = TASK_SEQ[1:] cargo = TASK_SEQ #print(cargo) # change handler to new_state's handler handler = self.handlers[new_state.upper()]
def randomWalk(tank: MoveDifferential, touchSensor: TouchSensor): btn = Button() while not btn.any(): if touchSensor.is_pressed: tank.off() else: tank.on(left_speed = 45, right_speed = 45)
def main(): '''The main function of our program''' # set the console just how we want it reset_console() set_cursor(OFF) set_font('Lat15-Terminus24x12') print('How are you?') print("") print("Hello Selina.") print("Hello Ethan.") STUD_MM = 8 tank = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16 * STUD_MM) motorLift = MediumMotor(OUTPUT_D) sound = Sound() # sound.speak('How are you master!') # sound.speak("I like my family") # sound.speak("I like my sister and i like my brother.") sound.beep() eye = InfraredSensor(INPUT_1) robot = Robot(tank, None, eye) botton = Button() while not botton.any(): distance = eye.distance(channel=1) heading = eye.heading(channel=1) print('distance: {}, heading: {}'.format(distance, heading)) motorLift.on_to_position(speed=40, position=-7200, block=True) #20 Rounds if distance is None: sound.speak("I am lost, there is no beacon!") else: if (distance < 14): tank.off() sound.speak("I am very close to the beacon!") motorLift.on_to_position(speed=40, position=7200, block=True) sound.speak("I had to get some more rubbish.") sound.speak("Please wait while I lift up my fork.") tank.turn_right(speed=20, degrees=random.randint(290, 340)) # random.randint(150, 210) tank.on_for_seconds(left_speed=20, right_speed=20, seconds=20) tank.turn_right(speed=20, degrees=330) motorLift.on_to_position(speed=40, position=0, block=True) elif distance >= 100: sound.speak("I am too faraway from the beacon") elif (distance < 99) and (-4 <= heading <= 4): # in right heading sound.speak("Moving farward") tank.on(left_speed=20, right_speed=20) else: if heading > 0: tank.turn_left(speed=20, degrees=20) else: tank.turn_right(speed=20, degrees=20) sound.speak("I am finding the beacon.") time.sleep(0.1)
def display_gyro_sensor(self, which_gyro_sensor): """Display reflected light intensity of said sensor""" gyro = self.choose_gyro_sensor(which_gyro_sensor) gyro.mode = gyro.MODE_GYRO_ANG btn = Button() angle = gyro.angle while True: self.write_to_console(str(angle), column=5, row=2, reset_console=True, inverse=True, alignment='C', font_size='L') angle = gyro.angle self.sleep_in_loop() if btn.any(): break
def display_color_sensor(self, which_color_sensor): """Display reflected light intensity of said sensor""" cs = self.choose_color_sensor(which_color_sensor) cs.MODE_COL_REFLECT = 'COL-REFLECT' btn = Button() light = cs.reflected_light_intensity while True: self.write_to_console(str(light), column=5, row=2, reset_console=True, inverse=True, alignment='C', font_size='L') light = cs.reflected_light_intensity self.sleep_in_loop() if btn.any(): break
def main(): # remove the following line and replace with your code btn = Button() # we will use any button to stop script cl = ColorSensor() cl.mode = 'COL-AMBIENT' while not btn.any(): # exit loop when any button pressed print("Ambient light reading:") print(cl.value()) sleep(1) # wait for 0.1 seconds
def read_from_color_sensor(self, which_color_sensor='right', read_white=True): """ Will show the value of the color sensor on the screen and return when any button is pressed TODO: Fix console display of light value which is changing size and location """ cs = self.choose_color_sensor(which_color_sensor) cs.MODE_COL_REFLECT = 'COL-REFLECT' btn = Button() light = cs.reflected_light_intensity while True: self.write_to_console(str(light), column=5, row=2, reset_console=True, inverse=(not read_white), alignment='C', font_size='L') light = cs.reflected_light_intensity self.sleep_in_loop() if btn.any(): break return light
class WhereAmI: def __init__(self): self.server_mac = '88:B1:11:79:C5:F6' self.port = 4 self.us = UltrasonicSensor() self.btn = Button() def run(self): s = bluetooth.BluetoothSocket(bluetooth.RFCOMM) s.connect((self.server_mac, self.port)) while True: text = str(self.us.distance_centimeters) if self.btn.any(): break s.send(text) print(text) time.sleep(0.1) s.close()
'left': 3, 'right': 2, 'enter': 4, 'backspace': 6 } button = Button() leds = Leds() display = Display() leds.all_off() display.clear() t = time() while time() - t < 1: display.text_grid( f"Button pressed: {button.any()}, Button number: {button.get_pressed_button_number}" ) print(button.any(), button.get_pressed_button_number) # works with numbers if button.wait_for_pressed(6): print("6 pressed") display.text_grid("6 pressed") leds.set_color('RIGHT', (0, 0, 1)) # and names if button.wait_for_pressed('up'): print('up pressed') display.text_grid("up pressed")
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_A, OUTPUT_B from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import ColorSensor from ev3dev2.button import Button from time import sleep cl = ColorSensor(INPUT_1) btn = Button() steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B) target = 25 # while not btn.any(): # Stop program by pressing any button # error = target - cl.reflected_light_intensity # turn = error * 1.5 # steer_pair.on(turn, 20) # sleep(0.2) while not btn.any(): # Stop program by pressing any button steer_pair.on(0, 50) sleep(0.2)
'''.format(left_cs.reflected_light_intensity, center_cs.reflected_light_intensity, right_cs.reflected_light_intensity)) if left_cs.reflected_light_intensity > 15 >= center_cs.reflected_light_intensity \ and right_cs.reflected_light_intensity > 15: straight() else: if not turn_left() and not turn_right( ) and center_cs.reflected_light_intensity > 15: conn.send( '------------------------------ OFF THE LINE ------------------------------' ) turn_right() if last_bl_cs is left_cs else turn_left( ) if last_bl_cs is right_cs else straight() three = [sensor.driver_name for sensor in list_sensors()].count('lego-ev3-color') == 3 logger_conn, motor_conn = Pipe(False) p = Process(target=log_data, args=(logger_conn, logger_cs)) p.start() while not btn.any(): follow_line_three(motor_conn) if three else follow_line(motor_conn) motors.off() motor_conn.send(None) p.join() print('Process joined')
#!/usr/bin/env micropython import time from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_D from ev3dev2.button import Button btn = Button() tank_pair = MoveTank(OUTPUT_A, OUTPUT_D) while True: if btn.any(): break tank_pair.on(left_speed=10, right_speed=20) time.sleep(0.05) tank_pair.off()
from ev3dev2.sensor.lego import GyroSensor from ev3dev2.button import Button from time import sleep import logging logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)5s: %(message)s') log = logging.getLogger(__name__) log.info('Starting GyroSensor Reader Program') any_button = Button() gyro_sensor = GyroSensor() cs = ColorSensor() try: while not any_button.any(): angle = gyro_sensor.angle intensity = cs.reflected_light_intensity log.info('Angle in Degree ' + str(angle) + ' Reflected light intensity ' + str(intensity)) #gyro_sensor.reset() gyro_sensor.wait_until_angle_changed_by(90) change_in_angle = abs(gyro_sensor.angle - angle) log.info('Angle Changed by ' + str(change_in_angle)) sleep(0.5)
class Ev3Robot: def __init__(self, wheel1=OUTPUT_B, wheel2=OUTPUT_C, wheel3=OUTPUT_A, wheel4=OUTPUT_D): self.steer_pair = MoveSteering(wheel1, wheel2) self.gyro = GyroSensor() self.tank_pair = MoveTank(wheel1, wheel2) self.motor1 = LargeMotor(wheel1) self.motor2 = LargeMotor(wheel2) self.motor3 = MediumMotor(wheel3) self.motor4 = MediumMotor(wheel4) self.color1 = ColorSensor(INPUT_1) self.color4 = ColorSensor(INPUT_4) self._black1 = 0 self._black4 = 0 self._white1 = 100 self._white4 = 100 self.gyro.mode = 'GYRO-ANG' self.led = Leds() self.btn = Button() self.btn._state = set() def write_color(self, file, value): # opens a file f = open(file, "w") # writes a value to the file f.write(str(value)) f.close() def read_color(self, file): # opens a file f = open(file, "r") # reads the value color = int(f.readline().strip()) f.close() return color def pivot_right(self, degrees, speed=20): # makes the robot pivot to the right until the gyro sensor # senses that it has turned the required number of degrees self.tank_pair.on(left_speed=speed, right_speed=0) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def pivot_left(self, degrees, speed=20): # makes the robot pivot to the left until the gyro sensor # senses that it has turned the required number of degrees self.tank_pair.on(left_speed=0, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees - 10) self.tank_pair.off() def old_spin_right(self, degrees, speed=20): #old program; don't use self.gyro.reset() value1 = self.gyro.angle self.tank_pair.on(left_speed=speed, right_speed=speed * -1) self.gyro.wait_until_angle_changed_by(degrees) value2 = self.gyro.angle self.tank_pair.on(left_speed=-30, right_speed=30) self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees) self.stop() def old_spin_left(self, degrees, speed=20): #old program; don't use value1 = self.gyro.angle self.tank_pair.on(left_speed=speed * -1, right_speed=speed) self.gyro.wait_until_angle_changed_by(degrees) value2 = self.gyro.angle self.tank_pair.on(left_speed=8, right_speed=-8) self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5) self.tank_pair.off() def dog_gear(self, degrees, motor, speed=30): if motor == 3: self.motor3.on_for_degrees(degrees=degrees, speed=speed) self.motor3.off() else: self.motor4.on_for_degrees(degrees=degrees, speed=speed) def go_straight_forward(self, cm, speed=20, wiggle_factor=1): value1 = self.motor1.position angle0 = self.gyro.angle rotations = cm / 19.05 #divides by circumference of the wheel # calculates the amount of degrees the robot has turned, then turns the # opposite direction and repeats until the robot has gone the required # number of centimeters while abs(self.motor1.position - value1) / 360 < rotations: angle = self.gyro.angle - angle0 self.steer_pair.on(steering=angle * wiggle_factor * -1, speed=speed * -1) self.steer_pair.off() def go_straight_backward(self, cm, speed=20, wiggle_factor=1): # see go_straight_forward value1 = self.motor1.position angle0 = self.gyro.angle rotations = cm / 19.05 while abs(self.motor1.position - value1) / 360 < rotations: angle = self.gyro.angle - angle0 self.steer_pair.on(steering=angle * wiggle_factor, speed=speed) self.steer_pair.off() def calibrate(self): # turns the led lights orange, and waits for a button to be pressed # (signal that the robot is on black), then calculates the reflected # light intensity of the black line, then does the same on the white line self.led.set_color('LEFT', 'ORANGE') self.led.set_color('RIGHT', 'ORANGE') while not self.btn.any(): sleep(0.01) self.led.set_color('LEFT', 'GREEN') self.led.set_color('RIGHT', 'GREEN') self._black1 = self.color1.reflected_light_intensity self._black4 = self.color4.reflected_light_intensity sleep(2) self.led.set_color('LEFT', 'ORANGE') self.led.set_color('RIGHT', 'ORANGE') while not self.btn.any(): sleep(0.01) self.led.set_color('LEFT', 'GREEN') self.led.set_color('RIGHT', 'GREEN') self._white1 = self.color1.reflected_light_intensity self._white4 = self.color4.reflected_light_intensity sleep(3) self.write_color("/tmp/black1", self._black1) self.write_color("/tmp/black4", self._black4) self.write_color("/tmp/white1", self._white1) self.write_color("/tmp/white4", self._white4) def read_calibration(self): # reads the color files self._black1 = self.read_color("/tmp/black1") self._black4 = self.read_color("/tmp/black4") self._white1 = self.read_color("/tmp/white1") self._white4 = self.read_color("/tmp/white4") def align_white(self, speed=20, t=96.8, direction=1): # goes forward until one of the color sensors sees the white line. while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI( self.color4) < t: self.steer_pair.on(steering=0, speed=speed * direction) self.steer_pair.off() # determines which sensor sensed the white line, then moves the opposite # motor until both sensors have sensed the white line if self.calibrate_RLI(self.color4) > t: while self.calibrate_RLI(self.color1) < t: self.motor1.on(speed=speed * direction) self.motor1.off() else: while self.calibrate_RLI(self.color4) < t: self.motor2.on(speed=speed * direction) self.motor2.off() def align_black(self, speed=20, t=4.7, direction=1): # see align_white while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI( self.color4) > t: self.steer_pair.on(steering=0, speed=speed * direction) self.steer_pair.off() if self.calibrate_RLI(self.color4) < t: while self.calibrate_RLI(self.color1) > t: self.motor1.on(speed=speed * direction) self.motor1.off() else: while self.calibrate_RLI(self.color4) > t: self.motor2.on(speed=speed * direction) self.motor2.off() def align(self, t, speed=-20, direction=1): # aligns three times for extra accuracy self.align_white(speed=speed, t=100 - t, direction=direction) self.align_black(speed=-5, t=t, direction=direction) self.align_white(speed=-5, t=100 - t, direction=direction) def calibrate_RLI(self, color_sensor): # returns a scaled value based on what black and white are if (color_sensor.address == INPUT_1): black = self._black1 white = self._white1 else: black = self._black4 white = self._white4 return (color_sensor.reflected_light_intensity - black) / (white - black) * 100 def stop(self): self.tank_pair.off() def spin_right(self, degrees, speed=30): self.turn(degrees, 100, speed) def spin_left(self, degrees, speed=30): self.turn(degrees, -100, speed) def turn(self, degrees, steering, speed=30): # turns at a decreasing speed until it turns the required amount of degrees value1 = self.gyro.angle s1 = speed d1 = 0 while d1 < degrees - 2: value2 = self.gyro.angle d1 = abs(value1 - value2) slope = speed / degrees s1 = (speed - slope * d1) * (degrees / 90) + 3 self.steer_pair.on(steering=steering, speed=s1) self.steer_pair.off()
motorSpeed = 30 # Motor speed (%) steeringValue = -100 # Steering value (to be used when turning around); goes from -100 to 100 steeringSpeed = 30 steeringDegrees = 0.5 minDistance = 20 # Minimum distance (in cm) before the brick starts decelerating or stops to turn around # _maxSpeed = 400 # Flag to be used as manual start/stop switch; default is False (brick does not move) started = False while True: # Brick LED is yellow when ready (waiting for a button press) leds.set_color('LEFT', 'YELLOW') leds.set_color('RIGHT', 'YELLOW') if button.any(): started = True leds.set_color('LEFT', 'GREEN') leds.set_color('RIGHT', 'GREEN') motor.on(SpeedPercent(motorSpeed), SpeedPercent(motorSpeed)) while started is True: if ultrasonic.distance_centimeters < minDistance: motor.off() # Stop motors death = 4 while ultrasonic.distance_centimeters < minDistance: steer.on_for_rotations(steeringValue, steeringSpeed, steeringDegrees) death -= 1 if (death == 0): started = False
class NekoNekoChan(object): def __init__(self): Config.update() self.sound = Sound() # sensor values self.sensLeft = ColorSensor(INPUT_1) self.sensRight = ColorSensor(INPUT_4) # TODO: Sensoren anschließen self.sensIR = InfraredSensor(INPUT_2) self.sensTouch = TouchSensor(INPUT_3) self.btn = Button() self.sensValues = {} # Classes for features self.drive = Drive() self.cross = Cross() # statemachine self.fsm = StateMachine() # adding States self.fsm.states["followLine"] = State("followLine") self.fsm.states["followLine"].addFunc(self.drive.followLine, self.sensValues) self.fsm.states["brake"] = State("brake") self.fsm.states["crossFirstTurn"] = State("crossFirstTurn") self.fsm.states["crossFirstTurn"].addFunc(self.cross.firstTurn, self.sensValues) self.fsm.states["checkNextExit"] = State("checkNextExit") self.fsm.states["checkNextExit"].addFunc(self.drive.followLine, self.sensValues) # adding Transitions self.fsm.transitions["toFollowLine"] = Transition("followLine") self.fsm.transitions["toBrake"] = Transition("brake") self.fsm.transitions["toBrake"].addFunc(self.drive.brake) self.fsm.transitions["toCrossFirstTurn"] = Transition("crossFirstTurn") def checkBlue(self): hue = self.sensValues["ColorLeft"][0] return hue > 0.4 and hue < 0.68 # TODO: measure best threshold for blue values def run(self): self.fsm.setState("followLine") while True: # update sensor values self.sensValues["ColorLeft"] = self.sensLeft.hls self.sensValues["ColorRight"] = self.sensRight.hls self.sensValues["IR"] = self.sensIR.proximity self.sensValues["Touch"] = self.sensTouch.is_pressed # copy current State curState = self.fsm.currentState #Debug.print(self.sensValues["ColorLeft"], self.sensValues["ColorRight"]) # TODO: find Lightness thresholds if self.btn.down: sleep(1) if self.btn.down: Config.update() self.drive.updateConfig() self.sound.beep() if curState == None: self.fsm.transition("toFollowLine") elif self.sensValues["ColorLeft"][1] < 10.0 or self.sensValues[ "ColorRight"][1] < 10.0: self.fsm.transition("toBrake") elif self.btn.any(): break elif curState.name != "followLine": self.fsm.transition("toFollowLine") """ # EmergencyStop TODO: Wert für Abgrund definieren # if self.sensValues["ColorLeft"] == ABGRUND or self.sensValues["ColorRight"] == ABGRUND: # self.fsm.transition("toBrake") # if clauses for changing state # calibrate sensors # wait for button press before starting # line following # intersection first turn # elif self.checkBlue(): # self.fsm.transition("toCrossFirstTurn") # detect ball # collect ball, turn around # intersection turn = entry turn """ self.fsm.execute() sleep(0.01)
class Robot: def __init__(self, output_a, output_d): self.right_motor = LargeMotor(output_a) self.left_motor = LargeMotor(output_d) self.btn = Button() self.cl = ColorSensor() self.us = UltrasonicSensor() self.us.mode='US-DIST-CM' self.gy = GyroSensor() self.gy.mode='GYRO-ANG' self.defaultSpeed = 450 self.right_sp = 450 self.left_sp = 450 self.lastColor1 = None self.lastColor2 = None self.lastColor3 = None self.node = [(0,0)] # self.direction = None self.directionStr = ['top', 'right', 'bottom', 'left'] def demarrer(self, vitessA, vitessD): self.move.on(vitessA, vitessD) self.speedA = vitessA self.speedD = vitessD def arrete(self): self.left_motor.stop(stop_action="coast") self.right_motor.stop(stop_action="coast") def arrete2(self): self.left_motor.stop(stop_action="brake") self.right_motor.stop(stop_action="brake") def drive(self): self.left_motor.run_forever(speed_sp=-self.left_sp) self.right_motor.run_forever(speed_sp=-self.right_sp) def drive_straight(self): self.left_motor.run_forever(speed_sp=-self.left_sp) self.right_motor.run_forever(speed_sp=-self.right_sp) sleep(0.6) self.arrete2() def turn_90(self, direction): motor_turns_deg = 360 if direction == 'left': motor_turns_deg = motor_turns_deg # May require some tuning depending on your surface! self.direction = (self.direction - 1) % 4 else: motor_turns_deg = -motor_turns_deg self.direction = (self.direction + 1) % 4 self.left_motor.run_forever(speed_sp=motor_turns_deg) self.right_motor.run_forever(speed_sp=-motor_turns_deg) begin = self.gy.value() while(abs(begin - self.gy.value()) < 85 and not self.btn.any()) : sleep(0.01) self.arrete2() def rotate(self): motor_turns_deg = -360 self.left_motor.run_forever(speed_sp=motor_turns_deg) self.right_motor.run_forever(speed_sp=-motor_turns_deg) wait = 0 found = False while((self.cl.color == Brown or self.cl.color == Black )and wait < 5 and not self.btn.any()) : sleep(0.1) wait = wait + 1 if self.cl.color == White or self.cl.color == Red : #print('FOUND = TRUE') found = True break if found == False : #print('FOUND = FALSE') motor_turns_deg = 360 self.left_motor.run_forever(speed_sp=motor_turns_deg) self.right_motor.run_forever(speed_sp=-motor_turns_deg) while (not self.btn.any()): print('current color : '+ str(self.cl.color)) if self.cl.color == White or self.cl.color == Red : break sleep(0.1) self.arrete2() def fini(self): exit() def run(self): self.arrete() self.lastColor1 = self.cl.color self.lastColor2 = self.cl.color self.lastColor3 = self.cl.color self.direction = 0 x = 0 y = 0 i = 0 while not self.btn.any() : # exit loop when any button pressed if self.cl.color == Black or self.cl.color == Brown : self.rotate() i = i + 1 distance = self.us.value()/10 color = self.cl.color self.beginAngle = self.gy.value()%360 direction = None print("----------------------------------") # print("tour : " + str(i)) # # print("distance: " + str(distance)) # print("color1 : " + str(self.lastColor1)) # print("color2 : " + str(self.lastColor2)) # print("color : " + str(color)) print("gyro: " + str(self.beginAngle)) shouldTurn = False direction = "left" if (distance < 10): shouldTurn = True self.node.append((x,y)) # if (color != self.lastColor3): # self.lastColor1 = self.lastColor2 # self.lastColor2 = self.lastColor3 # self.lastColor3 = color # if (self.lastColor3 == self.lastColor1 # and self.lastColor3 != self.lastColor2 # and self.lastColor2 == Black): # shouldTurn = True # direction = "left" # self.lastColor1 = self.lastColor3 # self.lastColor2 = self.lastColor3 # elif (self.lastColor3 != self.lastColor1 # and self.lastColor3 != self.lastColor2 # and self.lastColor2 == Black): # shouldTurn = True # # direction = "right" # self.lastColor1 = self.lastColor3 # self.lastColor2 = self.lastColor3 # if (self.lastColor3 == White and self.lastColor1 == Red): # direction = "left" # elif (self.lastColor3 == White and self.lastColor1 == Red): # direction = "right" if (shouldTurn == True): self.turn_90(direction) pass else: self.drive() pass # sleep(5) self.arrete()
from ev3dev2.sensor.lego import UltrasonicSensor from ev3dev2.sensor.lego import GyroSensor from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_D from ev3dev2.button import Button import logging logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)5s: %(message)s') log = logging.getLogger(__name__) gs = GyroSensor() us = UltrasonicSensor() tp = MoveTank(OUTPUT_A, OUTPUT_D) button = Button() try: while not button.any(): while not button.any(): tp.on(left_speed=50, right_speed=50) dist = int(us.distance_centimeters_continuous) log.info(dist) if dist < 60: break tp.on_for_rotations(50, -50, 1.15) except (KeyboardInterrupt): print('interrupt') tp.on(left_speed=0, right_speed=0)
#!/usr/bin/env python3 # An EV3 Python (library v2) solution to Exercise 3 # of the official Lego Robot Educator lessons that # are part of the EV3 education software from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C from ev3dev2.sensor.lego import ColorSensor from ev3dev2.button import Button from time import sleep 5 btn = Button() # we will use any button to stop script tank_pair = MoveTank(OUTPUT_B, OUTPUT_C) # Connect an EV3 color sensor to any sensor port. cl = ColorSensor() while not btn.any(): # exit loop when any button pressed # if we are over the black line (weak reflection) if cl.reflected_light_intensity < 30: # medium turn right tank_pair.on(left_speed=50, right_speed=-10) else: # strong reflection (>=30) so over white surface # medium turn left tank_pair.on(left_speed=-10, right_speed=50) sleep(0.1) # wait for 0.1 seconds
no_preto = False antes_preto = False no_vazio = False aprender_cores = False pegar_cano = False ir_pro_gasoduto = False no_gasoduto = False buscar_novo_cano = False while waiting: if btn.any(): # Verifica se algum botão foi pressionado sound.beep() global waiting global meeting_area waiting = False meeting_area = True else: sleep(0.01) rgbmax_e = definir_rgbmax('esq') rgbmax_d = definir_rgbmax('dir') while True: while meeting_area: #começa aleatoriamente na meeting area #termina com os sensores de cor no vazio certo sound.beep()
def main(): sensLight = ColorSensor(INPUT_1) # sensColor = ColorSensor(INPUT_4) # sensColor.mode = sensColor.MODE_COL_COLOR btn = Button() sound = Sound() steerPair = MoveSteering(OUTPUT_B, OUTPUT_C) # lastColor = sensColor.color_name speed = 90 kP = 2 kI = 0.1 kD = 0.5 target = 30 error = 0 lastError = 0 turn = 0 errorAccu = 0 integral = 0 sound.tone([(400, 400, 20), (800, 800, 20)]) while not btn.any(): sleep(0.1) sound.beep() sleep(2) debug_print('Error; Proportial; Integral; Derivative; Turn') while not btn.any(): lightValue = sensLight.reflected_light_intensity # colorName = sensColor.color_name # detect edge if lightValue == 0: steerPair.off() sleep(0.01) continue error = target - lightValue errorAccu += error derivative = error - lastError # limit integral if errorAccu > 200: errorAccu = 200 elif errorAccu < -200: errorAccu = -200 # apply gains proportional = error * kP integral = errorAccu * kI derivative *= kD # calculate turn value turn = proportional + integral + derivative # limit turn value if turn > 100: turn = 100 elif turn < -100: turn = -100 steerPair.on(int(turn * 1), speed) debug_print('{}; {}; {}; {}; {}'.format(int(error), int(proportional), int(integral), int(derivative), int(turn))) # if lastColor != colorName: # sound.speak(colorName) lastError = error # set current error to lastError at the end of the loop # lastColor = sensColor.color_name sleep(0.02)
class run2019: def __init__(self): self.btn = Button() self.LLight = LightSensor(INPUT_1) self.RLight = LightSensor(INPUT_4) self.cs = ColorSensor() self.drive = MoveTank(OUTPUT_A, OUTPUT_D) self.steer = MoveSteering(OUTPUT_A, OUTPUT_D) self.heightmotor = LargeMotor(OUTPUT_B) self.clawactuator = MediumMotor(OUTPUT_C) os.system('setfont Lat15-TerminusBold14') self.speed = 40 self.sectionCache = 0 self.orient = {'N': "1", 'E': "1", 'S': "1", 'W': "1"} def sensordata(self): print("Left Light Sensor: ", end="", file=sys.stderr) print(self.LLight.reflected_light_intensity, end=" ", file=sys.stderr) print("Right Light Sensor: ", end="", file=sys.stderr) print(self.RLight.reflected_light_intensity, file=sys.stderr) def turn(self, direc): # Half - works self.drive.on_for_degrees(SpeedDPS(225), SpeedDPS(225), 223) if direc == "L" or direc == "l": self.steer.on_for_degrees(steering=-100, speed=SpeedDPS(720), degrees=400) elif direc == "R" or direc == "r": self.steer.on_for_degrees(steering=100, speed=SpeedDPS(720), degrees=720) self.steer.off() def dti(self, speed, n, startCounting=False, sectionCache=0): # Drive to nth intersection kp = 1.1 ki = 0 kd = 0 integral = 0 perror = error = 0 inters = 0 piderror = 0 while not self.btn.any( ): # Remember to try stuff twice, this is a keyboard interrupt lv = self.LLight.reflected_light_intensity rv = self.RLight.reflected_light_intensity error = rv - lv integral += integral + error derivative = lv - perror piderror = (error * kp) + (integral * ki) + (derivative * kd) if speed + abs(piderror) > 100: if piderror >= 0: piderror = 100 - speed else: piderror = speed - 100 self.drive.on(left_speed=speed - piderror, right_speed=speed + piderror) sleep(0.01) perror = error # Drive up to nth intersection # These values are subject to change depending on outside factors, CHANGE ON COMPETITION DAY if (lv <= 50 and rv <= 55) or (lv <= 50 and rv >= 55) or ( lv >= 50 and rv <= 55): # Currently at an intersection inters += 1 if (startCounting == True): sectionCache += 1 if inters == n: # Currently at nth intersection self.drive.off() return self.drive.off() self.drive.on_for_seconds(SpeedDPS(115), SpeedDPS(115), 1) print("Left Value: {}, Right Value: {}, P error: {}, Inters: {}". format(lv, rv, piderror, inters), file=sys.stderr) def main(self): self.heightmotor.on(speed=self.speed) self.heightmotor.wait_until_not_moving() # # while not btn.any(): # # sensordata() # # ## STORING COLOURS self.drive.on_for_degrees( left_speed=self.speed, right_speed=self.speed, degrees=50) # To drive past little initial intersection print(self.orient, file=sys.stderr) #DEBUG self.turn("L") # # # GO TO FIRST BNPs self.dti(self.speed, 5, startCounting=True) self.turn("L") self.dti(self.speed, 1)
X_train = X_digits[:train_test_split] y_train = y_digits[:train_test_split] X_test = X_digits[train_test_split:] y_test = y_digits[train_test_split:] # ML function # Import the default K-Nearest Neighbors classifier knn = neighbors.KNeighborsClassifier(n_neighbors=5) # Train the classifer knn.fit(X_train, y_train) n = 7 while not btn.any(): # While no (not any) button is pressed. sleep(0.01) # Wait 0.01 second sound.beep() if cs.color == 2: sound.beep() #sound.speak(cs.color_name) n = 0 elif cs.color == 3: sound.beep() #sound.speak(cs.color_name) n = 1 elif cs.color == 4: sound.beep()
class Robot: def __init__(self): self.steer = MoveSteering(OUTPUT_B, OUTPUT_C) self.tank_pair = MoveTank(OUTPUT_B, OUTPUT_C) self.touch_sensor = TouchSensor() self.cl = ColorSensor() self.s = Sound() self.btn = Button() self.c_switch = True # True: Turning left, comp right, False: opposite. self.col_switch = True # True: black, False: white. self.tile_count = 0 self.tile_length = 230 # Sets up offset for variable light self.off_set = self.cl.reflected_light_intensity - 13 self.black_range = range(0, 30) self.white_range = range(30, 100) def move_degrees(self, degrees): self.tank_pair.on_for_degrees(left_speed=10, right_speed=10, degrees=degrees) def run(self): # Moves the robot off starting pad and onto black-white tiles self.tank_pair.on_for_degrees(left_speed=50, right_speed=50, degrees=90) while (self.cl.reflected_light_intensity in self.black_range): self.tank_pair.on(left_speed=20, right_speed=20) self.tank_pair.off() while not (self.cl.reflected_light_intensity in self.black_range): self.tank_pair.on(left_speed=20, right_speed=20) self.tank_pair.off() self.tank_pair.on_for_degrees(left_speed=50, right_speed=50, degrees=self.tile_length * 0.75) self.tank_pair.on_for_degrees(left_speed=20, right_speed=-20, degrees=180) while not (self.cl.reflected_light_intensity in self.white_range): self.tank_pair.on(left_speed=-20, right_speed=-20) self.tank_pair.off() self.tank_pair.on_for_degrees(left_speed=20, right_speed=20, degrees=self.tile_length * 0.25) current_tile = self.getColour() self.center_robot(current_tile) while not (self.cl.reflected_light_intensity in current_tile): self.tank_pair.on(left_speed=20, right_speed=20) self.tank_pair.off() #self.tank_pair.on(left_speed=20, right_speed=20) while (self.tile_count < 15): moved_degrees = 0 correct_alignment = True black_found = False while (True): while (self.cl.reflected_light_intensity in self.white_range): self.tank_pair.on() self.tank_pair.off() if (self.cl.reflected_light_intensity in self.black_range): black_found = True print("found black") break if (black_found): self.tile_count += 1 self.tank_pair.on(left_speed=20, right_speed=20) if (correct_alignment and self.tile_count > 1): self.move_degrees(self.tile_length * 0.25) turn_degrees_right = 0 turn_degrees_left = 0 while not (self.cl.reflected_light_intensity in self.white_range): self.tank_pair.on_for_degrees(left_speed=10, right_speed=-10, degrees=10) turn_degrees_right += 10 self.tank_pair.on_for_degrees(left_speed=-10, right_speed=10, degrees=turn_degrees_right) while not (self.cl.reflected_light_intensity in self.white_range): self.tank_pair.on_for_degrees(left_speed=-10, right_speed=10, degrees=10) turn_degrees_left += 10 if (turn_degrees_right < turn_degrees_left): self.tank_pair.on_for_degrees( left_speed=10, right_speed=-10, degrees=turn_degrees_left - turn_degrees_right) elif (turn_degrees_left < turn_degrees_right): self.tank_pair.on_for_degrees( left_speed=10, right_speed=-10, degrees=turn_degrees_right - turn_degrees_left) else: self.s.speak("This should never have happened.") self.tank_pair.on_for_degrees( left_speed=10, right_speed=-10, degrees=turn_degrees_left) else: while not (self.cl.reflected_light_intensity in self.white_range): pass self.tank_pair.off() if (moved_degrees > self.tile_length): self.realign() else: self.realign() ''' previous_tile_count = 0 saw_black_realign = False saw_black = True while (self.tile_count < 15): if not (t.is_alive()): # check for realignment if (previous_tile_count < self.tile_count): self.s.speak(str(self.tile_count)) previous_tile_count = self.tile_count if not (saw_black_realign): self.realign() t.start() saw_black_realign = False if (self.cl.reflected_light_intensity in self.white_range and saw_black): self.tile_count +=1 saw_black = False #self.tank_pair.off() #self.s.speak(str(self.tile_count)) #self.tank_pair.on(left_speed=20, right_speed=20) if (self.cl.reflected_light_intensity in self.black_range): saw_black = True saw_black_realign = True self.tank_pair.off() ''' ''' if (self.cl.reflected_light_intensity > 55 and self.col_switch): self.tank_pair.off() self.titleCount += 1 self.col_switch = False self.s.speak(str(self.titleCount)) self.tank_pair.on(left_speed=20, right_speed=20) if (self.cl.reflected_light_intensity < 20 and not self.col_switch): self.col_switch = True ''' def realign(self): # check side black is on self.s.speak("life is pain") # returns oposite colour def getColour(self): if (self.cl.reflected_light_intensity in self.black_range): return self.white_range elif (self.cl.reflected_light_intensity in self.white_range): return self.black_range def center_robot(self, colour): right_turn_count = 0 left_turn_count = 0 #bias = 1/2 while not (self.cl.reflected_light_intensity in colour): self.tank_pair.on_for_degrees(left_speed=10, right_speed=-10, degrees=10) right_turn_count += 1 #if (self.cl.reflected_light_intensity in self.gray_range): # bias = 3/4 self.tank_pair.on_for_degrees(left_speed=-10, right_speed=10, degrees=10 * right_turn_count) while not (self.cl.reflected_light_intensity in colour): self.tank_pair.on_for_degrees(left_speed=-10, right_speed=10, degrees=10) left_turn_count += 1 #if (self.cl.reflected_light_intensity in self.gray_range): # bias = 1/4 #if (abs(left_turn_count-right_turn_count) < 2): # bias = 1/2 self.tank_pair.on_for_degrees( left_speed=10, right_speed=-10, degrees=(10 * (right_turn_count + left_turn_count) / 2)) self.ninety_turn = 10 * (right_turn_count + left_turn_count) / 2 def length_of_tile(self): count = 0 while (self.cl.reflected_light_intensity in self.black_range): self.tank_pair.on_for_degrees(left_speed=10, right_speed=10, degrees=10) count += 1 self.s.speak(str(count * 10)) def checkColour(self): while not self.btn.any(): self.touch_sensor.wait_for_pressed() self.s.speak(str(self.cl.reflected_light_intensity))
#!/usr/bin/env/ python3 from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_D from ev3dev2.button import Button import time import logging logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)5s: %(message)s') log = logging.getLogger(__name__) tp = MoveTank(OUTPUT_A, OUTPUT_D) butt = Button() try: while not butt.any(): time.sleep(0.5) press = input("w to go forward, s to go backward, a to go right, d to go left, x to stop, t to end program") if press == "w": tp.on(left_speed = 50, right_speed = 50) print("going forward") elif press == "s": tp.on(left_speed = -50, right_speed = -50) print("going backward") elif press == "a": tp.on(left_speed = -50, right_speed = 50) print("going right") elif press == "d":
stdev = (var / 10)**0.5 mr = 0 c = 0 for i in range(10): if (media - 1.5 * stdev) < a[i] < (media + 1.5 * stdev): mr = mr + a[i] c = c + 1 if mr == 0: resultado = media else: resultado = mr / c return resultado while waiting: if btn.any(): # Checks if any button is pressed. sound.beep() # Wait for the beep to finish. global waiting waiting = False else: sleep(0.01) # Wait 0.01 second rgbmax_e = definir_rgbmax('esq') rgbmax_d = definir_rgbmax('dir') sound.speak('ok') #c = open('dist1.txt','w+') #c.close() # for i in range(100): # steering_pair.on_for_degrees(0,5,10)
print("object: ") print(objectTimeElapsed) isObject = False time.sleep(1) else: if isWater == True: print("WATER IS OFF") waterStop = time.time() waterTimeElapsed += waterStop - waterStart print("water: ") print(waterTimeElapsed) isWater = False time.sleep(1) if (btn.any()): print("WATER TRACKING ENDING..\n") isButton = True waterStop = time.time() waterTimeElapsed += waterStop - waterStart time.sleep(1) print("WATER RUNTIME TOTAL: ") print(waterTimeElapsed) print("WATER IN USE RUNTIME: ") print(objectTimeElapsed) print("WATER NOT IN USE RUNTIME: ") print(waterTimeElapsed - objectTimeElapsed, "\n") waterUsed = (objectTimeElapsed / waterTimeElapsed) * 100 print("WATER AMOUNT USED:")
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from ev3dev2.sensor.lego import ColorSensor from ev3dev2.button import Button from ev3dev2.sound import Sound from time import sleep cl = ColorSensor() btn = Button() sound = Sound() steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) sound.speak('Press the Enter key when the sensor is in dim light') btn.wait_for_bump('enter') dim = cl.ambient_light_intensity sound.beep() sound.speak('Press the Enter key when the sensor is in bright light') btn.wait_for_bump('enter') bright = cl.ambient_light_intensity sound.beep() sound.speak('3, 2, 1, go!') while not btn.any(): # Press any key to exit intensity = cl.ambient_light_intensity steer = (200 * (intensity - dim) / (bright - dim)) - 100 steer = min(max(steer, -100), 100) steer_pair.on(steering=steer, speed=30) sleep(0.1) # wait for 0.1 seconds
while not touch_sensor.is_pressed: pass wheels.on_for_rotations(0, SpeedPercent(50), 0.5) wheels.off() time.sleep(1) gyro.mode = GyroSensor.MODE_GYRO_CAL gyro.mode = GyroSensor.MODE_GYRO_RATE gyro.mode = GyroSensor.MODE_GYRO_ANG time.sleep(1) wheels.on(-100, SpeedPercent(15)) while gyro.angle > -180 or button.any(): pass wheels.on(0, SpeedPercent(-50)) while True: if touch_sensor.is_pressed: break wheels.on_for_rotations(0, SpeedPercent(50), 0.5) wheels.off() time.sleep(1) gyro.mode = GyroSensor.MODE_GYRO_CAL gyro.mode = GyroSensor.MODE_GYRO_RATE gyro.mode = GyroSensor.MODE_GYRO_ANG time.sleep(1)