def __init__(
self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
motor_class=LargeMotor,
polarity: str = Motor.POLARITY_NORMAL,
ir_sensor_port: str = INPUT_4,
# sites.google.com/site/ev3devpython/learn_ev3_python/using-sensors
ir_beacon_channel: int = 1):
self.left_motor = LargeMotor(address=left_motor_port)
self.right_motor = LargeMotor(address=right_motor_port)
self.tank_driver = \
MoveTank(
left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=motor_class)
self.steer_driver = \
MoveSteering(
left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=motor_class)
self.left_motor.polarity = self.right_motor.polarity = \
self.tank_driver.left_motor.polarity = \
self.tank_driver.right_motor.polarity = \
self.steer_driver.left_motor.polarity = \
self.steer_driver.right_motor.polarity = polarity
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.tank_drive_ir_beacon_channel = ir_beacon_channel
def __init__(self,
jaw_motor_port: str = OUTPUT_A,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.jaw_motor = MediumMotor(address=jaw_motor_port)
self.left_motor = LargeMotor(address=left_motor_port)
self.right_motor = LargeMotor(address=right_motor_port)
self.tank_driver = MoveTank(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.steer_driver = MoveSteering(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
motor_class=LargeMotor)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
self.roaring = False
self.walk_speed = self.NORMAL_WALK_SPEED
def run(power, target, kp, kd, ki, direction, minRef, maxRef):
lastError = error = integral = 0
left_motor.run_direct()
right_motor.run_direct()
free_count = 0
isFree = False
while not btn.any() and not isFree:
# if ts.value():
# print ('Breaking loop') # User pressed touch sensor
# break
refRead = col.value()
error = target - (100 * (refRead - minRef) / (maxRef - minRef))
derivative = error - lastError
lastError = error
integral = float(0.5) * integral + error
course = (kp * error + kd * derivative + ki * integral) * direction
for (motor, pow) in zip((left_motor, right_motor),
steering2(course, power)):
motor.duty_cycle_sp = pow
free_count, isFree = freeSpot(free_count, isFree)
sleep(0.01) # Aprox 100Hz
left_motor.stop()
right_motor.stop()
sleep(2)
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
steer_pair.on_for_degrees(steering=0,
speed=15,
degrees=200,
brake=False,
block=True)
parking.back_parl_park(steer_pair)
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Robot state
self.patrol_mode = False
self.follow_mode = False
# Internal Variables
self.light_intensity = 0
self.batt_voltage = 0
# Connect two large motors on output ports B and C
#self.drive = MoveTank(OUTPUT_D, OUTPUT_C)
self.steerdrive = MoveSteering(OUTPUT_C, OUTPUT_D)
self.leds = Leds()
self.ir = InfraredSensor()
self.ir.mode = 'IR-SEEK'
self.touch = TouchSensor()
self.light = ColorSensor(address='ev3-ports:in4')
self.sound = Sound()
# Start threads
threading.Thread(target=self._patrol_thread, daemon=True).start()
threading.Thread(target=self._follow_thread, daemon=True).start()
threading.Thread(target=self._pat_thread, daemon=True).start()
threading.Thread(target=self._power_thread, daemon=True).start()
threading.Thread(target=self._light_sensor_thread, daemon=True).start()
class WalkDog:
def __init__(self, left_motor_port, right_motor_port, sensor_mode):
self._movesteering = MoveSteering(left_motor_port, right_motor_port)
self._ir = InfraredSensor()
self._ir.mode = sensor_mode
@property
def ir(self):
return self._ir
@ir.setter
def ir(self, ir):
self._ir = ir
@property
def movesteering(self):
return _movesteering
@movesteering.setter
def movesteering(self, ms):
self._movesteering = ms
def convert_heading_to_steering(self, heading):
return heading * 2
def run(self, channel=1):
beacon_distance = self._ir.distance(channel)
head_angle = self._ir.heading(channel)
steering = self.convert_heading_to_steering(head_angle)
if (beacon_distance > 30):
self._movesteering.on(steering, 50)
else:
self._movesteering.off()
def __init__(self,
distance_sensors: EV3DistanceDetectors,
gyro: Gyro,
logger=None,
**kwargs):
self._logger = logger or logging.getLogger(__name__)
self._distance_sensors = distance_sensors
self._gyro = gyro
self._motor_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self._position_corrector = PositionCorrector(self._motor_pair,
self._gyro)
self._maze_square_length_mm = KwArgsUtil.kwarg_or_default(
180, 'maze_square_length_mm', **kwargs)
self._move_forward_speed_rpm = KwArgsUtil.kwarg_or_default(
60, 'move_forward_speed_rpm', **kwargs)
self._motor_pair_polarity_factor = KwArgsUtil.kwarg_or_default(
1, 'motor_pair_polarity_factor', **kwargs)
self._turn_speed_rpm = KwArgsUtil.kwarg_or_default(
50, 'turn_speed_rpm', **kwargs)
self._wheel_diameter_mm = KwArgsUtil.kwarg_or_default(
56, 'wheel_diameter_mm', **kwargs)
self._wheel_circumference_mm = math.pi * self._wheel_diameter_mm
self._wheelbase_width_at_centers_mm = KwArgsUtil.kwarg_or_default(
97.5, 'wheelbase_width_at_centers_mm', **kwargs)
self._turns_until_next_angle_corretion = KwArgsUtil.kwarg_or_default(
3, 'turns_until_next_angle_corretion', **kwargs)
self._angle_corretcion_speed = KwArgsUtil.kwarg_or_default(
25, 'angle_corretcion_speed', **kwargs)
self._angle_correction_move_backward_mm = KwArgsUtil.kwarg_or_default(
80.0, 'angle_correction_move_backward_mm', **kwargs)
self._angle_correction_move_forward_mm = KwArgsUtil.kwarg_or_default(
20.0, 'angle_correction_move_forward_mm', **kwargs)
self._wait_for_motors_and_gyro_after_move_sec = KwArgsUtil.kwarg_or_default(
0.1, 'wait_for_motors_and_gyro_after_move_sec', **kwargs)
def __init__(self, baseSpeed=500, dt=50):
self.leftMotor = LargeMotor(OUTPUT_C)
self.rightMotor = LargeMotor(OUTPUT_A)
self.steeringDrive = MoveSteering(OUTPUT_C, OUTPUT_A)
self.craneMotor = MediumMotor(OUTPUT_B)
self.baseSpeed = baseSpeed
self.dt = dt
def MoveBackward(self, steering=0, speed=40):
steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B, motor_class=LargeMotor)
steer_pair.on_for_seconds(steering=0,
speed=20,
seconds=2,
brake=True,
block=True)
def __init__(self):
self.server_address = (str(sys.argv[1]), 5000)
self.sensor = InfraredSensor()
self.motor = MediumMotor(OUTPUT_D)
self.drive_motor = MoveTank(OUTPUT_B, OUTPUT_C)
self.turn_motor = MoveSteering(OUTPUT_B, OUTPUT_C)
self.moved = 0
self.turned = 0
def __init__(self):
"""Constructor for the internal state of the robot, e.g. in which direction he is
currently looking"""
self.direction = Directions.up
self.gy = GyroSensor()
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_D, motor_class= LargeMotor)
self.zero_point = ColorSensor().reflected_light_intensity
self.s = Sensors()
def __init__(self, left_motor_port, right_motor_port, infra_mode,
ultra_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediumMotor = MediumMotor()
self.__ir = InfraredSensor()
self.__ir.mode = infra_mode
self.__us = UltrasonicSensor()
self.__us.mode = ultra_mode
def __init__(self, left_motor_port, right_motor_port, infra_sensor_mode,
color_sensor_mode):
self.__movesteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediummotor = MediumMotor()
self.__cs = ColorSensor()
self.__cs.mode = color_sensor_mode
self.__ir = InfraredSensor()
self.__ir.mode = infra_sensor_mode
def __init__(self,
left_address=OUTPUT_A,
right_address=OUTPUT_B,
touch_address=INPUT_3):
self._wheels = MoveSteering(left_address, right_address)
self._left = self._wheels.left_motor
self._right = self._wheels.right_motor
self._touch = TouchSensor(touch_address)
def __init__(self):
self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self.line_sensor = ColorSensor(INPUT_2)
self.line_counter = ColorSensor(INPUT_3)
self.sound = Sound()
self.gyro = GyroSensor(INPUT_1)
self.gyro.reset()
self.dir_state = "S"
def __init__(self):
self.pid = PIDController(kP=1.0, kI=0.0, kD=0.1)
# motors
try:
self.steerPair = MoveSteering(OUTPUT_B, OUTPUT_C)
except ev3dev2.DeviceNotFound as e:
Debug.print("Error:", e)
self.speed = Config.data['pid']['fast']['speed_max']
def __init__(self):
self.button = Button()
self.tank = MoveSteering(OUTPUT_C, OUTPUT_B)
self.cs = ColorSensor()
self.cs.mode = ColorSensor.MODE_COL_REFLECT
self.gs = GyroSensor()
self.gs.reset()
self.before_direction = self.gyro()
def __init__(self, wheel1 = OUTPUT_B, wheel2 = OUTPUT_C):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
def __init__(self, wheel_radius, wheel_spacing):
self._container_motor = MediumMotor(OUTPUT_C)
self._steering_drive = MoveSteering(OUTPUT_D, OUTPUT_A)
self._touch_sensor_front = TouchSensor(INPUT_1)
self._touch_sensor_top = TouchSensor(INPUT_2)
#self._ultrasonic_sensor = UltrasonicSensor(INPUT_2)
self._color_sensor = ColorSensor(INPUT_3)
#self._color_sensor.calibrate_white()
#self._color_sensor.mode = "RGB-RAW"
self._color_sensor.mode = "COL-REFLECT"
self._leds = Leds()
self._sound = Sound()
self.WHEEL_RADIUS = wheel_radius
self.WHEEL_SPACING = wheel_spacing
class FoobotDrive:
def __init__(self,
wheel_diameter,
wheel_dist,
gear_ratio=1,
default_speed=50,
default_turn_speed=20):
self.wheel_diameter = wheel_diameter
self.wheel_circ = wheel_diameter * pi
self.wheel_dist = wheel_dist
self.default_speed = default_speed
self.default_turn_speed = default_turn_speed
self.gear_ratio = gear_ratio
self.move_steering = MoveSteering(OUTPUT_A, OUTPUT_B)
self.move_tank = MoveTank(OUTPUT_A, OUTPUT_B)
def move_straigth(self, distance, speed=0):
if speed == 0:
speed = self.default_speed
rotations = distance / self.wheel_dist / self.gear_ratio
self.move_tank.stop()
self.move_tank.on_for_rotations(self.default_speed, self.default_speed,
rotations)
def turn(self, deg, speed=0):
steering = -100
if speed == 0:
speed = self.default_turn_speed
if deg < 0:
deg = 0 - deg
steering = 0 - steering
rotation_arc = (pi * self.wheel_dist) * (deg / 360)
rotations = rotation_arc / 8 / self.gear_ratio
self.move_tank.stop()
self.move_steering.on_for_rotations(steering, speed, rotations)
def turn_right(self, deg, speed=0):
self.turn(0 - deg, speed)
def turn_left(self, deg, speed=0):
self.turn(deg, speed)
def run(self, speed=0):
if speed == 0:
speed = self.default_speed
self.move_tank.on(speed)
def stop(self):
self.move_tank.stop()
def test_steering_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
drive = MoveSteering(OUTPUT_A, OUTPUT_B)
drive.on_for_rotations(25, SpeedDPS(400), 10)
self.assertEqual(drive.left_motor.position, 0)
self.assertEqual(drive.left_motor.position_sp, 10 * 360)
self.assertEqual(drive.left_motor.speed_sp, 400)
self.assertEqual(drive.right_motor.position, 0)
self.assertEqual(drive.right_motor.position_sp, 5 * 360)
self.assertEqual(drive.right_motor.speed_sp, 200)
class MovementController:
"""Class to move the robot"""
_ROTATION_TO_DEGREE = 1
_DISTANCE_TO_DEGREE = 1
def __init__(self):
self.moveSteering = MoveSteering(constants.LEFT_MOTOR_PORT,
constants.RIGHT_MOTOR_PORT)
def rotate(self, degrees, speed=100, block=True):
"""Rotate the robot a certain number of degrees. Positive is counter-clockwise"""
self.moveSteering.on_for_degrees(
100,
SpeedPercent(speed),
degrees * MovementController._ROTATION_TO_DEGREE,
block=block)
def travel(self, distance, speed=100, block=True):
"""Make the robot move forward or backward a certain number of cm"""
self.moveSteering.on_for_degrees(
0,
speed,
distance * MovementController._DISTANCE_TO_DEGREE,
block=block)
def steer(self, direction, speed=100):
"""Make the robot move in a direction"""
self.moveSteering.on(direction, speed)
def stop(self):
"""Make robot stop"""
self.moveSteering.off()
class AutoDrive:
def __init__(self, left_motor_port, right_motor_port, infra_mode,
ultra_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediumMotor = MediumMotor()
self.__ir = InfraredSensor()
self.__ir.mode = infra_mode
self.__us = UltrasonicSensor()
self.__us.mode = ultra_mode
def __run_forward(self):
self.__moveSteering.on(0, 30)
def __run_backward_rotations(self, rotations):
self.__moveSteering.on_for_rotations(0, 20, -rotations)
def __stop(self):
self.__moveSteering.off()
def __back_and_turn(self, steering):
self.__moveSteering.on_for_rotations(-steering, 20, -1)
def __scan_around_distance(self):
proximities = {}
distance = 0
index = 0
for deg in [-90, 30, 30, 30, 30, 30, 30]:
self.__mediumMotor.on_for_degrees(10, deg)
distance = self.__ir.proximity
proximities[-90 + index * 30] = distance
index += 1
time.sleep(1)
self.__mediumMotor.on_for_degrees(10, -90)
# print("%s" % proximities)
return proximities
def __find_clear_direction(self, proximitiesDic):
max_distance = max(list(proximitiesDic.values()))
direction = list(proximitiesDic.keys())[list(
proximitiesDic.values()).index(max_distance)]
#print("%d" % direction)
steering = self.__convert_direction_to_steering(direction)
return steering
def __convert_direction_to_steering(self, direction):
return 5 * direction / 9
def __ultrasonic_distance(self):
return self.__us.distance_centimeters
def run(self):
self.__run_forward()
block_distance = self.__ultrasonic_distance()
if (block_distance < 20):
self.__stop()
around_distance = self.__scan_around_distance()
steering = self.__find_clear_direction(around_distance)
self.__run_backward_rotations(0.5)
self.__back_and_turn(steering)
def __init__(self,
wheel_diameter,
wheel_dist,
gear_ratio=1,
default_speed=50,
default_turn_speed=20):
self.wheel_diameter = wheel_diameter
self.wheel_circ = wheel_diameter * pi
self.wheel_dist = wheel_dist
self.default_speed = default_speed
self.default_turn_speed = default_turn_speed
self.gear_ratio = gear_ratio
self.move_steering = MoveSteering(OUTPUT_A, OUTPUT_B)
self.move_tank = MoveTank(OUTPUT_A, OUTPUT_B)
def __init__(self, *args_module_name):
super().__init__(*args_module_name)
self.m_left = OUTPUT_B # Motor left
self.m_right = OUTPUT_A # Motor right
self.s_us = INPUT_1 # Sensor Ultrasonic
self.sl_left = INPUT_3 # Sensor Light left
self.sl_right = INPUT_4 # Sensor Right left
self.mMT = MoveTank(self.m_left, self.m_right) # move with 2 motors
self.mMS = MoveSteering(self.m_left, self.m_right) # move on position
self.sUS = UltrasonicSensor(self.s_us)
self.sLS_left = LightSensor(INPUT_3)
self.sLS_right = LightSensor(INPUT_4)
self.thread_detect_danger = ControlThread()
self.thread_detect_light_intesity = ControlThread()
self.stop_detect_light_intesity = False
class Ev3Robot:
def __init__(self, wheel1=OUTPUT_B, wheel2=OUTPUT_C):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motorB = LargeMotor(OUTPUT_B)
self.motorC = LargeMotor(OUTPUT_C)
def pivot_right(self, degrees, speed=20):
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def spin_right(self, degrees, speed=20):
self.gyro.mode = 'GYRO-ANG'
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
self.tank_pair.off()
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-20, right_speed=20)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees)
def spin_left(self, degrees, speed=20):
self.gyro.mode = 'GYRO-ANG'
value1 = self.gyro.angles
self.gyro.reset()
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
self.tank_pair.off()
value2 = self.gyro.angle
self.tank_pair.on(left_speed=20, right_speed=-20)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees)
def go_straight_forward(self, cm, speed=30):
value1 = self.motorB.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while (self.motorB.position - value1) / 360 < rotations:
self.gyro.mode = 'GYRO-ANG'
angle = self.gyro.angle - angle0
print(angle, file=stderr)
self.steer_pair.on(steering=angle * -1, speed=speed)
def __init__(self, motorL=None, motorR=None, motorM=None, gyroPort=None, colorL=None, colorR=None, colorM=None):
if motorL: self.motorL = LargeMotor(motorL)
if motorR: self.motorR = LargeMotor(motorR)
if motorM: self.motorM = Motor(motorM)
if gyroPort:
self.gyro = GyroSensor(gyroPort)
self.gyro.mode = 'GYRO-CAL'
time.sleep(0.2)
self.gyro.mode = 'GYRO-ANG'
if colorL: self.colorL = ColorSensor(colorL)
if colorR: self.colorR = ColorSensor(colorR)
if colorM: self.colorM = ColorSensor(colorM)
if motorL and motorR:
self.drive = MoveSteering(motorL, motorR)
self.move = MoveTank(motorL, motorR)
print("Successful initialization!")
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 acceleration(
degrees,
finalSpeed,
steering=0,
robot=MoveSteering(OUTPUT_A, OUTPUT_B),
motorA=LargeMotor(OUTPUT_A)
): # Function to accelerate while moving so the robot can get traction before moving fast
"""Function to accelerate the robot and drive a specific distance"""
motorA.reset() #reseting how many degrees the robot has moved
motorA.position = 0 # setting how many degrees the robot has moved
accelerateDegree = degrees * 0.8
# declerationDegree = degrees * 0.2'
speed = 0 #starting speed
while motorA.position < degrees and False == Constants.STOP: #while the robot hasen't moved the target amount of degree's(distance)
if motorA.position < accelerateDegree and False == Constants.STOP:
if speed < finalSpeed: # while the robot hasen't accelerated to the target speed
speed += 5 #speed up
robot.on(steering=steering, speed=speed) # Start Moving
sleep(0.1) #wait so that it doesn't accelerate immidiatly
else:
robot.on(steering=steering,
speed=finalSpeed) # otherwise just keep moving
sleep(0.01)
elif False == Constants.STOP:
if speed > 10:
speed -= 5
robot.on(steering=steering, speed=speed)
sleep(0.05)
else:
robot.on(steering=steering, speed=speed)
sleep(0.01)
robot.off() # Stop Moving
def lineSquare(
leftMotor=LargeMotor(OUTPUT_A),
rightMotor=LargeMotor(OUTPUT_B),
robot=MoveSteering(OUTPUT_A, OUTPUT_B),
colorLeft=ColorSensor(INPUT_1),
colorRight=ColorSensor(INPUT_3)): # look below for what this is doing
'''Function to square the robot precisely on a black line'''
if True == Constants.STOP: return
colorLeft.mode = 'COL-REFLECT' #setting colorsenso mode
colorRight.mode = 'COL-REFLECT' #setting colorsensor mode
counter = 0 # setting that we've linesquared 0 times so far
while counter < 2 and False == Constants.STOP: # linesquare 2 times
left = Thread(target=MoveLeftMotor)
right = Thread(
target=MoveRightMotor
) # Linesquare left and right motor at the same time using thread
left.start() #starting the Thread
right.start() # starting the Thread
left.join() #join so we wait for this thread to finish
right.join() #join so we wait for this thread to finish
accelerationMoveBackward(
steering=0, finalSpeed=20, degrees=DistanceToDegree(
1)) # move backward so we can do it again for extra precision
counter += 1
def constructor():
sound = Sound()
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
touch_sensor_left = TouchSensor(INPUT_1)
touch_sensor_right = TouchSensor(INPUT_4)
return sound, motor_pair, touch_sensor_left, touch_sensor_right
