def run(fun):
# Connect two motors and two (different) light sensors
mL = LargeMotor('outC')
mR = LargeMotor('outB')
sL = ColorSensor('in1')
sR = ColorSensor('in4')
gy = GyroSensor('in3')
# Check if the sensors are connected
assert sL.connected, "Left ColorSensor is not connected"
assert sR.connected, "Right ColorSensor is not conected"
assert gy.connected, "Gyro is not connected"
gyro = Gyro()
light_sensors = LightSensors()
encoder = Encoder()
# Set the motor mode
mL.polarity = "normal" # "inversed"
mR.polarity = "normal" # "inversed"
mL.position = 0
mR.position = 0
def stop_motors():
mL.run_direct()
mR.run_direct()
mL.duty_cycle_sp = 0
mR.duty_cycle_sp = 0
stop_motors()
# The example doesn't end on its own.
# Use CTRL-C to exit it (needs command line).
# This is a generic way to be informed
# of this event and then take action.
def signal_handler(sig, frame):
stop_motors()
print('Shut down gracefully')
exit(0)
# Install the signal handler for CTRL+C
signal(SIGINT, signal_handler)
print('Press Ctrl+C to exit')
per = {
'mL': mL,
'mR': mR,
'sL': sL,
'sR': sR,
'gy': gy
}
light_sensors.init(per)
gyro.init(gy)
# Endless loop reading sensors and controlling motors.
last_log = time()
last_now = time()
max_dt = 0
dts = 0
speed_mL = None
speed_mR = None
while True:
state = {}
state = light_sensors(per, state)
state = encoder(per, state)
state = gyro(per, state)
state = fun(per, state)
max_speed = 40 * TICKS_PER_CM
_speed_mL = state.get('mL', 0)
if _speed_mL != speed_mL:
speed_mL = _speed_mL
mL.run_forever(speed_sp=speed_mL/100 * max_speed)
_speed_mR = state.get('mR', 0)
if _speed_mR != speed_mR:
speed_mR = _speed_mR
mR.run_forever(speed_sp=speed_mR/100 * max_speed)
dts += 1
now = time()
dt = now - last_now
last_now = now
if dt > max_dt: max_dt = dt
if now - last_log > 5.0:
last_log = now
print("avg fps: ", dts / 5.0)
print('min fps: ', 1 / max_dt)
max_dt = 0
dts = 0
import time
from ev3dev.ev3 import LargeMotor, ColorSensor, UltrasonicSensor, Sound, GyroSensor
from enum import Enum, IntEnum
from pickled import pickled
from ring import RingBuf
from util import SubinstructionError
# -- MOTORS --
mLeft = LargeMotor('outD')
mRight = LargeMotor('outC')
mRight.polarity = 'inversed'
mLeft.polarity = 'inversed'
# -- SENSORS --
sSonic = UltrasonicSensor('in1')
sSonic.mode = UltrasonicSensor.MODE_US_DIST_CM
sGyro = GyroSensor('in2')
sGyro.mode = GyroSensor.MODE_GYRO_ANG
cLeft = ColorSensor('in3')
cRight = ColorSensor('in4')
""" Marks which side of the robot the line is on. """
class MoveDir(IntEnum):
LINE_LEFT = -1
LINE_RIGHT = 1
