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