Beispiel #1
0
    def __init__(self, drive_autonomous=False, loop=None):
        """ Initialise Robot """

        self.board = PyMata3()
        self.dc_motors = DcMotors(self.board)
        self.sonar = Sonar(self.board)
        self.controller = RemoteControl()
        self.audio = Audio()

        if loop:
            self.loop = loop
        else:
            self.loop = self.board.loop
        self.drive_autonomous = drive_autonomous
Beispiel #2
0
class Robot:

    def __init__(self, drive_autonomous=False, loop=None):
        """ Initialise Robot """

        self.board = PyMata3()
        self.dc_motors = DcMotors(self.board)
        self.sonar = Sonar(self.board)
        self.controller = RemoteControl()
        self.audio = Audio()

        if loop:
            self.loop = loop
        else:
            self.loop = self.board.loop
        self.drive_autonomous = drive_autonomous

    def run(self):
        """ This is the main running loop

        This will handle time based action like starting, stopping
        and turning for x amount of time when an obstacle is detected
        """

        music_playing = False
        turn_attempts = 0

        print('Starting main loop')
        while not self.controller.START:
            future = asyncio.ensure_future(self.controller.handle_events())
            self.loop.run_until_complete(future)

            # Switch driving mode
            if self.controller.SELECT:
                if self.drive_autonomous:
                    print('Switching to remote controlled driving')
                    self.drive_autonomous = False
                else:
                    print('Switching to autonomous driving')
                    self.drive_autonomous = True
                    turn_attempts = 0
                    self.dc_motors.forward(100)
            
            if self.controller.CROSS and not music_playing:
                # Start the music baby
                self.loop.run_until_complete(self.audio.play_audio('static/audio/hello_son.mp3'))
                music_playing = True
    
            if self.drive_autonomous:
                self._detect_obstacles(turn_attempts)
            else:
                self._manual_control()

    def _manual_control(self):
        """ Handles one loop cycle of manual driving """
        if self.controller.UP and self.controller.LEFT:
            self.dc_motors.up_left(255)
        elif self.controller.UP and self.controller.RIGHT:
            self.dc_motors.up_right(255)
        elif self.controller.DOWN and self.controller.LEFT:
            self.dc_motors.down_left(255)
        elif self.controller.DOWN and self.controller.RIGHT:
            self.dc_motors.down_right(255)
        elif self.controller.UP:
            self.dc_motors.forward(255)
        elif self.controller.DOWN:
            self.dc_motors.reverse(255)
        elif self.controller.LEFT:
            self.dc_motors.left(255)
        elif self.controller.RIGHT:
            self.dc_motors.right(255)
        else:
            self.dc_motors.stop()

    def _detect_obstacles(self, turn_attempts):
        """ Handles one loop cycle of autonomous driving """
        current_time = time.time()
        distance = self.sonar.distance
        print('Distance: %s' % distance)

        # Getting to close, lets try to turn
        if distance < MIN_DISTANCE and not self.dc_motors.state.startswith('turning'):
            turn_attempts += 1
            random_value = random.randint(1, 2)
            if random_value == 1:
                print('Getting too close, turning left for 2 seconds!')
                self.dc_motors.left(255, 3)
            elif random_value == 2:
                self.dc_motors.right(255, 3)
                print('Getting too close, turning right for 2 seconds!')

        # Turning doesnt seem to work, lets go in reverse
        elif turn_attempts > MAX_TURN_ATTEMPTS:
            self.dc_motors.reverse(100, 3)
            turn_attempts = 0

        # Handle finished motor actions
        elif self.dc_motors.action_time_duration:
            action_end_time = self.dc_motors.action_time_start + self.dc_motors.action_time_duration

            print('start: {}\nduration: {}\nend: {}\ncurrent: {}'
                  .format(self.dc_motors.action_time_start,
                          self.dc_motors.action_time_duration,
                          action_end_time,
                          current_time))

            # TODO: I DONT GET THIS AT ALL!! THIS SHOULD READ
            # if action_end_time >= current_time:
            # BUT THEN IT DOESNT WORK. QUESS IM UP TOO LONG TO SEE IT
            if action_end_time <= current_time:
                # Check if the obstacle is cleared
                if self.dc_motors.state.startswith('turning'):
                    if distance < MIN_DISTANCE:
                        print("%s for 2 seconds wasn't enough. Lets do another 2 sec" % self.dc_motors.state)
                        if self.dc_motors.state.endswith('left'):
                            self.dc_motors.left(255, 2)
                        else:
                            self.dc_motors.right(255, 2)
                        turn_attempts += 1
                    else:
                        print('Ok, obstacle cleared, moving forward...')
                        self.dc_motors.forward(100)
                        turn_attempts = 0

                # Reverse action completed, lets turn around
                elif self.dc_motors.state == 'reverse':
                    # TODO: Deduplicate random turn code
                    turn_attempts += 1
                    random_value = random.randint(1, 2)
                    if random_value == 1:
                        print('Finished reversing, turning left for 2 sec')
                        self.dc_motors.left(255, 2)
                    elif random_value == 2:
                        self.dc_motors.right(255, 2)
                        print('Finished reversing, turning right for 2 sec')

        # run for a bit before re-iterating through the loop again
        self.board.sleep(0.2)

    def shutdown(self):
        """ Shutdown robot """
        self.board.shutdown()
        loop = asyncio.get_event_loop()
        loop.run_until_complete(asyncio.sleep(.1))
        for t in asyncio.Task.all_tasks(loop):
            t.cancel()
        loop.run_until_complete(asyncio.sleep(.1))
        loop.stop()
        loop.close()
        sys.exit(0)