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)
