Exemple #1
0
 def __init__(self, port='A', speed=100, **kwargs):
   self.port = port
   self.speed = speed
   self.test_direction = 1
   self.motor = MediumMotor(port=self.port)
   self.motor.reset()
   self.task_state = None
 def test_medium_motor(self):
     get_input('Attach a MediumMotor then continue')
     d = MediumMotor()
     print(d.driver_name)
     d.run_forever(100, speed_regulation=False)
     print(d.state)
     time.sleep(5)
     d.stop()
 def test_medium_motor(self):
     get_input('Attach a MediumMotor then continue')
     d = MediumMotor()
     print(d.type)
     d.run_forever(100, regulation_mode=False)
     print(d.run)
     time.sleep(5)
     d.stop()
Exemple #4
0
 def __init__(self):
     self.ir = InfraredSensor()
     self.color = ColorSensor()
     self.left_track = LargeMotor(Motor.PORT.D)
     self.right_track = LargeMotor(Motor.PORT.A)
     self.mouth = MediumMotor(Motor.PORT.B)
     self.key = Key()
     self.led = LED()
     self.reset_leds()
    def test_medium_motor(self):
        get_input('Attach a MediumMotor then continue')
        d = MediumMotor()
        print(d.type)
        #d.run_forever(25, regulation_mode=False)
	d.run_time_limited(time_sp=5000, speed_sp=25, regulation_mode=False,
                           stop_mode=Motor.STOP_MODE.COAST, ramp_up_sp=100, ramp_down_sp=100)
        print(d.run)
        time.sleep(5)
        d.stop()
Exemple #6
0
 def __init__(self):
     self.ir = InfraredSensor()
     self.color = ColorSensor()
     self.left_track = LargeMotor(Motor.PORT.D)
     self.right_track = LargeMotor(Motor.PORT.A)
     self.mouth = MediumMotor(Motor.PORT.B)
     self.key = Key()
     self.led = LED()
     self.reset_leds()
Exemple #7
0
 def test_medium_motor(self):
     get_input('Attach a MediumMotor then continue')
     d = MediumMotor()
     print(d.type)
     d.run_forever(100, regulation_mode=False)
     print(d.run)
     time.sleep(5)
     d.stop()
 def test_medium_motor(self):
     get_input('Attach a MediumMotor then continue')
     d = MediumMotor()
     print(d.driver_name)
     d.run_forever(100, speed_regulation=False)
     print(d.state)
     time.sleep(5)
     d.stop()
Exemple #9
0
class Arm():
  """ Class Arm represents build from LEGO Mindstorms arm for catching and moving cans of beer """

  def __init__(self, port='A', speed=100, **kwargs):
    self.port = port
    self.speed = speed
    self.test_direction = 1
    self.motor = MediumMotor(port=self.port)
    self.motor.reset()
    self.task_state = None

  @revert_task_state_to_none_after_call
  def get(self):
    self.task_state = GETTING
    self.speed = 100
    self.motor.run_forever(self.speed, regulation_mode=False)

  @revert_task_state_to_none_after_call
  def put(self):
    self.task_state = PUTTING
    self.speed = -50
    self.motor.run_forever(self.speed, regulation_mode=False)

  @revert_task_state_to_none_after_call
  def stop(self):
    self.task_state = STOPPING
    self.motor.stop()

  def test(self):
    while True:
      if self.test_direction == -1:
        self.put()
      else:
        self.get()
      self.test_direction *= -1
      raw_input("Press enter")
    self.motor.stop()
Exemple #10
0
class Robot:
    def __init__(self):
        self.ir = InfraredSensor()
        self.color = ColorSensor()
        self.left_track = LargeMotor(Motor.PORT.D)
        self.right_track = LargeMotor(Motor.PORT.A)
        self.mouth = MediumMotor(Motor.PORT.B)
        self.key = Key()
        self.led = LED()
        self.reset_leds()

    def reset_leds(self):
        self.led.left.on()
        self.led.right.on()
        self.led.left.color = LED.COLOR.GREEN
        self.led.right.color = LED.COLOR.GREEN


    def keys(self):
        keys = ('up', 'down', 'left', 'right', 'backspace', 'enter')
        dump = [x + ":" + str(getattr(self.key, x)) for x in keys]
        print " ".join(dump)
        return self.key

    def music(self, tune):
        subprocess.call(["aplay", "%s.wav" % tune])

    def distance_front(self):
        return self.ir.prox

    def ball_captured(self):
        while True:
            try:
                return self.color.reflect > 0
            except IOError:
                pass # this sometimes happens, try again

    def drive(self, distance_mm, block_until_done=True, power=600):
        self.led.left.color = LED.COLOR.AMBER
        print('drive '+str(distance_mm) + 'mm')
        distance_rotations_ratio = 360/109.0
        #distance_rotations_ratio = 360/113.0
        rel_position = distance_mm * distance_rotations_ratio
        ramp = 1000
        self._rotate_track_rel(self.left_track, rel_position, power, ramp)
        self._rotate_track_rel(self.right_track, rel_position, power, ramp)
        if (block_until_done):
            return self.move_until_finished(distance_mm < 0)
        else:
            self.reset_leds()
            return True

    def drive_until_distance(self, ir_distance):
        self.led.right.color = LED.COLOR.AMBER
        print('driving until ir shows ' + str(ir_distance))
        self.drive(20000, False)
        while (self.distance_front() > ir_distance):
            time.sleep(0.1)
        self.stop()
        self.reset_leds()

    def turn(self, degrees, block_until_done=True):
        print('turn '+str(degrees))
        #ratio = 560 / 90.0
        ratio = 530 / 90.0
        if degrees > 0:
            led = self.led.right
        else:
            led = self.led.left
        led.blink(color=LED.COLOR.AMBER, delay_on=100, delay_off=200)
        rel_position = ratio * degrees
        power = 400
        ramp = 50
        self._rotate_track_rel(self.left_track, -rel_position, power, ramp)
        self._rotate_track_rel(self.right_track, rel_position, power, ramp)
        if (block_until_done):
            return self.move_until_finished()
        else:
            return True

    def motors_running(self):
        return self.left_track.state != 'idle' or self.right_track.state != 'idle'

    def stop(self):
        self.left_track.stop()
        self.right_track.stop()
        self.left_track.reset()
        self.right_track.reset()

    def move_until_finished(self, reverse=False):
        print('move until finished')
        while (self.motors_running()):
            if not reverse and self.distance_front() < 10:
                self.stop()
                print('action interrupted')
                return False  # Was interrupted
            else:
                time.sleep(0.1)
        self.reset_leds()
        return True  # Finished as planned

    def avoid_wall(self, degrees_each, threshold=50, backoff_mm=100, backoff_angle=30):
        if self.distance_front() < 30:
            self.drive(-backoff_mm)
        self.turn(-degrees_each, True)
        self.turn(degrees_each, False)
        found_neg, max_neg = self.move_until_moving_away(threshold)
        self.turn(degrees_each, True)
        self.turn(-degrees_each, False)
        found_pos, max_pos = self.move_until_moving_away(threshold)

        #max_pos = max(80, max_pos)
        #max_neg = max(80, max_pos)

        correction = 0
        scale = 1
        print("positive degrees: " + str(found_pos) + ", " + str(max_pos))
        print("negative degrees: " + str(found_neg) + ", " + str(max_neg))
        if found_pos and not found_neg:
            correction = -1
        elif found_neg and not found_pos:
            correction = 1
        elif found_pos and found_neg:
            scale = 2
            if max_pos > max_neg:
                correction = 1
            elif max_neg > max_pos:
                correction = -1

        print("correction: " + str(correction) + ", scale: " + str(scale))
        if correction != 0:
            self.drive(-backoff_mm * scale)
            self.turn(backoff_angle * correction)
            self.drive(backoff_mm * scale)
            return True

        return False

    def move_until_moving_away(self, threshold, current_min=99):
        max_distance = 0
        found = False
        while self.motors_running() and self.distance_front() > threshold:
            max_distance = max(max_distance, self.distance_front())
            print('find object a ' + str(self.distance_front()) + " / " + str(max_distance))
            time.sleep(0.05)
        if self.distance_front() <= threshold:
            min_distance = current_min
            while self.motors_running():
                current_distance = self.distance_front()
                max_distance = max(max_distance, self.distance_front())
                print('find object b ' + str(self.distance_front()) + " / " + str(max_distance))
                print('find object b ' + str(min_distance) + " / " + str(max_distance))
                if current_distance <= min_distance:
                    min_distance = current_distance
                else:
                    found = True
            found = True
        while self.motors_running():
            print('find object c ' + str(self.distance_front()) + " / " + str(max_distance))
            max_distance = max(max_distance, self.distance_front())
            time.sleep(0.05)
        return found, max_distance



    def look_around(self, degrees_each, threshold=20, reverse=False):
        min_distance = self.distance_front()
        mult = 1 if not reverse else -1
        self.turn(-mult * degrees_each, True)
        self.turn(mult * degrees_each * 2, False)
        while self.motors_running() and self.distance_front() > threshold:
            time.sleep(0.05)

        if self.distance_front() <= threshold:
            min_distance = self.distance_front()
            while self.motors_running():
                current_distance = self.distance_front()
                if current_distance <= min_distance:
                    min_distance = current_distance
                else:
                    self.stop()
                    print('stopping at ' + str(current_distance))
                    return True # todo calculate the angle we ended up at

        self.move_until_finished()
        self.turn(-mult * degrees_each, True)

    def open_mouth(self):
        self.led.left.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self.led.right.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self._move_mouth(-5*360)
        self.led.left.color = LED.COLOR.RED
        self.led.right.color = LED.COLOR.RED

    def close_mouth(self):
        self.led.left.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self.led.right.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self._move_mouth(0)

    def eat_ball(self):
        """Assumes ball is in front, close enough"""
        captured = False
        self.open_mouth()
        self.drive(400, False)
        while (self.motors_running()):
            if (self.ball_captured()):
                self.close_mouth()
                captured = True
            else:
                time.sleep(0.05)
        self.close_mouth()
        self.drive(-400, True)
        return captured

    def get_absolute_track_positions(self):
        return (self.left_track.position, self.right_track.position)

    def restore_absolute_track_positions(self, positions, power, ramp):
        print('Current track positions: ' + str(self.get_absolute_track_positions()))
        print('Want to restore: ' + str(positions))
        self._rotate_track_abs(self.left_track, positions[0], power, ramp)
        self._rotate_track_abs(self.right_track, positions[1], power, ramp)

    def _rotate_track_rel(self, track, rel_position, power, ramp):
        track.position_mode = Motor.POSITION_MODE.RELATIVE
        track.run_position_limited(position_sp=rel_position,
                                   speed_sp=power,
                                   stop_mode=Motor.STOP_MODE.BRAKE,
                                   ramp_up_sp=ramp,
                                   ramp_down_sp=ramp)

    def _rotate_track_abs(self, track, abs_position, power, ramp):
        track.position_mode = Motor.POSITION_MODE.ABSOLUTE
        track.run_position_limited(position_sp=abs_position,
                                   speed_sp=power,
                                   stop_mode=Motor.STOP_MODE.BRAKE,
                                   ramp_up_sp=ramp,
                                   ramp_down_sp=ramp)

    def _move_mouth(self, position):
        self.mouth.position_mode = Motor.POSITION_MODE.ABSOLUTE
        self.mouth.run_position_limited(position_sp=position,
                                        speed_sp=1000,
                                        stop_mode=Motor.STOP_MODE.BRAKE)
        while (self.mouth.state != 'idle'):
            time.sleep(0.1)
Exemple #11
0
class Robot:
    def __init__(self):
        self.ir = InfraredSensor()
        self.color = ColorSensor()
        self.left_track = LargeMotor(Motor.PORT.D)
        self.right_track = LargeMotor(Motor.PORT.A)
        self.mouth = MediumMotor(Motor.PORT.B)
        self.key = Key()
        self.led = LED()
        self.reset_leds()

    def reset_leds(self):
        self.led.left.on()
        self.led.right.on()
        self.led.left.color = LED.COLOR.GREEN
        self.led.right.color = LED.COLOR.GREEN

    def keys(self):
        keys = ('up', 'down', 'left', 'right', 'backspace', 'enter')
        dump = [x + ":" + str(getattr(self.key, x)) for x in keys]
        print " ".join(dump)
        return self.key

    def music(self, tune):
        subprocess.call(["aplay", "%s.wav" % tune])

    def distance_front(self):
        return self.ir.prox

    def ball_captured(self):
        while True:
            try:
                return self.color.reflect > 0
            except IOError:
                pass  # this sometimes happens, try again

    def drive(self, distance_mm, block_until_done=True, power=600):
        self.led.left.color = LED.COLOR.AMBER
        print('drive ' + str(distance_mm) + 'mm')
        distance_rotations_ratio = 360 / 109.0
        #distance_rotations_ratio = 360/113.0
        rel_position = distance_mm * distance_rotations_ratio
        ramp = 1000
        self._rotate_track_rel(self.left_track, rel_position, power, ramp)
        self._rotate_track_rel(self.right_track, rel_position, power, ramp)
        if (block_until_done):
            return self.move_until_finished(distance_mm < 0)
        else:
            self.reset_leds()
            return True

    def drive_until_distance(self, ir_distance):
        self.led.right.color = LED.COLOR.AMBER
        print('driving until ir shows ' + str(ir_distance))
        self.drive(20000, False)
        while (self.distance_front() > ir_distance):
            time.sleep(0.1)
        self.stop()
        self.reset_leds()

    def turn(self, degrees, block_until_done=True):
        print('turn ' + str(degrees))
        #ratio = 560 / 90.0
        ratio = 530 / 90.0
        if degrees > 0:
            led = self.led.right
        else:
            led = self.led.left
        led.blink(color=LED.COLOR.AMBER, delay_on=100, delay_off=200)
        rel_position = ratio * degrees
        power = 400
        ramp = 50
        self._rotate_track_rel(self.left_track, -rel_position, power, ramp)
        self._rotate_track_rel(self.right_track, rel_position, power, ramp)
        if (block_until_done):
            return self.move_until_finished()
        else:
            return True

    def motors_running(self):
        return self.left_track.state != 'idle' or self.right_track.state != 'idle'

    def stop(self):
        self.left_track.stop()
        self.right_track.stop()
        self.left_track.reset()
        self.right_track.reset()

    def move_until_finished(self, reverse=False):
        print('move until finished')
        while (self.motors_running()):
            if not reverse and self.distance_front() < 10:
                self.stop()
                print('action interrupted')
                return False  # Was interrupted
            else:
                time.sleep(0.1)
        self.reset_leds()
        return True  # Finished as planned

    def avoid_wall(self,
                   degrees_each,
                   threshold=50,
                   backoff_mm=100,
                   backoff_angle=30):
        if self.distance_front() < 30:
            self.drive(-backoff_mm)
        self.turn(-degrees_each, True)
        self.turn(degrees_each, False)
        found_neg, max_neg = self.move_until_moving_away(threshold)
        self.turn(degrees_each, True)
        self.turn(-degrees_each, False)
        found_pos, max_pos = self.move_until_moving_away(threshold)

        #max_pos = max(80, max_pos)
        #max_neg = max(80, max_pos)

        correction = 0
        scale = 1
        print("positive degrees: " + str(found_pos) + ", " + str(max_pos))
        print("negative degrees: " + str(found_neg) + ", " + str(max_neg))
        if found_pos and not found_neg:
            correction = -1
        elif found_neg and not found_pos:
            correction = 1
        elif found_pos and found_neg:
            scale = 2
            if max_pos > max_neg:
                correction = 1
            elif max_neg > max_pos:
                correction = -1

        print("correction: " + str(correction) + ", scale: " + str(scale))
        if correction != 0:
            self.drive(-backoff_mm * scale)
            self.turn(backoff_angle * correction)
            self.drive(backoff_mm * scale)
            return True

        return False

    def move_until_moving_away(self, threshold, current_min=99):
        max_distance = 0
        found = False
        while self.motors_running() and self.distance_front() > threshold:
            max_distance = max(max_distance, self.distance_front())
            print('find object a ' + str(self.distance_front()) + " / " +
                  str(max_distance))
            time.sleep(0.05)
        if self.distance_front() <= threshold:
            min_distance = current_min
            while self.motors_running():
                current_distance = self.distance_front()
                max_distance = max(max_distance, self.distance_front())
                print('find object b ' + str(self.distance_front()) + " / " +
                      str(max_distance))
                print('find object b ' + str(min_distance) + " / " +
                      str(max_distance))
                if current_distance <= min_distance:
                    min_distance = current_distance
                else:
                    found = True
            found = True
        while self.motors_running():
            print('find object c ' + str(self.distance_front()) + " / " +
                  str(max_distance))
            max_distance = max(max_distance, self.distance_front())
            time.sleep(0.05)
        return found, max_distance

    def look_around(self, degrees_each, threshold=20, reverse=False):
        min_distance = self.distance_front()
        mult = 1 if not reverse else -1
        self.turn(-mult * degrees_each, True)
        self.turn(mult * degrees_each * 2, False)
        while self.motors_running() and self.distance_front() > threshold:
            time.sleep(0.05)

        if self.distance_front() <= threshold:
            min_distance = self.distance_front()
            while self.motors_running():
                current_distance = self.distance_front()
                if current_distance <= min_distance:
                    min_distance = current_distance
                else:
                    self.stop()
                    print('stopping at ' + str(current_distance))
                    return True  # todo calculate the angle we ended up at

        self.move_until_finished()
        self.turn(-mult * degrees_each, True)

    def open_mouth(self):
        self.led.left.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self.led.right.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self._move_mouth(-5 * 360)
        self.led.left.color = LED.COLOR.RED
        self.led.right.color = LED.COLOR.RED

    def close_mouth(self):
        self.led.left.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self.led.right.blink(color=LED.COLOR.RED, delay_on=100, delay_off=200)
        self._move_mouth(0)

    def eat_ball(self):
        """Assumes ball is in front, close enough"""
        captured = False
        self.open_mouth()
        self.drive(400, False)
        while (self.motors_running()):
            if (self.ball_captured()):
                self.close_mouth()
                captured = True
            else:
                time.sleep(0.05)
        self.close_mouth()
        self.drive(-400, True)
        return captured

    def get_absolute_track_positions(self):
        return (self.left_track.position, self.right_track.position)

    def restore_absolute_track_positions(self, positions, power, ramp):
        print('Current track positions: ' +
              str(self.get_absolute_track_positions()))
        print('Want to restore: ' + str(positions))
        self._rotate_track_abs(self.left_track, positions[0], power, ramp)
        self._rotate_track_abs(self.right_track, positions[1], power, ramp)

    def _rotate_track_rel(self, track, rel_position, power, ramp):
        track.position_mode = Motor.POSITION_MODE.RELATIVE
        track.run_position_limited(position_sp=rel_position,
                                   speed_sp=power,
                                   stop_mode=Motor.STOP_MODE.BRAKE,
                                   ramp_up_sp=ramp,
                                   ramp_down_sp=ramp)

    def _rotate_track_abs(self, track, abs_position, power, ramp):
        track.position_mode = Motor.POSITION_MODE.ABSOLUTE
        track.run_position_limited(position_sp=abs_position,
                                   speed_sp=power,
                                   stop_mode=Motor.STOP_MODE.BRAKE,
                                   ramp_up_sp=ramp,
                                   ramp_down_sp=ramp)

    def _move_mouth(self, position):
        self.mouth.position_mode = Motor.POSITION_MODE.ABSOLUTE
        self.mouth.run_position_limited(position_sp=position,
                                        speed_sp=1000,
                                        stop_mode=Motor.STOP_MODE.BRAKE)
        while (self.mouth.state != 'idle'):
            time.sleep(0.1)