class EV3D4RemoteControlled(RemoteControlledTank): def __init__(self, medium_motor=OUTPUT_A, left_motor=OUTPUT_C, right_motor=OUTPUT_B): RemoteControlledTank.__init__(self, left_motor, right_motor) self.medium_motor = MediumMotor(medium_motor) self.medium_motor.reset()
def doLifter(): """Test code for operating lifter""" lifter = MediumMotor() lifter.reset() lifter.run_to_rel_pos(position_sp=60, speed_sp=100, stop_action='hold') lifter.wait_while('running') sleep(1) lifter.run_to_rel_pos(position_sp=-60, speed_sp=100, stop_action='hold') lifter.wait_while('running')
class El3ctricGuitar: NOTES = [1318, 1174, 987, 880, 783, 659, 587, 493, 440, 392, 329, 293] N_NOTES = len(NOTES) def __init__( self, lever_motor_port: str = OUTPUT_D, touch_sensor_port: str = INPUT_1, ir_sensor_port: str = INPUT_4): self.lever_motor = MediumMotor(address=lever_motor_port) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.leds = Leds() self.speaker = Sound() def start_up(self): self.leds.animate_flash( color='ORANGE', groups=('LEFT', 'RIGHT'), sleeptime=0.5, duration=3, block=True) self.lever_motor.on_for_seconds( speed=5, seconds=1, brake=False, block=True) self.lever_motor.on_for_degrees( speed=-5, degrees=30, brake=True, block=True) sleep(0.1) self.lever_motor.reset() def play_music(self): if self.touch_sensor.is_released: raw = sum(self.ir_sensor.proximity for _ in range(4)) / 4 self.speaker.tone( self.NOTES[min(round(raw / 5), self.N_NOTES - 1)] - 11 * self.lever_motor.position, 100, play_type=Sound.PLAY_WAIT_FOR_COMPLETE) def main(self): self.start_up() while True: self.play_music()
class TRACK3R(RemoteControlledTank): """ Base class for all TRACK3R variations. The only difference in the child classes are in how the medium motor is handled. To enable the medium motor toggle the beacon button on the EV3 remote. """ def __init__(self, medium_motor, left_motor, right_motor): RemoteControlledTank.__init__(self, left_motor, right_motor) self.medium_motor = MediumMotor(medium_motor) self.medium_motor.reset()
class EV3D4WebControlled(WebControlledTank): def __init__(self, medium_motor=OUTPUT_A, left_motor=OUTPUT_C, right_motor=OUTPUT_B): WebControlledTank.__init__(self, left_motor, right_motor) self.medium_motor = MediumMotor(medium_motor) if not self.medium_motor.connected: log.error("%s is not connected" % self.medium_motor) sys.exit(1) self.medium_motor.reset()
class EV3D4RemoteControlled(RemoteControlledTank): def __init__( self, left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C, medium_motor_port: str = OUTPUT_A, speed: float = 400, ir_beacon_channel: int = 1): super().__init__( left_motor=left_motor_port, right_motor=right_motor_port, polarity='inversed', speed=speed, channel=ir_beacon_channel) self.medium_motor = MediumMotor(address=medium_motor_port) self.medium_motor.reset()
m2.on(-15) while g.angle - g_off > degrees - 8: if command_handler(bob.commands): reset_motors() return f.write("offset: {} angle: {} delta: {}\n".format( g_off, g.angle, g.angle - g_off)) m1.on((10 - (5 * (g.angle - g_off) / degrees))) m2.on((-10 + (5 * (g.angle - g_off) / degrees))) m1.off() m2.off() g_off = 0 time.sleep(1) m4.reset() def dance(): global bob while True: m1.on(100) m2.on(-100) if command_handler(bob.commands): reset_motors() return time.sleep(0.5) m1.on(-100) m2.on(100) time.sleep(0.5)
LeftAction = MediumMotor(OUTPUT_A) RightAction = MediumMotor(OUTPUT_D) LeftWheel = LargeMotor(OUTPUT_B) RightWheel = LargeMotor(OUTPUT_C) btn = Button() contin = True print('*** ATHENA ROCKS! ***') print('1 - Left for Trip 1') print('2 - Up for Trip 2') print('3 - Right for Trip 3') print('4 - Down for Trip 4') print('5 - Center for Trip 5') sound.beep() while contin: RightAction.reset() LeftAction.reset() LeftWheel.reset() RightWheel.reset() if btn.left == True: runTrip1() sound.beep() elif btn.up == True: runTrip2() sound.beep() elif btn.right == True: runTrip3() sound.beep() elif btn.down == True: runTrip4() sound.beep()
class MindCuber(object): scan_order = [ 5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54, 51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43, 44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31 ] hold_cube_pos = 85 rotate_speed = 400 flip_speed = 450 flip_speed_push = 400 def __init__(self): self.shutdown = False self.flipper = LargeMotor(OUTPUT_A) self.turntable = LargeMotor(OUTPUT_B) self.colorarm = MediumMotor(OUTPUT_C) self.color_sensor = ColorSensor() self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW self.infrared_sensor = InfraredSensor() self.init_motors() self.state = ['U', 'D', 'F', 'L', 'B', 'R'] self.rgb_solver = None signal.signal(signal.SIGTERM, self.signal_term_handler) signal.signal(signal.SIGINT, self.signal_int_handler) filename_max_rgb = 'max_rgb.txt' if os.path.exists(filename_max_rgb): with open(filename_max_rgb, 'r') as fh: for line in fh: (color, value) = line.strip().split() if color == 'red': self.color_sensor.red_max = int(value) log.info("red max is %d" % self.color_sensor.red_max) elif color == 'green': self.color_sensor.green_max = int(value) log.info("green max is %d" % self.color_sensor.green_max) elif color == 'blue': self.color_sensor.blue_max = int(value) log.info("blue max is %d" % self.color_sensor.blue_max) def init_motors(self): for x in (self.flipper, self.turntable, self.colorarm): x.reset() log.info("Initialize flipper %s" % self.flipper) self.flipper.on(SpeedDPS(-50), block=True) self.flipper.off() self.flipper.reset() log.info("Initialize colorarm %s" % self.colorarm) self.colorarm.on(SpeedDPS(500), block=True) self.colorarm.off() self.colorarm.reset() log.info("Initialize turntable %s" % self.turntable) self.turntable.off() self.turntable.reset() def shutdown_robot(self): log.info('Shutting down') self.shutdown = True if self.rgb_solver: self.rgb_solver.shutdown = True for x in (self.flipper, self.turntable, self.colorarm): # We are shutting down so do not 'hold' the motors x.stop_action = 'brake' x.off(False) def signal_term_handler(self, signal, frame): log.error('Caught SIGTERM') self.shutdown_robot() def signal_int_handler(self, signal, frame): log.error('Caught SIGINT') self.shutdown_robot() def apply_transformation(self, transformation): self.state = [self.state[t] for t in transformation] def rotate_cube(self, direction, nb): current_pos = self.turntable.position final_pos = 135 * round( (self.turntable.position + (270 * direction * nb)) / 135.0) log.info( "rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" % (direction, nb, current_pos, final_pos)) if self.flipper.position > 35: self.flipper_away() self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), final_pos) if nb >= 1: for i in range(nb): if direction > 0: transformation = [0, 1, 5, 2, 3, 4] else: transformation = [0, 1, 3, 4, 5, 2] self.apply_transformation(transformation) def rotate_cube_1(self): self.rotate_cube(1, 1) def rotate_cube_2(self): self.rotate_cube(1, 2) def rotate_cube_3(self): self.rotate_cube(-1, 1) def rotate_cube_blocked(self, direction, nb): # Move the arm down to hold the cube in place self.flipper_hold_cube() # OVERROTATE depends on lot on MindCuber.rotate_speed current_pos = self.turntable.position OVERROTATE = 18 final_pos = int(135 * round( (current_pos + (270 * direction * nb)) / 135.0)) temp_pos = int(final_pos + (OVERROTATE * direction)) log.info( "rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s" % (direction, nb, current_pos, temp_pos, final_pos)) self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), temp_pos) self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4), final_pos) def rotate_cube_blocked_1(self): self.rotate_cube_blocked(1, 1) def rotate_cube_blocked_2(self): self.rotate_cube_blocked(1, 2) def rotate_cube_blocked_3(self): self.rotate_cube_blocked(-1, 1) def flipper_hold_cube(self, speed=300): current_position = self.flipper.position # Push it forward so the cube is always in the same position # when we start the flip if (current_position <= MindCuber.hold_cube_pos - 10 or current_position >= MindCuber.hold_cube_pos + 10): self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(speed), MindCuber.hold_cube_pos) sleep(0.05) def flipper_away(self, speed=300): """ Move the flipper arm out of the way """ log.info("flipper_away()") self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(speed), 0) def flip(self): """ Motors will sometimes stall if you call on_to_position() multiple times back to back on the same motor. To avoid this we call a 50ms sleep in flipper_hold_cube() and after each on_to_position() below. We have to sleep after the 2nd on_to_position() because sometimes flip() is called back to back. """ log.info("flip()") if self.shutdown: return # Move the arm down to hold the cube in place self.flipper_hold_cube() # Grab the cube and pull back self.flipper.ramp_up_sp = 200 self.flipper.ramp_down_sp = 0 self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190) sleep(0.05) # At this point the cube is at an angle, push it forward to # drop it back down in the turntable self.flipper.ramp_up_sp = 200 self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(self.flip_speed_push), MindCuber.hold_cube_pos) sleep(0.05) transformation = [2, 4, 1, 3, 0, 5] self.apply_transformation(transformation) def colorarm_middle(self): log.info("colorarm_middle()") self.colorarm.on_to_position(SpeedDPS(600), -750) def colorarm_corner(self, square_index): """ The lower the number the closer to the center """ log.info("colorarm_corner(%d)" % square_index) position_target = -580 if square_index == 1: position_target -= 10 elif square_index == 3: position_target -= 30 elif square_index == 5: position_target -= 20 elif square_index == 7: pass else: raise ScanError( "colorarm_corner was given unsupported square_index %d" % square_index) self.colorarm.on_to_position(SpeedDPS(600), position_target) def colorarm_edge(self, square_index): """ The lower the number the closer to the center """ log.info("colorarm_edge(%d)" % square_index) position_target = -640 if square_index == 2: position_target -= 20 elif square_index == 4: position_target -= 40 elif square_index == 6: position_target -= 20 elif square_index == 8: pass else: raise ScanError( "colorarm_edge was given unsupported square_index %d" % square_index) self.colorarm.on_to_position(SpeedDPS(600), position_target) def colorarm_remove(self): log.info("colorarm_remove()") self.colorarm.on_to_position(SpeedDPS(600), 0) def colorarm_remove_halfway(self): log.info("colorarm_remove_halfway()") self.colorarm.on_to_position(SpeedDPS(600), -400) def scan_face(self, face_number): log.info("scan_face() %d/6" % face_number) if self.shutdown: return if self.flipper.position > 35: self.flipper_away(100) self.colorarm_middle() self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb self.k += 1 i = 1 target_pos = 115 self.colorarm_corner(i) # The gear ratio is 3:1 so 1080 is one full rotation self.turntable.reset() self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), 1080, block=False) self.turntable.wait_until('running') while True: # 135 is 1/8 of full rotation if self.turntable.position >= target_pos: current_color = self.color_sensor.rgb self.colors[int(MindCuber.scan_order[self.k])] = current_color i += 1 self.k += 1 if i == 9: # Last face, move the color arm all the way out of the way if face_number == 6: self.colorarm_remove() # Move the color arm far enough away so that the flipper # arm doesn't hit it else: self.colorarm_remove_halfway() break elif i % 2: self.colorarm_corner(i) if i == 1: target_pos = 115 elif i == 3: target_pos = 380 else: target_pos = i * 135 else: self.colorarm_edge(i) if i == 2: target_pos = 220 elif i == 8: target_pos = 1060 else: target_pos = i * 135 if self.shutdown: return if i < 9: raise ScanError('i is %d..should be 9' % i) self.turntable.wait_until_not_moving() self.turntable.off() self.turntable.reset() log.info("\n") def scan(self): log.info("scan()") self.colors = {} self.k = 0 self.scan_face(1) self.flip() self.scan_face(2) self.flip() self.scan_face(3) self.rotate_cube(-1, 1) self.flip() self.scan_face(4) self.rotate_cube(1, 1) self.flip() self.scan_face(5) self.flip() self.scan_face(6) if self.shutdown: return log.info("RGB json:\n%s\n" % json.dumps(self.colors)) self.rgb_solver = RubiksColorSolverGeneric(3) self.rgb_solver.enter_scan_data(self.colors) self.rgb_solver.crunch_colors() self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict() log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba)) # This is only used if you want to rotate the cube so U is on top, F is # in the front, etc. You would do this if you were troubleshooting color # detection and you want to pause to compare the color pattern on the # cube vs. what we think the color pattern is. ''' log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier") self.rotate_cube(-1, 1) self.flip() self.flipper_away() self.rotate_cube(1, 1) input('Paused') ''' def move(self, face_down): log.info("move() face_down %s" % face_down) position = self.state.index(face_down) actions = { 0: ["flip", "flip"], 1: [], 2: ["rotate_cube_2", "flip"], 3: ["rotate_cube_1", "flip"], 4: ["flip"], 5: ["rotate_cube_3", "flip"] }.get(position, None) for a in actions: if self.shutdown: break getattr(self, a)() def run_kociemba_actions(self, actions): log.info('Action (kociemba): %s' % ' '.join(actions)) total_actions = len(actions) for (i, a) in enumerate(actions): if self.shutdown: break if a.endswith("'"): face_down = list(a)[0] rotation_dir = 1 elif a.endswith("2"): face_down = list(a)[0] rotation_dir = 2 else: face_down = a rotation_dir = 3 log.info("Move %d/%d: %s%s (a %s)" % (i, total_actions, face_down, rotation_dir, pformat(a))) self.move(face_down) if rotation_dir == 1: self.rotate_cube_blocked_1() elif rotation_dir == 2: self.rotate_cube_blocked_2() elif rotation_dir == 3: self.rotate_cube_blocked_3() log.info("\n") def resolve(self): if self.shutdown: return cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))] output = check_output(cmd).decode('ascii') if 'ERROR' in output: msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd), output) log.error(msg) print(msg) sys.exit(1) actions = output.strip().split() self.run_kociemba_actions(actions) self.cube_done() def cube_done(self): self.flipper_away() def wait_for_cube_insert(self): rubiks_present = 0 rubiks_present_target = 10 log.info('wait for cube...to be inserted') while True: if self.shutdown: break dist = self.infrared_sensor.proximity # It is odd but sometimes when the cube is inserted # the IR sensor returns a value of 100...most of the # time it is just a value less than 50 if dist < 50 or dist == 100: rubiks_present += 1 log.info("wait for cube...distance %d, present for %d/%d" % (dist, rubiks_present, rubiks_present_target)) else: if rubiks_present: log.info('wait for cube...cube removed (%d)' % dist) rubiks_present = 0 if rubiks_present >= rubiks_present_target: log.info('wait for cube...cube found and stable') break time.sleep(0.1)
class ALBERT(object): def __init__(self): # ev3dev initialization self.leds = Leds() self.sound = Sound() self.arm_base = LargeMotor(OUTPUT_D) self.arm_shoulder = LargeMotor(OUTPUT_C) self.arm_wrist = LargeMotor(OUTPUT_B) self.arm_gripper = MediumMotor(OUTPUT_A) self.station = Workstation() self.color_sensor = ColorSensor(INPUT_1) self.find_indicator() self.rotate_base(STATION_COLOR) self.reset_all_motors() def find_indicator(self): ''' Search for a valid color indicator. ''' if self.color_sensor.color in VALID_COLORS: return self.arm_base.on(10) while self.color_sensor.color not in VALID_COLORS: pass self.arm_base.stop() def reset_all_motors(self): ''' Reset all motor tach counts. ''' self.arm_base.reset() self.arm_shoulder.reset() self.arm_wrist.reset() self.arm_gripper.reset() def rotate_base(self, color): ''' Rotate from one color indicator to another. Color order is: YELLOW <--> BLUE <--> RED STORE <--> STATION <--> STERILE ''' current_color = self.color_sensor.color if current_color == color: return direction = 1 if (current_color == STATION_COLOR and color == STERILE_COLOR) or current_color == STORE_COLOR: direction = -1 self.arm_base.on(SPEEDS[0] * direction, block=False) while self.color_sensor.color != color: pass self.arm_base.stop() self.arm_base.on_for_rotations(SPEEDS[0], direction * STRIPE_BIAS) def make_plate(self): ''' Sequence to make a plate. ''' self.get_plate() self.lift_lid() self.swab_plate() self.lower_lid() self.store_plate() def check_plate(self): ''' Sequence to check plate. ''' self.get_plate(from_storage=True, upside_down=True) self.move_to_keypoint(KP_UP_HORZ) refl = self.station.check_status() self.move_to_keypoint(KP_DOWN_HORZ) self.store_plate(is_upside_down=True) return refl def get_plate(self, from_storage=False, upside_down=False): ''' Sequence to get a plate and place it in the workstation. Post-conditions Gripper: WIDE Arm: DOWN Base: STATION ''' src = STORE_COLOR if from_storage else STERILE_COLOR self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(src) self.set_gripper(GRIP_NARROW) self.move_to_keypoint(KP_DOWN_HORZ) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(STATION_COLOR) dest_up = KP_UP_VERT_INVERT if upside_down else KP_UP_VERT dest_down = KP_DOWN_VERT_INVERT if upside_down else KP_DOWN_VERT self.move_to_keypoint(dest_up) self.move_to_keypoint(dest_down) self.set_gripper(GRIP_WIDE) self.move_to_keypoint(KP_DOWN_HORZ) def lift_lid(self): ''' Lift the dish lid. Pre-condition Gripper: WIDE Arm: DOWN Base: STATION Post-condition Gripper: CLOSED Arm: UP ''' self.move_to_keypoint(KP_DOWN_HORZ_LID) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) def lower_lid(self): ''' Lower the dish lid. Pre-condition Gripper: CLOSED Arm: UP Base: STATION Post-condition Gripper: WIDE Arm: DOWN ''' self.move_to_keypoint(KP_DOWN_HORZ_LID) self.set_gripper(GRIP_WIDE) def swab_plate(self): ''' Call the Workstation swab routine. ''' self.station.swab() def store_plate(self, is_upside_down=False): ''' Sequence to store a plate. ''' src_down = KP_DOWN_VERT_INVERT if is_upside_down else KP_DOWN_VERT self.move_to_keypoint(src_down) self.set_gripper(GRIP_CLOSED) self.move_to_keypoint(KP_UP_HORZ) self.rotate_base(STORE_COLOR) self.move_to_keypoint(KP_DOWN_HORZ) self.set_gripper(GRIP_NARROW) self.move_to_keypoint(KP_UP_VERT) self.rotate_base(STATION_COLOR) self.set_gripper(GRIP_CLOSED) def move_to_keypoint(self, keypoint): ''' Move the should/wrist to a keypoint.''' # keypoint is [shoulder, wrist] with unit of rotations self.arm_shoulder.on_to_position( SPEEDS[1], keypoint[0] * self.arm_shoulder.count_per_rot) self.arm_wrist.on_to_position( SPEEDS[2], keypoint[1] * self.arm_wrist.count_per_rot) # pause to let things settle time.sleep(MOVE_PAUSE) def set_gripper(self, position): ''' Set the gripper position. ''' self.arm_gripper.on_to_position( 25, self.arm_gripper.count_per_rot * position) time.sleep(MOVE_PAUSE)
class AthenaRobot(object): # constructors for the robot with default parameters of wheel radius and ports def __init__(self, wheelRadiusCm=4, leftLargeMotorPort=OUTPUT_B, rightLargeMotorPort=OUTPUT_C, leftMediumMotorPort=OUTPUT_A, rightMediumMotorPort=OUTPUT_D, leftSensorPort=INPUT_1, rightSensorPort=INPUT_4, ultraSonicSensorPort=INPUT_2): #self is the current object, everything below for self are member variables self.wheelRadiusCm = wheelRadiusCm self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm self.leftLargeMotor = LargeMotor(leftLargeMotorPort) self.rightLargeMotor = LargeMotor(rightLargeMotorPort) self.leftMediumMotor = MediumMotor(leftMediumMotorPort) self.rightMediumMotor = MediumMotor(rightMediumMotorPort) self.leftSensor = ColorSensor(leftSensorPort) self.rightSensor = ColorSensor(rightSensorPort) # run a distance in centimeters at speed of centimeters per second def run(self, distanceCm, speedCmPerSecond, brake=True, block=True): if speedCmPerSecond < 0: raise Exception('speed cannot be negative') # Calculate degrees of distances and SpeedDegreePerSecond degreesToRun = distanceCm / self.wheelCircumferenceCm * 360 speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360 print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format( degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed), file=sys.stderr) # run motors based on the calculated results self.leftLargeMotor.on_for_degrees( SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False) self.rightLargeMotor.on_for_degrees( SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block) # turn a angle in degrees, positive means turn right and negative means turn left. def turn(self, degree, speed=10, brake=True, block=True): # 1.9 is a scale factor from experiments degreesToRun = degree * 1.275 # Turn at the speed self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, False) self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block) def turnRight(self, degree, speed=10, brake=True, block=True): self.turn(degree, speed, brake, block) def turnLeft(self, degree, speed=10, brake=True, block=True): self.turn(-degree, speed, brake, block) def turnOnRightWheel(self, degree, speed=10, brake=True, block=True): degreesToRun = degree * 2.7 self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block) def turnRightOnRightWheel(self, degree, speed=10, brake=True, block=True): self.turnOnRightWheel(degree, speed, brake, block) def turnLeftOnRightWheel(self, degree, speed=10, brake=True, block=True): self.turnOnRightWheel(-degree, speed, brake, block) def turnOnLeftWheel(self, degree, speed=10, brake=True, block=True): degreesToRun = degree * 2.7 self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, block) def turnRightOnLeftWheel(self, degree, speed=10, brake=True, block=True): self.turnOnLeftWheel(degree, speed, brake, block) def turnLeftOnLeftWheel(self, degree, speed=10, brake=True, block=True): self.turnOnLeftWheel(-degree, speed, brake, block) #Medium Motor Movement def moveMediumMotor(self, isLeft, speed, degrees, brake=True, block=True): #sees which motor is running if isLeft == False: self.rightMediumMotor.on_for_degrees(speed, degrees, brake, block) else: self.leftMediumMotor.on_for_degrees(speed, degrees, brake, block) # Following a line with one sensor def lineFollow(self, whiteThreshold=98, blackThreshold=15, scale=0.2, useLeftSensor=True, useLeftEdge=True, runDistanceCM=300): self.leftLargeMotor.reset() self.rightLargeMotor.reset() # Allow an attached backsensor. Ixf useBackSensor, defining back sensor and revert useLeftEdge since motor is actually going backward initialPos = self.leftLargeMotor.position # remember initial position loop = True while loop: # use left or right sensor based on passed in useLeftSensor reflect = self.leftSensor.reflected_light_intensity if useLeftSensor == True else self.rightSensor.reflected_light_intensity # Allow an attached backsensor. If useBackSensor, use reflected_light_intensity of that sensor leftPower = abs(reflect - blackThreshold) * scale rightPower = abs(whiteThreshold - reflect) * scale # if we follow the right edge, need to swap left and right if useLeftEdge == False: oldLeft = leftPower leftPower = rightPower rightPower = oldLeft self.leftLargeMotor.on(-leftPower) self.rightLargeMotor.on(-rightPower) # Calculate the distance run in CM distanceRanInCM = abs((self.leftLargeMotor.position - initialPos) * (self.wheelCircumferenceCm / self.leftLargeMotor.count_per_rot)) # Printing the reflected light intensity with the powers of the two motors print( "LineFollow - reflect: {0:3d} leftPower: {1:3f} rightPower: {2:3f} lMotorPos: {3:3d} distanceRanInCM {4:3f}" .format(reflect, leftPower, rightPower, self.leftLargeMotor.position, distanceRanInCM), file=sys.stderr) if distanceRanInCM >= runDistanceCM: loop = False # Stopping the motor once the loop is over self.leftLargeMotor.off() self.rightLargeMotor.off() # run until both conditions are met def onUntilTwoConditions(self, leftCondition, rightCondition, caller, speed=5, consecutiveHit=5, sleepTime=0.01): # Start motor at passed speonUntilTwoConditionsed. self.leftLargeMotor.on(-speed) self.rightLargeMotor.on(-speed) condLeftCounter = 0 condRightCounter = 0 condLeftMet = False condRightMet = False while (not condLeftMet or not condRightMet): # check left condition if (leftCondition()): condLeftCounter += 1 else: condLeftCounter = 0 # reset to zero if (condLeftCounter >= consecutiveHit): if (condRightMet): sleep(.1) self.leftLargeMotor.off() condLeftMet = True # check right condition if (rightCondition()): condRightCounter += 1 else: condRightCounter = 0 # reset to zero if (condRightCounter >= consecutiveHit): if (condLeftMet): sleep(.1) self.rightLargeMotor.off() condRightMet = True print( "{4}-onUntilTwoConditions: left_reflected: {0:3d}, right_reflected: {1:3d}, leftHit: {2:3d}, rightHit: {3:3d}" .format(self.leftSensor.reflected_light_intensity, self.rightSensor.reflected_light_intensity, condLeftCounter, condRightCounter, caller), file=sys.stderr) sleep(sleepTime) self.leftLargeMotor.off() self.rightLargeMotor.off() def onUntilWhiteLine(self, consecutiveHit=5, speed=5, sleepTime=0.01, white_threshold=85): self.onUntilTwoConditions( lambda: self.leftSensor.reflected_light_intensity > white_threshold, lambda: self.rightSensor.reflected_light_intensity > white_threshold, "onUntilWhiteLine", speed, consecutiveHit, sleepTime) def onUntilBlackLine(self, consecutiveHit=5, speed=5, sleepTime=0.01, black_threshold=30): self.onUntilTwoConditions( lambda: self.leftSensor.reflected_light_intensity < black_threshold, lambda: self.rightSensor.reflected_light_intensity < black_threshold, "onUntilBlackLine", speed, consecutiveHit, sleepTime) # run until find a game line def onUntilGameLine(self, consecutiveHit=5, speed=5, sleepTime=0.01, white_threshold=85, black_threshold=30): self.onUntilWhiteLine(consecutiveHit, speed, sleepTime, white_threshold) self.onUntilBlackLine(consecutiveHit, speed, sleepTime, black_threshold) # run until condition is met def onUntilCondition(self, condition, caller, leftSpeed=5, rightSpeed=5, consecutiveHit=5, sleepTime=0.01, revert=False): # Start motor at passed speonUntilTwoConditionsed. self.leftLargeMotor.on(-leftSpeed if revert == False else leftSpeed) self.rightLargeMotor.on(-rightSpeed if revert == False else rightSpeed) counter = 0 condMet = False while (not condMet): # check condition if (condition()): counter += 1 else: counter = 0 # reset to zero if (counter >= consecutiveHit): self.leftLargeMotor.off() self.rightLargeMotor.off() condMet = True print( "{4}-onUntilCondition: ColorSensor(left_reflected: {0:3d}, right_reflected: {1:3d}, hit: {2:3d}), UltraSonicSensor(distance_centimeters: {3:3f})" .format(self.leftSensor.reflected_light_intensity, self.rightSensor.reflected_light_intensity, counter, self.ultraSonicSensor.distance_centimeters, caller), file=sys.stderr) sleep(sleepTime) self.leftLargeMotor.off() self.rightLargeMotor.off() def onUntilLeftBlack(self, speed=5, consecutiveHit=5, sleepTime=0.01, black_threshold=30): self.onUntilCondition( lambda: self.leftSensor.reflected_light_intensity < black_threshold, "onUntilLeftBlack", speed, speed, consecutiveHit, sleepTime) def onUntilLeftWhite(self, speed=5, consecutiveHit=5, sleepTime=0.01, white_threshold=85): self.onUntilCondition( lambda: self.leftSensor.reflected_light_intensity > white_threshold, "onUntilLeftWhite", speed, speed, consecutiveHit, sleepTime) def onUntilRightBlack(self, speed=5, consecutiveHit=5, sleepTime=0.01, black_threshold=30): self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity < black_threshold, "onUntilRightBlack", speed, speed, consecutiveHit, sleepTime) def onUntilRightWhite(self, speed=5, consecutiveHit=5, sleepTime=0.01, white_threshold=85): self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity > white_threshold, "onUntilRightWhite", speed, speed, consecutiveHit, sleepTime) def turnUntilLeftBlack(self, turnLeft, speed, consecutiveHit=2, black_threshold=30): self.onUntilCondition( lambda: self.leftSensor.reflected_light_intensity < black_threshold, "turnUntilLeftBlack", 0 if turnLeft == True else speed, speed if turnLeft == True else 0, consecutiveHit) def turnUntilLeftWhite(self, turnLeft, speed, consecutiveHit=2, white_threshold=85): self.onUntilCondition( lambda: self.leftSensor.reflected_light_intensity > white_threshold, "turnUntilLeftWhite", 0 if turnLeft == True else speed, speed if turnLeft == True else 0, consecutiveHit) def turnUntilRightBlack(self, turnLeft, speed, consecutiveHit=2, black_threshold=30): self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity < black_threshold, "turnUntilRightBlack", 0 if turnLeft == True else speed, speed if turnLeft == True else 0, consecutiveHit) def turnUntilRightWhite(self, turnLeft, speed, consecutiveHit=2, white_threshold=85): self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity > white_threshold, "turnUntilRightWhite", 0 if turnLeft == True else speed, speed if turnLeft == True else 0, consecutiveHit) # Go until sensor reading has a specified offset or reach to the threshhold def onUntilRightDarkerBy(self, difference, black_threshold=20, speed=10, consecutiveHit=2): originalValue = self.rightSensor.reflected_light_intensity print("onUntilRightDarkerBy - originalValue: {0:3d}, diff: {1:3d}". format(originalValue, difference), file=sys.stderr) self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity - originalValue < -difference or self.rightSensor.reflected_light_intensity < black_threshold, "onUntilRightSensorDiff", consecutiveHit=consecutiveHit) def onUntilRightLighterBy(self, difference, white_threshold=80, speed=10, consecutiveHit=2): originalValue = self.rightSensor.reflected_light_intensity print("onUntilRightLighterBy - originalValue: {0:3d}, diff: {1:3d}". format(originalValue, difference), file=sys.stderr) self.onUntilCondition( lambda: self.rightSensor.reflected_light_intensity - originalValue > difference or self.rightSensor.reflected_light_intensity > white_threshold, "onUntilRightSensorDiff", consecutiveHit=consecutiveHit) def onUntilLeftDarkerBy(self, difference, black_threshold=20, speed=10, consecutiveHit=2): originalValue = self.leftSensor.reflected_light_intensity print( "onUntilLeftDarkerBy - originalValue: {0:3d}, diff: {1:3d}".format( originalValue, difference), file=sys.stderr) self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity - originalValue < -difference or self.leftSensor. reflected_light_intensity < black_threshold, "onUntilLeftSensorDiff", consecutiveHit=consecutiveHit) def onUntilLeftLighterBy(self, difference, white_threshold=80, speed=10, consecutiveHit=2): originalValue = self.leftSensor.reflected_light_intensity print("onUntilLeftLighterBy - originalValue: {0:3d}, diff: {1:3d}". format(originalValue, difference), file=sys.stderr) self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity - originalValue > difference or self.leftSensor. reflected_light_intensity > white_threshold, "onUntilLeftSensorDiff", consecutiveHit=consecutiveHit) #uses Ultrasonic sensor to see wall as going back def revertSafely(self, speed=100, distanceToStop=10, consecutiveHit=1, sleepTime=0.01): self.onUntilCondition( lambda: self.ultraSonicSensor.distance_centimeters < distanceToStop, "revertSafely", speed, speed, consecutiveHit, sleepTime, True) # Calibrating White for Sensor def calibrateColorSensor(self, sensorInput): sensor = ColorSensor(sensorInput) # Calibration sensor.calibrate_white() # Done Signal sound.beep() # Calibrating Color for Sensor def testColorSensor(self, sensorInput, sensorPort, repeatNumber=10, pauseNumber=0.2, speed=0): sensor = ColorSensor(sensorInput) if (speed > 0): self.leftLargeMotor.on(speed) self.rightLargeMotor.on(speed) times = 0 # For loop while times != repeatNumber: # Print print("testColorSensor- Port: {0:3d}: {1:3d}".format( sensorPort, sensor.reflected_light_intensity), file=sys.stderr) time.sleep(pauseNumber) times = times + 1 self.leftLargeMotor.off() self.rightLargeMotor.off() def resetMediumMotor(): self.moveMediumMotor(isLeft=False, speed=50, degrees=1000) self.rightMediumMotor.reset() def testRobot(self): self.leftLargeMotor.on_for_degrees(20, 180) sleep(.1) self.rightLargeMotor.on_for_degrees(20, 180) sleep(.1) self.moveMediumMotor(True, 10, 180) sleep(.1) self.moveMediumMotor(False, 10, 180) sleep(.1) self.run(20, 10) sleep(.1) self.run(-20, 10) sleep(.1) self.turn(90) sleep(.1) self.turn(-90) sleep(.1) self.turnOnLeftWheel(90) sleep(.1) self.turnOnLeftWheel(-90) sleep(.1) self.turnOnRightWheel(90) sleep(.1) self.turnOnRightWheel(-90) sleep(.1) self.calibrateColorSensor(INPUT_1) self.calibrateColorSensor(INPUT_4) self.testColorSensor(INPUT_1, 1) self.testColorSensor(INPUT_4, 4)
class Robot: def __init__(self): self.sound = Sound() self.direction_motor = MediumMotor(OUTPUT_D) self.swing_motorL = LargeMotor(OUTPUT_A) self.swing_motorC = LargeMotor(OUTPUT_B) self.swing_motorR = LargeMotor(OUTPUT_C) self.swing_motors = [ self.swing_motorL, self.swing_motorC, self.swing_motorR ] self.touch_sensor = TouchSensor(INPUT_1) self.console = Console(DEFAULT_FONT) self.buttons = Button() self.beeps_enabled = True def beep(self, frequency=700, wait_for_comeplete=False): if not self.beeps_enabled: return play_type = Sound.PLAY_WAIT_FOR_COMPLETE if wait_for_comeplete else Sound.PLAY_NO_WAIT_FOR_COMPLETE self.sound.beep("-f %i" % frequency, play_type=play_type) def calibrate_dir(self): ''' Calibrate direction motor ''' # Run motor with 25% power, and wait until it stops running self.direction_motor.on(SpeedPercent(-10), block=False) # while (not self.direction_motor.STATE_OVERLOADED): # print(self.direction_motor.duty_cycle) self.direction_motor.wait_until(self.direction_motor.STATE_OVERLOADED) self.direction_motor.stop_action = Motor.STOP_ACTION_COAST self.direction_motor.stop() time.sleep(.5) # Reset to straight # self.direction_motor.on_for_seconds(SpeedPercent(10), .835, brake=True, block=True) self.direction_motor.on_for_degrees(SpeedPercent(10), 127, brake=True, block=True) self.direction_motor.reset() print("Motor reset, position: " + str(self.direction_motor.position)) time.sleep(.5) def calibrate_swing(self): for m in self.swing_motors: m.stop_action = m.STOP_ACTION_HOLD m.on(SpeedPercent(6)) self.swing_motorC.wait_until(self.swing_motorC.STATE_OVERLOADED, 2000) for m in self.swing_motors: m.stop_action = m.STOP_ACTION_HOLD m.on_for_degrees(SpeedPercent(5), -15, brake=True, block=False) self.swing_motorC.wait_while('running') for m in self.swing_motors: m.reset() m.stop_action = m.STOP_ACTION_HOLD m.stop() print("Ready Angle: %i" % self.swing_motorC.position) def ready_swing(self, angle: int): right_angle = -(angle / 3) # adjust motors to target angle for m in self.swing_motors: m.stop_action = Motor.STOP_ACTION_HOLD m.on_for_degrees(SpeedPercent(2), right_angle, brake=True, block=False) self.swing_motorC.wait_while('running') for m in self.swing_motors: m.stop_action = Motor.STOP_ACTION_HOLD m.stop() print("Swing Angle: %i" % self.swing_motorC.position) def set_direction(self, direction): print("Setting direction to: " + str(direction)) #direction = self.__aim_correction(direction) self.direction_motor.on_for_degrees(SpeedPercent(10), round(direction * 3)) print("Direction set to: " + str(self.direction_motor.position)) # # def __aim_correction(self, direction): # x = direction # y = -0.00000000429085685725*x**6 + 0.00000004144345630728*x**5 + 0.00001219331494759860*x**4 + 0.00020702946527870400*x**3 + 0.00716486965517779000*x**2 + 1.29675836037884000000*x + 0.27064829453014400000 # # return y def shoot(self, power): print("SHOOT, power: %i" % power) for m in self.swing_motors: m.duty_cycle_sp = -power for m in self.swing_motors: m.run_direct() time.sleep(.5) self.swing_motorC.wait_until_not_moving() for m in self.swing_motors: m.reset() def wait_for_button(self): self.touch_sensor.wait_for_bump() def __set_display(self, str): self.console.set_font(font=LARGER_FONT, reset_console=True) self.console.text_at(str, column=1, row=1, reset_console=True) def wait_for_power_select(self, power=0, angle=0, steps=1): self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) def left(): power -= steps if power < 0: power = 0 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['left'], timeout_ms=150) def right(): power += steps if power > 100: power = 100 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['right'], timeout_ms=150) def up(): angle += steps if angle > 110: angle = 110 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['up'], timeout_ms=150) def down(): angle -= steps if angle < 0: angle = 0 self.__set_display("Pow: %i\nAngle: %i" % (power, angle)) self.buttons.wait_for_released(buttons=['down'], timeout_ms=150) while not self.touch_sensor.is_pressed: if self.buttons.left: left() elif self.buttons.right: right() elif self.buttons.up: up() elif self.buttons.down: down() self.console.set_font(font=DEFAULT_FONT, reset_console=True) return (power, angle) def select_connection_mode(self): self.__set_display("Enable Connection\nLeft: True - Right: False") enabled = True while not self.touch_sensor.is_pressed: if self.buttons.left: enabled = True self.buttons.wait_for_released(buttons=['left']) break elif self.buttons.right: enabled = False self.buttons.wait_for_released(buttons=['right']) break self.console.set_font(font=DEFAULT_FONT, reset_console=True) return enabled def print(self, string): print(string)
class Gripp3r(RemoteControlledTank): """ To enable the medium motor toggle the beacon button on the EV3 remote. """ CLAW_DEGREES_OPEN = 225 CLAW_DEGREES_CLOSE = 920 CLAW_SPEED_PCT = 50 def __init__(self, left_motor_port=OUTPUT_B, right_motor_port=OUTPUT_C, medium_motor_port=OUTPUT_A): RemoteControlledTank.__init__(self, left_motor_port, right_motor_port) self.set_polarity(MediumMotor.POLARITY_NORMAL) self.medium_motor = MediumMotor(medium_motor_port) self.ts = TouchSensor() self.mts = MonitorTouchSensor(self) self.mrc = MonitorRemoteControl(self) self.shutdown_event = Event() # Register our signal_term_handler() to be called if the user sends # a 'kill' to this process or does a Ctrl-C from the command line signal.signal(signal.SIGTERM, self.signal_term_handler) signal.signal(signal.SIGINT, self.signal_int_handler) self.claw_open(True) self.remote.on_channel4_top_left = self.claw_close self.remote.on_channel4_bottom_left = self.claw_open def shutdown_robot(self): if self.shutdown_event.is_set(): return self.shutdown_event.set() log.info('shutting down') self.mts.shutdown_event.set() self.mrc.shutdown_event.set() self.remote.on_channel4_top_left = None self.remote.on_channel4_bottom_left = None self.left_motor.off(brake=False) self.right_motor.off(brake=False) self.medium_motor.off(brake=False) self.mts.join() self.mrc.join() def signal_term_handler(self, signal, frame): log.info('Caught SIGTERM') self.shutdown_robot() def signal_int_handler(self, signal, frame): log.info('Caught SIGINT') self.shutdown_robot() def claw_open(self, state): if state: # Clear monitor_ts while we are opening the claw. We do this because # the act of opening the claw presses the TouchSensor so we must # tell mts to ignore that press. self.mts.monitor_ts.clear() self.medium_motor.on(speed=self.CLAW_SPEED_PCT * -1, block=True) self.medium_motor.off() self.medium_motor.reset() self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT, position=self.CLAW_DEGREES_OPEN, brake=False, block=True) self.mts.monitor_ts.set() def claw_close(self, state): if state: self.medium_motor.on_to_position(speed=self.CLAW_SPEED_PCT, position=self.CLAW_DEGREES_CLOSE) def main(self): self.mts.start() self.mrc.start() self.shutdown_event.wait()
screenw = display.xres screenh = display.yres # hostMACAddress = '00:17:E9:B2:8A:AF' # The MAC address of a Bluetooth adapter on the server. The server might have multiple Bluetooth adapters. # Fetch BT MAC address automatically cmd = "hciconfig" device_id = "hci0" sp_result = subprocess.run(cmd, stdout=subprocess.PIPE, universal_newlines=True) hostMACAddress = sp_result.stdout.split("{}:".format(device_id))[1].split("BD Address: ")[1].split(" ")[0].strip() debug_print (hostMACAddress) print (hostMACAddress) # reset the kick motor to a known good position kickMotor.on_for_seconds(speed=-10, seconds=0.5) kickMotor.on_for_seconds(speed=10, seconds=2, brake=False) kickMotor.reset() kicking = False kick_power = 0 max_kick = 1000 port = 3 # port number is arbitrary, but must match between server and client backlog = 1 size = 1024 s = bluetooth.BluetoothSocket(bluetooth.RFCOMM) s.bind((hostMACAddress, port)) s.listen(backlog) while True: try: reset_console() print (hostMACAddress) leds.set_color('LEFT', 'AMBER')
from ev3dev2.motor import MediumMotor from time import sleep mA = MediumMotor('outA') mA.reset() def lift(direction): if(direction == "up"): mA.run_to_rel_pos(position_sp=130, speed_sp=300, stop_action="coast") else: mA.run_to_rel_pos(position_sp=-130, speed_sp=300) mA.wait_while('running')