<reponame>BotsForRoss/PyCNC
from __future__ import division

import cnc.logging_config as logging_config
from cnc import hal
from cnc.pulses import *
from cnc.coordinates import *
from cnc.heater import *
from cnc.enums import *
from cnc.watchdog import *
from cnc.audio import AudioPlayer


class GMachineException(Exception):
    """ Exceptions while processing gcode line.
    """
    pass


class GMachine(object):
    """ Main object which control and keep state of whole machine: steppers,
        spindle, extruder etc
    """

    def __init__(self):
        """ Initialization.
        """
        self._position = Coordinates(0.0, 0.0, 0.0, 0.0)
        # init variables
        self._velocity = 0
        self._local = None
        self._convertCoordinates = 0
        self._absoluteCoordinates = 0
        self._plane = None
        self._extruder_id = 0
        hal.init()
        self.watchdog = HardwareWatchdog()

        self.reset()

    def release(self):
        """ Free all resources.
        """
        AudioPlayer.stop()
        hal.deinit()

    def reset(self):
        """ Reinitialize all program configurable thing.
        """
        self._velocity = min(MAX_VELOCITY_MM_PER_MIN_X,
                             MAX_VELOCITY_MM_PER_MIN_Y,
                             MAX_VELOCITY_MM_PER_MIN_Z,
                             self._get_extruder_max_speed())
        self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
        self._convertCoordinates = 1.0
        self._absoluteCoordinates = True
        self._plane = PLANE_XY

    def __check_delta(self, delta):
        pos = self._position + delta
        if not pos.is_in_aabb(Coordinates(0.0, 0.0, 0.0, 0.0),
                Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM, 0)) or \
                pos.e < 0 or pos.e > EXTRUDER_LENGTH_MM:
            raise GMachineException("out of effective area")

    # noinspection PyMethodMayBeStatic
    def __check_velocity(self, max_velocity):
        if max_velocity.x > MAX_VELOCITY_MM_PER_MIN_X \
                or max_velocity.y > MAX_VELOCITY_MM_PER_MIN_Y \
                or max_velocity.z > MAX_VELOCITY_MM_PER_MIN_Z \
                or max_velocity.e > self._get_extruder_max_speed():
            raise GMachineException("out of maximum speed")

    def _get_extruder(self):
        """
        Returns:
            Extruder -- the current extruder
        """

        return hal.get_extruder(self._extruder_id)

    def _get_extruder_max_speed(self):
        """
        Returns:
            float -- the max speed of the current extruder in mm/min
        """

        return self._get_extruder().get_max_speed() * 60.0

    def _get_extruder_speed(self, delta_mm, velocity_mm_per_min):
        """
        Pull the E axis speed out of the delta position vector and overall speed about all axes.
        This is also done in pulses.py, but must be repeated because extruders don't use pulses.

        Arguments:
            delta_mm {Coordinates} -- the difference in position in mm
            velocity_mm_per_min {float} -- the speed about all axes in mm/min

        Returns:
            {float} -- the speed of the E axis only
        """
        distance_mm = abs(delta_mm)
        distance_total_mm = distance_mm.length()
        if distance_total_mm == 0:
            return 0
        velocity = distance_mm * (velocity_mm_per_min / distance_total_mm)
        return velocity.e

    def _move_linear(self, delta, velocity):
        delta = delta.round_to_nearest_pulse()
        if delta.is_zero():
            return
        self.__check_delta(delta)

        logging.info("Moving linearly {}".format(delta))
        gen = PulseGeneratorLinear(delta, velocity)
        self.__check_velocity(gen.max_velocity())

        extruder_speed = self._get_extruder_speed(delta, velocity)
        self._start_extruder_move(delta.e, extruder_speed)
        hal.move(gen)

        # save position
        self._position = self._position + delta

    @staticmethod
    def __quarter(a, b):
        """Takes the coordinates a and b of a point relative to the center of the circle
            and returns the quarter of the circle the point is in

        Arguments:
            a {float} -- The first coordinate
            b {float} -- The second coordinate

        Returns:
            {int} -- The quarter the specified point exists in
        """

        if a > 0 and b >= 0:
            return 1
        if a <= 0 and b > 0:
            return 2
        if a < 0 and b <= 0:
            return 3
        if a >= 0 and b < 0:
            return 4

    def __check_circle(self, delta_a, delta_b, radius_a, radius_b, direction, position_a, position_b,
                       table_a, table_b, pulses_per_mm_a, pulses_per_mm_b):
        """Validates the circle to be drawn, checking for a valid radius, a valid endpoint, and if the circle
            is bounded within the table

            The coordinates are labeled as a and b because the plane is selectable and a and b stand for arbitrary
            combinations of x, y and z.

        Arguments:
            delta_a, delta_b {float} -- coordinates of the endpoint relative to the position
            radius_a, radius_b {float} -- coordinates of the circle's center relative to the position
            direction {RotationalDirection(Enum)} -- the direction the circle will draw
            position_a, position_b {float} -- the current position (starting position) in absolute coordinates
            table_a, table_b {int} -- the table's maximum for both the a and b axis
            pulses_per_mm_a, pulses_per_mm_b {float} -- the pulses per mm for both axis'

        Raises:
            GMachineException -- raised if the radius is zero
            GMachineException -- raised if the endpoint not on the defined circle
            GMachineException -- raised if the circle would draw out of bounds
        """

        radius = math.hypot(radius_a, radius_b)
        if radius == 0:
            raise GMachineException("circle radius is zero")
        # check if (delta_a, delta_b) is on the specified circle
        if not math.isclose(math.hypot(delta_a - radius_a, delta_b - radius_b), radius,
                            rel_tol=min(1.0 / pulses_per_mm_a, 1.0 / pulses_per_mm_b)):
            raise GMachineException("endpoint not on circle")
        # check if the drawn circle is inside the table
        start_quarter = GMachine.__quarter(-radius_a, -radius_b)
        if delta_a == 0 and delta_b == 0: # If the endpoint and position are the same, check the full circle
            end_quarter = 5
        else:
            end_quarter = GMachine.__quarter(delta_a - radius_a, delta_b - radius_b)

        if start_quarter == end_quarter:
            return

        # If the start and end points are not in the same quarter, there will be new maximum values that have to be checked against
        # boundry conditions
        is_raise = False
        quarter = start_quarter
        prev_quarter = quarter
        for _ in range(4):
            if direction == CW:
                quarter -= 1
                if quarter == 0: quarter = 4
            else:
                quarter += 1
                if quarter == 5: quarter = 1

            if (quarter == 1 and prev_quarter == 4) or (quarter == 4 and prev_quarter == 1):
                is_raise = (position_a + radius_a + radius > table_a)
            elif (quarter == 1 and prev_quarter == 2) or (quarter == 2 and prev_quarter == 1):
                is_raise = (position_b + radius_b + radius > table_b)
            elif (quarter == 2 and prev_quarter == 3) or (quarter == 3 and prev_quarter == 2):
                is_raise = (position_a + radius_a - radius < 0)
            elif (quarter == 3 and prev_quarter == 4) or (quarter == 4 and prev_quarter == 3):
                is_raise = (position_b + radius_b - radius < 0)
            if is_raise:
                raise GMachineException("circle out of bounds")

            if quarter == end_quarter:
                break
            prev_quarter = quarter


    def _move_circular(self, delta, radius, velocity, direction):
        delta = delta.round_to_nearest_pulse()
        self.__check_delta(delta)
        # get delta vector and put it on circle
        if self._plane == PLANE_XY:
            self.__check_circle(delta.x, delta.y, radius.x, radius.y,
                                direction, self._position.x,
                                self._position.y, TABLE_SIZE_X_MM,
                                TABLE_SIZE_Y_MM, STEPPER_PULSES_PER_MM_X,
                                STEPPER_PULSES_PER_MM_Y)
        elif self._plane == PLANE_YZ:
            self.__check_circle(delta.y, delta.z, radius.y, radius.z,
                                direction, self._position.y,
                                self._position.z, TABLE_SIZE_Y_MM,
                                TABLE_SIZE_Z_MM, STEPPER_PULSES_PER_MM_Y,
                                STEPPER_PULSES_PER_MM_Z)
        elif self._plane == PLANE_ZX:
            self.__check_circle(delta.z, delta.x, radius.z, radius.x,
                                direction, self._position.z,
                                self._position.x, TABLE_SIZE_Z_MM,
                                TABLE_SIZE_X_MM, STEPPER_PULSES_PER_MM_Z,
                                STEPPER_PULSES_PER_MM_X)
        radius = radius.round_to_nearest_pulse()
        logging.info("Moving circularly {} {} {} with radius {}"
                     " and velocity {}".format(self._plane, delta,
                                               direction, radius, velocity))
        gen = PulseGeneratorCircular(delta, radius, self._plane,
                                     direction, velocity)
        self.__check_velocity(gen.max_velocity())
        # do movements
        extruder_speed = self._get_extruder_speed(delta, velocity)
        self._start_extruder_move(delta.e, extruder_speed)
        hal.move(gen)
        # save position
        self._position = self._position + delta

    def safe_zero(self, x=True, y=True, z=True):
        """ Move head to zero position safely.
        :param x: boolean, move X axis to zero
        :param y: boolean, move Y axis to zero
        :param z: boolean, move Z axis to zero
        """
        if x and not y:
            self._move_linear(Coordinates(-self._position.x, 0, 0, 0),
                              MAX_VELOCITY_MM_PER_MIN_X)
        elif y and not x:
            self._move_linear(Coordinates(0, -self._position.y, 0, 0),
                              MAX_VELOCITY_MM_PER_MIN_X)
        elif x and y:
            d = Coordinates(-self._position.x, -self._position.y, 0, 0)
            self._move_linear(d, min(MAX_VELOCITY_MM_PER_MIN_X,
                                     MAX_VELOCITY_MM_PER_MIN_Y))
        if z:
            d = Coordinates(0, 0, -self._position.z, 0)
            self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z)

    def position(self):
        """ Return current machine position (after the latest command)
            Note that hal might still be moving motors and in this case
            function will block until motors stops.
            This function for tests only.
            :return current position.
        """
        hal.join()
        return self._position

    def plane(self):
        """ Return current plane for circular interpolation. This function for
            tests only.
            :return current plane.
        """
        return self._plane

    def _start_extruder_move(self, delta, speed):
        """
        Start moving the current extruder asynchronously

        Arguments:
            delta {float} -- the difference in position, in mm
            speed {float} -- the speed to move the extruder, in mm/minute
        """
        extruder = hal.get_extruder(self._extruder_id)
        pos = extruder.get_position()
        extruder.set_position(pos + delta, speed / 60.0)

    def _set_extruder(self, extruder_id):
        """
        Change extruders

        Arguments:
            extruder_id {int} -- the id of the extruder from 0 to NUM_EXTRUDERS
        """
        if extruder_id < 0 or extruder_id >= len(EXTRUDER_CONFIG):
            raise ValueError('invalid extruder id {}'.format(extruder_id))
        extruder = hal.get_extruder(extruder_id)
        extruder.join()
        self._position.e = extruder.get_position()
        self._extruder_id = extruder_id

    def do_command(self, gcode):
        """ Perform action.
        :param gcode: GCode object which represent one gcode line
        :return String if any answer require, None otherwise.
        """
        if gcode is None:
            return None
        answer = None
        logging.debug("got command " + str(gcode.params))
        # read command
        c = gcode.command()
        if c is None and gcode.has_coordinates():
            c = 'G1'
        # read parameters
        if self._absoluteCoordinates:
            coord = gcode.coordinates(self._position - self._local,
                                      self._convertCoordinates)
            coord = coord + self._local
            delta = coord - self._position
        else:
            delta = gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
                                      self._convertCoordinates)
            # coord = self._position + delta
        velocity = gcode.get('F', self._velocity)
        radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
                              self._convertCoordinates)
        # check parameters
        if velocity < MIN_VELOCITY_MM_PER_MIN:
            raise GMachineException("feed speed too low")
        # select command and run it
        if c == 'G0':  # rapid move
            vl = max(MAX_VELOCITY_MM_PER_MIN_X,
                     MAX_VELOCITY_MM_PER_MIN_Y,
                     MAX_VELOCITY_MM_PER_MIN_Z,
                     self._get_extruder_max_speed())
            l = delta.length()
            if l > 0:
                proportion = abs(delta) / l
                if proportion.x > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_X / proportion.x)
                    if v < vl:
                        vl = v
                if proportion.y > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_Y / proportion.y)
                    if v < vl:
                        vl = v
                if proportion.z > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_Z / proportion.z)
                    if v < vl:
                        vl = v
                if proportion.e > 0:
                    v = int(self._get_extruder_max_speed() / proportion.e)
                    if v < vl:
                        vl = v
            self._move_linear(delta, vl)
        elif c == 'G1':  # linear interpolation
            self._move_linear(delta, velocity)
        elif c == 'G2':  # circular interpolation, clockwise
            self._move_circular(delta, radius, velocity, CW)
        elif c == 'G3':  # circular interpolation, counterclockwise
            self._move_circular(delta, radius, velocity, CCW)
        elif c == 'G4':  # delay in s
            if not gcode.has('P'):
                raise GMachineException("P is not specified")
            pause = gcode.get('P', 0)
            if pause < 0:
                raise GMachineException("bad delay")
            hal.join()
            time.sleep(pause)
        elif c == 'G17':  # XY plane select
            self._plane = PLANE_XY
        elif c == 'G18':  # ZX plane select
            self._plane = PLANE_ZX
        elif c == 'G19':  # YZ plane select
            self._plane = PLANE_YZ
        elif c == 'G20':  # switch to inches
            self._convertCoordinates = 25.4
        elif c == 'G21':  # switch to mm
            self._convertCoordinates = 1.0
        elif c == 'G28':  # home
            axises = gcode.has('X'), gcode.has('Y'), gcode.has('Z')
            if axises == (False, False, False):
                axises = True, True, True
            self.safe_zero(*axises)
            hal.join()
            if not hal.calibrate(*axises):
                raise GMachineException("failed to calibrate")
        elif c == 'G53':  # switch to machine coords
            raise GMachineException('Not supported')
            # TODO not sure what local/machine coordinates mean and how to handle this with multiple extruders
            # self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
        elif c == 'G90':  # switch to absolute coords
            self._absoluteCoordinates = True
        elif c == 'G91':  # switch to relative coords
            self._absoluteCoordinates = False
        elif c == 'G92':  # switch to local coords
            raise GMachineException('Not supported')
            # if gcode.has_coordinates():
            #     self._local = self._position - gcode.coordinates(
            #         Coordinates(self._position.x - self._local.x,
            #                     self._position.y - self._local.y,
            #                     self._position.z - self._local.z,
            #                     self._position.e - self._local.e),
            #         self._convertCoordinates)
            # else:
            #     self._local = self._position
        elif c == 'M2' or c == 'M30':  # program finish, reset everything.
            self.reset()
        elif c == 'M72':
            audio_id = int(gcode.get('P', 0))
            if audio_id not in AUDIO_FILES:
                raise GMachineException('Audio ID not recognized')
            audio_filepath = AUDIO_BASE_FILEPATH + AUDIO_FILES[audio_id]
            logging.info('playing audio from {}'.format(audio_filepath))
            AudioPlayer.play(audio_filepath)
        elif c == 'M84':  # disable motors
            hal.disable_steppers()
        elif c == 'M111':  # enable debug
            logging_config.debug_enable()
        elif c == 'M114':  # get current position
            hal.join()
            p = self.position()
            answer = "X:{} Y:{} Z:{} E:{}".format(p.x, p.y, p.z, p.e)
        elif c == 'T':  # select tool (extruder)
            self._set_extruder(int(gcode.get('T')))
        elif c is None:  # command not specified(ie just F was passed)
            pass
        # commands below are added just for compatibility
        elif c == 'M82':  # absolute mode for extruder
            if not self._absoluteCoordinates:
                raise GMachineException("Not supported, use G90/G91")
        elif c == 'M83':  # relative mode for extruder
            if self._absoluteCoordinates:
                raise GMachineException("Not supported, use G90/G91")
        else:
            raise GMachineException("unknown command")
        # save parameters on success
        self._velocity = velocity
        logging.debug("position {}".format(self._position))
        return answer

from __future__ import division

import cnc.logging_config as logging_config
from cnc import hal
from cnc.pulses import *
from cnc.coordinates import *
from cnc.heater import *
from cnc.enums import *
from cnc.watchdog import *
from cnc.audio import AudioPlayer


class GMachineException(Exception):
    """ Exceptions while processing gcode line.
    """
    pass


class GMachine(object):
    """ Main object which control and keep state of whole machine: steppers,
        spindle, extruder etc
    """

    def __init__(self):
        """ Initialization.
        """
        self._position = Coordinates(0.0, 0.0, 0.0, 0.0)
        # init variables
        self._velocity = 0
        self._local = None
        self._convertCoordinates = 0
        self._absoluteCoordinates = 0
        self._plane = None
        self._extruder_id = 0
        hal.init()
        self.watchdog = HardwareWatchdog()

        self.reset()

    def release(self):
        """ Free all resources.
        """
        AudioPlayer.stop()
        hal.deinit()

    def reset(self):
        """ Reinitialize all program configurable thing.
        """
        self._velocity = min(MAX_VELOCITY_MM_PER_MIN_X,
                             MAX_VELOCITY_MM_PER_MIN_Y,
                             MAX_VELOCITY_MM_PER_MIN_Z,
                             self._get_extruder_max_speed())
        self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
        self._convertCoordinates = 1.0
        self._absoluteCoordinates = True
        self._plane = PLANE_XY

    def __check_delta(self, delta):
        pos = self._position + delta
        if not pos.is_in_aabb(Coordinates(0.0, 0.0, 0.0, 0.0),
                Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM, 0)) or \
                pos.e < 0 or pos.e > EXTRUDER_LENGTH_MM:
            raise GMachineException("out of effective area")

    # noinspection PyMethodMayBeStatic
    def __check_velocity(self, max_velocity):
        if max_velocity.x > MAX_VELOCITY_MM_PER_MIN_X \
                or max_velocity.y > MAX_VELOCITY_MM_PER_MIN_Y \
                or max_velocity.z > MAX_VELOCITY_MM_PER_MIN_Z \
                or max_velocity.e > self._get_extruder_max_speed():
            raise GMachineException("out of maximum speed")

    def _get_extruder(self):
        """
        Returns:
            Extruder -- the current extruder
        """

        return hal.get_extruder(self._extruder_id)

    def _get_extruder_max_speed(self):
        """
        Returns:
            float -- the max speed of the current extruder in mm/min
        """

        return self._get_extruder().get_max_speed() * 60.0

    def _get_extruder_speed(self, delta_mm, velocity_mm_per_min):
        """
        Pull the E axis speed out of the delta position vector and overall speed about all axes.
        This is also done in pulses.py, but must be repeated because extruders don't use pulses.

        Arguments:
            delta_mm {Coordinates} -- the difference in position in mm
            velocity_mm_per_min {float} -- the speed about all axes in mm/min

        Returns:
            {float} -- the speed of the E axis only
        """
        distance_mm = abs(delta_mm)
        distance_total_mm = distance_mm.length()
        if distance_total_mm == 0:
            return 0
        velocity = distance_mm * (velocity_mm_per_min / distance_total_mm)
        return velocity.e

    def _move_linear(self, delta, velocity):
        delta = delta.round_to_nearest_pulse()
        if delta.is_zero():
            return
        self.__check_delta(delta)

        logging.info("Moving linearly {}".format(delta))
        gen = PulseGeneratorLinear(delta, velocity)
        self.__check_velocity(gen.max_velocity())

        extruder_speed = self._get_extruder_speed(delta, velocity)
        self._start_extruder_move(delta.e, extruder_speed)
        hal.move(gen)

        # save position
        self._position = self._position + delta

    @staticmethod
    def __quarter(a, b):
        """Takes the coordinates a and b of a point relative to the center of the circle
            and returns the quarter of the circle the point is in

        Arguments:
            a {float} -- The first coordinate
            b {float} -- The second coordinate

        Returns:
            {int} -- The quarter the specified point exists in
        """

        if a > 0 and b >= 0:
            return 1
        if a <= 0 and b > 0:
            return 2
        if a < 0 and b <= 0:
            return 3
        if a >= 0 and b < 0:
            return 4

    def __check_circle(self, delta_a, delta_b, radius_a, radius_b, direction, position_a, position_b,
                       table_a, table_b, pulses_per_mm_a, pulses_per_mm_b):
        """Validates the circle to be drawn, checking for a valid radius, a valid endpoint, and if the circle
            is bounded within the table

            The coordinates are labeled as a and b because the plane is selectable and a and b stand for arbitrary
            combinations of x, y and z.

        Arguments:
            delta_a, delta_b {float} -- coordinates of the endpoint relative to the position
            radius_a, radius_b {float} -- coordinates of the circle's center relative to the position
            direction {RotationalDirection(Enum)} -- the direction the circle will draw
            position_a, position_b {float} -- the current position (starting position) in absolute coordinates
            table_a, table_b {int} -- the table's maximum for both the a and b axis
            pulses_per_mm_a, pulses_per_mm_b {float} -- the pulses per mm for both axis'

        Raises:
            GMachineException -- raised if the radius is zero
            GMachineException -- raised if the endpoint not on the defined circle
            GMachineException -- raised if the circle would draw out of bounds
        """

        radius = math.hypot(radius_a, radius_b)
        if radius == 0:
            raise GMachineException("circle radius is zero")
        # check if (delta_a, delta_b) is on the specified circle
        if not math.isclose(math.hypot(delta_a - radius_a, delta_b - radius_b), radius,
                            rel_tol=min(1.0 / pulses_per_mm_a, 1.0 / pulses_per_mm_b)):
            raise GMachineException("endpoint not on circle")
        # check if the drawn circle is inside the table
        start_quarter = GMachine.__quarter(-radius_a, -radius_b)
        if delta_a == 0 and delta_b == 0: # If the endpoint and position are the same, check the full circle
            end_quarter = 5
        else:
            end_quarter = GMachine.__quarter(delta_a - radius_a, delta_b - radius_b)

        if start_quarter == end_quarter:
            return

        # If the start and end points are not in the same quarter, there will be new maximum values that have to be checked against
        # boundry conditions
        is_raise = False
        quarter = start_quarter
        prev_quarter = quarter
        for _ in range(4):
            if direction == CW:
                quarter -= 1
                if quarter == 0: quarter = 4
            else:
                quarter += 1
                if quarter == 5: quarter = 1

            if (quarter == 1 and prev_quarter == 4) or (quarter == 4 and prev_quarter == 1):
                is_raise = (position_a + radius_a + radius > table_a)
            elif (quarter == 1 and prev_quarter == 2) or (quarter == 2 and prev_quarter == 1):
                is_raise = (position_b + radius_b + radius > table_b)
            elif (quarter == 2 and prev_quarter == 3) or (quarter == 3 and prev_quarter == 2):
                is_raise = (position_a + radius_a - radius < 0)
            elif (quarter == 3 and prev_quarter == 4) or (quarter == 4 and prev_quarter == 3):
                is_raise = (position_b + radius_b - radius < 0)
            if is_raise:
                raise GMachineException("circle out of bounds")

            if quarter == end_quarter:
                break
            prev_quarter = quarter


    def _move_circular(self, delta, radius, velocity, direction):
        delta = delta.round_to_nearest_pulse()
        self.__check_delta(delta)
        # get delta vector and put it on circle
        if self._plane == PLANE_XY:
            self.__check_circle(delta.x, delta.y, radius.x, radius.y,
                                direction, self._position.x,
                                self._position.y, TABLE_SIZE_X_MM,
                                TABLE_SIZE_Y_MM, STEPPER_PULSES_PER_MM_X,
                                STEPPER_PULSES_PER_MM_Y)
        elif self._plane == PLANE_YZ:
            self.__check_circle(delta.y, delta.z, radius.y, radius.z,
                                direction, self._position.y,
                                self._position.z, TABLE_SIZE_Y_MM,
                                TABLE_SIZE_Z_MM, STEPPER_PULSES_PER_MM_Y,
                                STEPPER_PULSES_PER_MM_Z)
        elif self._plane == PLANE_ZX:
            self.__check_circle(delta.z, delta.x, radius.z, radius.x,
                                direction, self._position.z,
                                self._position.x, TABLE_SIZE_Z_MM,
                                TABLE_SIZE_X_MM, STEPPER_PULSES_PER_MM_Z,
                                STEPPER_PULSES_PER_MM_X)
        radius = radius.round_to_nearest_pulse()
        logging.info("Moving circularly {} {} {} with radius {}"
                     " and velocity {}".format(self._plane, delta,
                                               direction, radius, velocity))
        gen = PulseGeneratorCircular(delta, radius, self._plane,
                                     direction, velocity)
        self.__check_velocity(gen.max_velocity())
        # do movements
        extruder_speed = self._get_extruder_speed(delta, velocity)
        self._start_extruder_move(delta.e, extruder_speed)
        hal.move(gen)
        # save position
        self._position = self._position + delta

    def safe_zero(self, x=True, y=True, z=True):
        """ Move head to zero position safely.
        :param x: boolean, move X axis to zero
        :param y: boolean, move Y axis to zero
        :param z: boolean, move Z axis to zero
        """
        if x and not y:
            self._move_linear(Coordinates(-self._position.x, 0, 0, 0),
                              MAX_VELOCITY_MM_PER_MIN_X)
        elif y and not x:
            self._move_linear(Coordinates(0, -self._position.y, 0, 0),
                              MAX_VELOCITY_MM_PER_MIN_X)
        elif x and y:
            d = Coordinates(-self._position.x, -self._position.y, 0, 0)
            self._move_linear(d, min(MAX_VELOCITY_MM_PER_MIN_X,
                                     MAX_VELOCITY_MM_PER_MIN_Y))
        if z:
            d = Coordinates(0, 0, -self._position.z, 0)
            self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z)

    def position(self):
        """ Return current machine position (after the latest command)
            Note that hal might still be moving motors and in this case
            function will block until motors stops.
            This function for tests only.
            :return current position.
        """
        hal.join()
        return self._position

    def plane(self):
        """ Return current plane for circular interpolation. This function for
            tests only.
            :return current plane.
        """
        return self._plane

    def _start_extruder_move(self, delta, speed):
        """
        Start moving the current extruder asynchronously

        Arguments:
            delta {float} -- the difference in position, in mm
            speed {float} -- the speed to move the extruder, in mm/minute
        """
        extruder = hal.get_extruder(self._extruder_id)
        pos = extruder.get_position()
        extruder.set_position(pos + delta, speed / 60.0)

    def _set_extruder(self, extruder_id):
        """
        Change extruders

        Arguments:
            extruder_id {int} -- the id of the extruder from 0 to NUM_EXTRUDERS
        """
        if extruder_id < 0 or extruder_id >= len(EXTRUDER_CONFIG):
            raise ValueError('invalid extruder id {}'.format(extruder_id))
        extruder = hal.get_extruder(extruder_id)
        extruder.join()
        self._position.e = extruder.get_position()
        self._extruder_id = extruder_id

    def do_command(self, gcode):
        """ Perform action.
        :param gcode: GCode object which represent one gcode line
        :return String if any answer require, None otherwise.
        """
        if gcode is None:
            return None
        answer = None
        logging.debug("got command " + str(gcode.params))
        # read command
        c = gcode.command()
        if c is None and gcode.has_coordinates():
            c = 'G1'
        # read parameters
        if self._absoluteCoordinates:
            coord = gcode.coordinates(self._position - self._local,
                                      self._convertCoordinates)
            coord = coord + self._local
            delta = coord - self._position
        else:
            delta = gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
                                      self._convertCoordinates)
            # coord = self._position + delta
        velocity = gcode.get('F', self._velocity)
        radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
                              self._convertCoordinates)
        # check parameters
        if velocity < MIN_VELOCITY_MM_PER_MIN:
            raise GMachineException("feed speed too low")
        # select command and run it
        if c == 'G0':  # rapid move
            vl = max(MAX_VELOCITY_MM_PER_MIN_X,
                     MAX_VELOCITY_MM_PER_MIN_Y,
                     MAX_VELOCITY_MM_PER_MIN_Z,
                     self._get_extruder_max_speed())
            l = delta.length()
            if l > 0:
                proportion = abs(delta) / l
                if proportion.x > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_X / proportion.x)
                    if v < vl:
                        vl = v
                if proportion.y > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_Y / proportion.y)
                    if v < vl:
                        vl = v
                if proportion.z > 0:
                    v = int(MAX_VELOCITY_MM_PER_MIN_Z / proportion.z)
                    if v < vl:
                        vl = v
                if proportion.e > 0:
                    v = int(self._get_extruder_max_speed() / proportion.e)
                    if v < vl:
                        vl = v
            self._move_linear(delta, vl)
        elif c == 'G1':  # linear interpolation
            self._move_linear(delta, velocity)
        elif c == 'G2':  # circular interpolation, clockwise
            self._move_circular(delta, radius, velocity, CW)
        elif c == 'G3':  # circular interpolation, counterclockwise
            self._move_circular(delta, radius, velocity, CCW)
        elif c == 'G4':  # delay in s
            if not gcode.has('P'):
                raise GMachineException("P is not specified")
            pause = gcode.get('P', 0)
            if pause < 0:
                raise GMachineException("bad delay")
            hal.join()
            time.sleep(pause)
        elif c == 'G17':  # XY plane select
            self._plane = PLANE_XY
        elif c == 'G18':  # ZX plane select
            self._plane = PLANE_ZX
        elif c == 'G19':  # YZ plane select
            self._plane = PLANE_YZ
        elif c == 'G20':  # switch to inches
            self._convertCoordinates = 25.4
        elif c == 'G21':  # switch to mm
            self._convertCoordinates = 1.0
        elif c == 'G28':  # home
            axises = gcode.has('X'), gcode.has('Y'), gcode.has('Z')
            if axises == (False, False, False):
                axises = True, True, True
            self.safe_zero(*axises)
            hal.join()
            if not hal.calibrate(*axises):
                raise GMachineException("failed to calibrate")
        elif c == 'G53':  # switch to machine coords
            raise GMachineException('Not supported')
            # TODO not sure what local/machine coordinates mean and how to handle this with multiple extruders
            # self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
        elif c == 'G90':  # switch to absolute coords
            self._absoluteCoordinates = True
        elif c == 'G91':  # switch to relative coords
            self._absoluteCoordinates = False
        elif c == 'G92':  # switch to local coords
            raise GMachineException('Not supported')
            # if gcode.has_coordinates():
            #     self._local = self._position - gcode.coordinates(
            #         Coordinates(self._position.x - self._local.x,
            #                     self._position.y - self._local.y,
            #                     self._position.z - self._local.z,
            #                     self._position.e - self._local.e),
            #         self._convertCoordinates)
            # else:
            #     self._local = self._position
        elif c == 'M2' or c == 'M30':  # program finish, reset everything.
            self.reset()
        elif c == 'M72':
            audio_id = int(gcode.get('P', 0))
            if audio_id not in AUDIO_FILES:
                raise GMachineException('Audio ID not recognized')
            audio_filepath = AUDIO_BASE_FILEPATH + AUDIO_FILES[audio_id]
            logging.info('playing audio from {}'.format(audio_filepath))
            AudioPlayer.play(audio_filepath)
        elif c == 'M84':  # disable motors
            hal.disable_steppers()
        elif c == 'M111':  # enable debug
            logging_config.debug_enable()
        elif c == 'M114':  # get current position
            hal.join()
            p = self.position()
            answer = "X:{} Y:{} Z:{} E:{}".format(p.x, p.y, p.z, p.e)
        elif c == 'T':  # select tool (extruder)
            self._set_extruder(int(gcode.get('T')))
        elif c is None:  # command not specified(ie just F was passed)
            pass
        # commands below are added just for compatibility
        elif c == 'M82':  # absolute mode for extruder
            if not self._absoluteCoordinates:
                raise GMachineException("Not supported, use G90/G91")
        elif c == 'M83':  # relative mode for extruder
            if self._absoluteCoordinates:
                raise GMachineException("Not supported, use G90/G91")
        else:
            raise GMachineException("unknown command")
        # save parameters on success
        self._velocity = velocity
        logging.debug("position {}".format(self._position))
        return answer
Exceptions while processing gcode line. Main object which control and keep state of whole machine: steppers,         spindle, extruder etc Initialization. # init variables Free all resources. Reinitialize all program configurable thing. # noinspection PyMethodMayBeStatic Returns:             Extruder -- the current extruder Returns:             float -- the max speed of the current extruder in mm/min Pull the E axis speed out of the delta position vector and overall speed about all axes.         This is also done in pulses.py, but must be repeated because extruders don't use pulses.          Arguments:             delta_mm {Coordinates} -- the difference in position in mm             velocity_mm_per_min {float} -- the speed about all axes in mm/min          Returns:             {float} -- the speed of the E axis only # save position Takes the coordinates a and b of a point relative to the center of the circle             and returns the quarter of the circle the point is in          Arguments:             a {float} -- The first coordinate             b {float} -- The second coordinate          Returns:             {int} -- The quarter the specified point exists in Validates the circle to be drawn, checking for a valid radius, a valid endpoint, and if the circle             is bounded within the table              The coordinates are labeled as a and b because the plane is selectable and a and b stand for arbitrary             combinations of x, y and z.          Arguments:             delta_a, delta_b {float} -- coordinates of the endpoint relative to the position             radius_a, radius_b {float} -- coordinates of the circle's center relative to the position             direction {RotationalDirection(Enum)} -- the direction the circle will draw             position_a, position_b {float} -- the current position (starting position) in absolute coordinates             table_a, table_b {int} -- the table's maximum for both the a and b axis             pulses_per_mm_a, pulses_per_mm_b {float} -- the pulses per mm for both axis'          Raises:             GMachineException -- raised if the radius is zero             GMachineException -- raised if the endpoint not on the defined circle             GMachineException -- raised if the circle would draw out of bounds # check if (delta_a, delta_b) is on the specified circle # check if the drawn circle is inside the table # If the endpoint and position are the same, check the full circle # If the start and end points are not in the same quarter, there will be new maximum values that have to be checked against # boundry conditions # get delta vector and put it on circle # do movements # save position Move head to zero position safely.         :param x: boolean, move X axis to zero         :param y: boolean, move Y axis to zero         :param z: boolean, move Z axis to zero Return current machine position (after the latest command)             Note that hal might still be moving motors and in this case             function will block until motors stops.             This function for tests only.             :return current position. Return current plane for circular interpolation. This function for             tests only.             :return current plane. Start moving the current extruder asynchronously          Arguments:             delta {float} -- the difference in position, in mm             speed {float} -- the speed to move the extruder, in mm/minute Change extruders          Arguments:             extruder_id {int} -- the id of the extruder from 0 to NUM_EXTRUDERS Perform action.         :param gcode: GCode object which represent one gcode line         :return String if any answer require, None otherwise. # read command # read parameters # coord = self._position + delta # check parameters # select command and run it # rapid move # linear interpolation # circular interpolation, clockwise # circular interpolation, counterclockwise # delay in s # XY plane select # ZX plane select # YZ plane select # switch to inches # switch to mm # home # switch to machine coords # TODO not sure what local/machine coordinates mean and how to handle this with multiple extruders # self._local = Coordinates(0.0, 0.0, 0.0, 0.0) # switch to absolute coords # switch to relative coords # switch to local coords # if gcode.has_coordinates(): #     self._local = self._position - gcode.coordinates( #         Coordinates(self._position.x - self._local.x, #                     self._position.y - self._local.y, #                     self._position.z - self._local.z, #                     self._position.e - self._local.e), #         self._convertCoordinates) # else: #     self._local = self._position # program finish, reset everything. # disable motors # enable debug # get current position # select tool (extruder) # command not specified(ie just F was passed) # commands below are added just for compatibility # absolute mode for extruder # relative mode for extruder # save parameters on success