vincent/PyCNC
1
0
Fork 0
mirror of https://github.com/sinseman44/PyCNC.git synced 2026-04-20 18:48:11 +00:00
Code Issues Projects Releases Wiki Activity

parser for circular interpolation

Browse Source
This commit is contained in:
Nikolay Khabarov
2017-05-22 04:01:45 +03:00
parent d77d8e0b20
commit 0de4f3dc00
9 changed files with 247 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
cnc/enums.py
View File

@@ -2,16 +2,40 @@
"""
class Plane(object):
class Enum(object):
""" Base class for enums
"""
__global_increment = 1
def __init__(self, for_str):
""" Initialize base class for enumerates.
:param for_str: return value for build in str() function
"""
self.value = Enum.__global_increment
self._str = for_str
Enum.__global_increment += 1
def __eq__(self, other):
return self.value == other.value
def __str__(self):
return self._str
class Plane(Enum):
""" Enum for choosing plane for circular interpolation.
"""
PLANE_XY = 1
PLANE_ZX = 2
PLANE_YZ = 3
pass
PLANE_XY = Plane("XY")
PLANE_ZX = Plane("ZX")
PLANE_YZ = Plane("YZ")
class RotationDirection(object):
class RotationDirection(Enum):
""" Enum for choosing rotation direction.
"""
CW = 1
CCW = 2
pass
CW = RotationDirection("CW")
CCW = RotationDirection("CCW")

135
cnc/gmachine.py
View File

@@ -1,9 +1,11 @@
from __future__ import division
import time
import logging
import math
from cnc import hal
from cnc.coordinates import Coordinates
from cnc.enums import *
from cnc.config import *
@@ -28,6 +30,8 @@ class GMachine(object):
self._local = None
self._convertCoordinates = 0
self._absoluteCoordinates = 0
self._plane = None
self._radius = None
self.reset()
hal.init()
@@ -47,40 +51,139 @@ class GMachine(object):
self._local = Coordinates(0.0, 0.0, 0.0)
self._convertCoordinates = 1.0
self._absoluteCoordinates = True
self._plane = PLANE_XY
self._radius = Coordinates(0.0, 0.0, 0.0)
def _spindle(self, spindle_speed):
hal.join()
hal.spindle_control(100.0 * spindle_speed / SPINDLE_MAX_RPM)
def _move(self, delta, velocity):
def __check_delta(self, delta):
pos = self._position + delta
if not pos.is_in_aabb(Coordinates(0.0, 0.0, 0.0),
Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)):
raise GMachineException("out of effective area")
def _move_linear(self, delta, velocity):
delta = delta.round(1.0 / STEPPER_PULSES_PER_MM)
if delta.is_zero():
return
np = self._position + delta
if not np.is_in_aabb(Coordinates(0.0, 0.0, 0.0),
Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)):
raise GMachineException("out of effective area")
self.__check_delta(delta)
hal.move_linear(delta, velocity)
# save position
self._position = np
self._position = self._position + delta
def __quarter(self, pa, pb):
if pa >= 0 and pb >= 0:
return 1
if pa < 0 and pb >= 0:
return 2
if pa < 0 and pb < 0:
return 3
if pa >= 0 and pb < 0:
return 4
def __adjust_circle(self, da, db, ra, rb, dir, pa, pb, ma, mb):
r = math.sqrt(ra * ra + rb * rb)
if r == 0:
raise GMachineException("circle radius is zero")
l = math.sqrt(da * da + db * db)
sq = self.__quarter(-ra, -rb)
if l == 0: # full circle
ea = da
eb = db
eq = 5 # mark as non-existing to check all
else:
ea = da / l * r + ra
eb = db / l * r + rb
eq = self.__quarter(ea - ra, eb - rb)
# iterate coordinates quarters and check if we fit table
q = sq
pq = q
for _ in range(0, 4):
if dir == CW:
q -= 1
else:
q += 1
if q <= 0:
q = 4
elif q >= 5:
q = 1
if q == eq:
break
is_raise = False
if (pq == 1 and q == 4) or (pq == 4 and q == 1):
is_raise = (pa + ra + r > ma)
elif (pq == 1 and q == 2) or (pq == 2 and q == 1):
is_raise = (pb + rb + r > mb)
elif (pq == 2 and q == 3) or (pq == 3 and q == 2):
is_raise = (pa + ra - r < 0)
elif (pq == 3 and q == 4) or (pq == 4 and q == 3):
is_raise = (pb + rb - r < 0)
if is_raise:
raise GMachineException("out of effective area")
pq = q
return ea, eb
def _circular(self, delta, radius, velocity, direction):
delta = delta.round(1.0 / STEPPER_PULSES_PER_MM)
self.__check_delta(delta)
# get delta vector and put it on circle
circle_end = Coordinates(0,0,0)
if self._plane == PLANE_XY:
circle_end.x, circle_end.y = self.__adjust_circle(delta.x, delta.y,
radius.x, radius.y, direction,
self._position.x, self._position.y,
TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM)
circle_end.z = delta.z
elif self._plane == PLANE_YZ:
circle_end.y, circle_end.z = self.__adjust_circle(delta.y, delta.z,
radius.y, radius.z, direction,
self._position.y, self._position.z,
TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)
circle_end.x = delta.x
elif self._plane == PLANE_ZX:
circle_end.z, circle_end.x = self.__adjust_circle(delta.z, delta.x,
radius.z, radius.x, direction,
self._position.z, self._position.x,
TABLE_SIZE_Z_MM, TABLE_SIZE_X_MM)
circle_end.y = delta.y
circle_end = circle_end.round(1.0 / STEPPER_PULSES_PER_MM)
hal.move_circular(circle_end, radius, self._plane, velocity, direction)
# if finish coords is not on circle, move some distance linearly
linear_delta = delta - circle_end
if not linear_delta.is_zero():
logging.info("Moving additionally {} to finish circle command".
format(linear_delta))
hal.move_linear(linear_delta, velocity)
# save position
self._position = self._position + circle_end + linear_delta
def home(self):
""" Move head to park position
"""
d = Coordinates(0, 0, -self._position.z)
self._move(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
d = Coordinates(-self._position.x, -self._position.y, 0)
self._move(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
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 do_command(self, gcode):
""" Perform action.
:param gcode: GCode object which represent one gcode line
@@ -103,6 +206,7 @@ class GMachine(object):
velocity = gcode.get('F', self._velocity)
spindle_rpm = gcode.get('S', self._spindle_rpm)
pause = gcode.get('P', self._pause)
radius = gcode.radius(self._radius, self._convertCoordinates)
# check parameters
if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN:
raise GMachineException("bad feed speed")
@@ -112,12 +216,22 @@ class GMachine(object):
raise GMachineException("bad delay")
# select command and run it
if c == 'G0': # rapid move
self._move(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
self._move_linear(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
elif c == 'G1': # linear interpolation
self._move(delta, velocity)
self._move_linear(delta, velocity)
elif c == 'G2': # circular interpolation, clockwise
self._circular(delta, radius, velocity, CW)
elif c == 'G3': # circular interpolation, counterclockwise
self._circular(delta, radius, velocity, CCW)
elif c == 'G4': # delay in s
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
@@ -150,5 +264,6 @@ class GMachine(object):
self._velocity = velocity
self._spindle_rpm = spindle_rpm
self._pause = pause
self._radius = radius
logging.debug("position {}, {}, {}".format(
self._position.x, self._position.y, self._position.z))

15
cnc/hal.py
View File

@@ -25,11 +25,24 @@
# do_something()
#
#
# def move_circular(delta, radius, plane, velocity, direction):
# """ Move with circular interpolation.
# :param delta: finish position delta from the beginning, must be on
# circle on specified plane. Zero means full circle.
# :param radius: vector to center of circle.
# :param plane: plane to interpolate.
# :param velocity: velocity in mm per min.
# :param direction: clockwise or counterclockwise.
# """
# do_something()
#
#
# def join():
# """ Wait till motors work.
# """
# do_something()
#
#
# def deinit():
# """ De-initialise hal, stop any hardware.
# """
@@ -51,6 +64,8 @@ if 'spindle_control' not in locals():
raise NotImplementedError("hal.spindle_control() not implemented")
if 'move_linear' not in locals():
raise NotImplementedError("hal.move_linear() not implemented")
if 'move_circular' not in locals():
raise NotImplementedError("hal.move_circular() not implemented")
if 'join' not in locals():
raise NotImplementedError("hal.join() not implemented")
if 'deinit' not in locals():

18
cnc/hal_raspberry/hal.py
View File

@@ -90,7 +90,7 @@ def init():
def spindle_control(percent):
""" Spindle control implementation.
:param percent: Spindle speed in percent. If 0, stop the spindle.
:param percent: spindle speed in percent. If 0, stop the spindle.
"""
logging.info("spindle control: {}%".format(percent))
if percent > 0:
@@ -101,7 +101,7 @@ def spindle_control(percent):
def move_linear(delta, velocity):
""" Move head to specified position
:param delta: Coordinated object, delta position in mm
:param delta: coordinated object, delta position in mm
:param velocity: velocity in mm per min
"""
logging.info("move {} with velocity {}".format(delta, velocity))
@@ -166,6 +166,20 @@ def move_linear(delta, velocity):
+ str(round(generator.total_time_s(), 2)) + "s")
def move_circular(delta, radius, plane, velocity, direction):
""" Move with circular interpolation.
:param delta: finish position delta from the beginning, must be on
circle on specified plane. Zero means full circle.
:param radius: vector to center of circle.
:param plane: plane to interpolate.
:param velocity: velocity in mm per min.
:param direction: clockwise or counterclockwise.
"""
logging.info("TODO move_circular {} {} {} with radius {} and velocity {}".
format(plane, delta, direction, radius, velocity))
# TODO
def join():
""" Wait till motors work.
"""

14
cnc/hal_virtual.py
View File

@@ -85,6 +85,20 @@ def move_linear(delta, velocity):
+ "s, estimated " + str(round(generator.total_time_s(), 2)) + "s")
def move_circular(delta, radius, plane, velocity, direction):
""" Move with circular interpolation.
:param delta: finish position delta from the beginning, must be on
circle on specified plane. Zero means full circle.
:param radius: vector to center of circle.
:param plane: plane to interpolate.
:param velocity: velocity in mm per min.
:param direction: clockwise or counterclockwise.
"""
logging.info("TODO move_circular {} {} {} with radius {} and velocity {}".
format(plane, delta, direction, radius, velocity))
# TODO
def join():
""" Wait till motors work.
"""