def disconnect(self):
        try:
            # Disconnect serial
            self._drive_serial.close()
            self._sensor_serial.close()

            # Set ready status
            self._ready = False

            # Print status
            messageutils.print_info('Arduino serial ports successfully disconnected')
        except serial.SerialException:
            messageutils.print_error('Could not disconnect from Arduino serial ports')
    def connect(self):
        try:
            # Connect to serial
            self._drive_serial = serial.Serial(self._drive_com_port, baudrate=self._drive_baud_rate, timeout=5, write_timeout=5)
            self._sensor_serial = serial.Serial(self._sensor_com_port, baudrate=self._sensor_baud_rate, timeout=5, write_timeout=5)

            # Sleep while connection completes
            time.sleep(1)

            # Set ready status
            self._ready = True

            # Print status
            messageutils.print_info('Arduino serial ports successfully connected')
        except serial.SerialException:
            messageutils.print_error('Could not connect to Arduino serial ports')
    def rotate(self, rate, degrees):
        # Check RPS status before continuing
        if not self._ready:
            messageutils.print_error('Serial not connected')
            return

        # Get initial angle
        x, y, r = self.get_position()

        initial_r = r

        if degrees > 0:
            self.set_rotation_rate(rate)

            while r < initial_r + degrees:
                x, y, r = self.get_position()
                messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

            self.set_rotation_rate(0)
        elif degrees < 0:
            self.set_rotation_rate(-rate)

            while r > initial_r + degrees:
                x, y, r = self.get_position()
                messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

            self.set_rotation_rate(0)

        messageutils.print_info("Turn complete")
    def travel(self, rate, x_dist, y_dist):
        # Check RPS status before continuing
        if not self._ready:
            messageutils.print_error('Serial not connected')
            return

        # Status update
        messageutils.print_info('Traveling %d forward/back and %d right/left' % (y_dist, x_dist))

        # Calculate slow rate ahead of time
        if rate >= 500:
            slow_rate = 500
        else:
            slow_rate = rate

        correction_rate = 200

        # Zero position
        self.zero_position()

        # Get current position
        x, y, r = self.get_position()

        # Handle X positioning
        if x_dist != 0:
            if x_dist > 0:
                # Set the velocity
                self.set_velocity(rate, 0)

                # Travel until distance has been reached
                while x < x_dist - 3:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Approaching target')

                # Slow down for accuracy
                self.set_velocity(slow_rate, 0)

                # Travel until distance has been reached
                while x < x_dist:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Target met')
            elif x_dist < 0:
                # Set the X velocity
                self.set_velocity(-rate, 0)

                # Travel until distance has been reached
                while x > x_dist + 3:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Approaching target')

                # Slow down for accuracy
                self.set_velocity(-slow_rate, 0)

                # Travel until distance has been reached
                while x > x_dist:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Target met')

            # Distance met, stop moving
            self.set_velocity(0, 0)

            # Determine X error
            x, y, r = self.get_position()

            if abs(y) > 0:
                messageutils.print_info('Correcting Y error')

                # Correct x change
                if y > 0:
                    self.set_velocity(0, -correction_rate)
                    while y > 0:
                        x, y, r = self.get_position()
                        messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))
                elif y < 0:
                    self.set_velocity(0, correction_rate)
                    while y < 0:
                        x, y, r = self.get_position()
                        messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

            # Final stop
            self.set_velocity(0, 0)

        # Zero position
        self.zero_position()

        # Get current position
        x, y, r = self.get_position()

        # Handle Y positioning
        if y_dist != 0:
            if y_dist > 0:
                # Set the velocity
                self.set_velocity(0, rate)

                # Travel until distance has been reached
                while y < y_dist - 3:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Approaching target')

                # Slow down for accuracy
                self.set_velocity(0, slow_rate)

                # Travel until distance has been reached
                while y < y_dist:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Target met')
            elif y_dist < 0:
                # Set the X velocity
                self.set_velocity(0, -rate)

                # Travel until distance has been reached
                while y > y_dist + 3:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Approaching target')

                # Slow down for accuracy
                self.set_velocity(0, -slow_rate)

                # Travel until distance has been reached
                while y > y_dist:
                    x, y, r = self.get_position()
                    messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

                # Status update
                messageutils.print_info('Target met')

            # Distance met, stop moving
            self.set_velocity(0, 0)

            # Determine X error
            x, y, r = self.get_position()

            if abs(x) > 0:
                messageutils.print_info('Correcting X error')

                # Correct x change
                if x > 0:
                    self.set_velocity(-correction_rate, 0)
                    while x > 0:
                        x, y, r = self.get_position()
                        messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))
                elif x < 0:
                    self.set_velocity(correction_rate, 0)
                    while x < 0:
                        x, y, r = self.get_position()
                        messageutils.print_info('X: ' + str(x) + ', Y: ' + str(y) + ', R: ' + str(r))

            # Final stop
            self.set_velocity(0, 0)