vincent/PyCNC
1
0
Fork 0
mirror of https://github.com/sinseman44/PyCNC.git synced 2026-07-16 08:37:09 +00:00
Code Issues Projects Releases Wiki Activity

add direction control in pulsegenerator

Browse Source
This commit is contained in:
Nikolay Khabarov
2017-05-28 02:25:19 +03:00
parent a87e2a379b
commit ff93f526a4
5 changed files with 145 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cnc/hal_raspberry/hal.py
+23 -20
View File
@@ -31,7 +31,7 @@ STEP_PIN_MASK_E = 1 << STEPPER_STEP_PIN_E
def init():
""" Initialize GPIO pins and machine itself, including callibration if
needed. Do not return till all procedure is completed.
needed. Do not return till all procedures are completed.
"""
gpio.init(STEPPER_STEP_PIN_X, rpgpio.GPIO.MODE_OUTPUT)
gpio.init(STEPPER_STEP_PIN_Y, rpgpio.GPIO.MODE_OUTPUT)
@@ -120,30 +120,33 @@ def move_linear(delta, velocity):
while dma.is_active():
time.sleep(0.001)
# set direction pins
if delta.x > 0.0:
gpio.clear(STEPPER_DIR_PIN_X)
else:
gpio.set(STEPPER_DIR_PIN_X)
if delta.y > 0.0:
gpio.clear(STEPPER_DIR_PIN_Y)
else:
gpio.set(STEPPER_DIR_PIN_Y)
if delta.z > 0.0:
gpio.clear(STEPPER_DIR_PIN_Z)
else:
gpio.set(STEPPER_DIR_PIN_Z)
if delta.e > 0.0:
gpio.clear(STEPPER_DIR_PIN_E)
else:
gpio.set(STEPPER_DIR_PIN_E)
# prepare and run dma
dma.clear()
prev = 0
is_ran = False
st = time.time()
for tx, ty, tz, te in generator:
for dir, tx, ty, tz, te in generator:
if dir: # set up directions
pins_to_set = 0
pins_to_clear = 0
if tx > 0:
pins_to_clear |= 1 << STEPPER_DIR_PIN_X
elif tx < 0:
pins_to_set |= 1 << STEPPER_DIR_PIN_X
if ty > 0:
pins_to_clear |= 1 << STEPPER_DIR_PIN_Y
elif ty < 0:
pins_to_set |= 1 << STEPPER_DIR_PIN_Y
if tz > 0:
pins_to_clear |= 1 << STEPPER_DIR_PIN_Z
elif tz < 0:
pins_to_set |= 1 << STEPPER_DIR_PIN_Z
if te > 0:
pins_to_clear |= 1 << STEPPER_DIR_PIN_E
elif te < 0:
pins_to_set |= 1 << STEPPER_DIR_PIN_E
dma.add_set_clear(pins_to_set, pins_to_clear)
continue
pins = 0
k = int(round(min(x for x in (tx, ty, tz, te) if x is not None)
* US_IN_SECONDS))
cnc/hal_raspberry/rpgpio.py
+18
View File
@@ -166,6 +166,24 @@ class DMAGPIO(DMAProto):
self._physmem.write(self.__current_address, "8I", data)
self.__current_address = next_cb
def add_set_clear(self, pins_to_set, pins_to_clear):
""" Change state of gpio pins.
:param pins_to_set: bitwise mask which pins should be set.
:param pins_to_clear: bitwise mask which pins should be clear.
"""
next_cb = self.__current_address + self._DMA_CONTROL_BLOCK_SIZE
if next_cb > self._physmem.get_size():
raise MemoryError("Out of allocated memory.")
next1 = self._physmem.get_bus_address() + next_cb
source = next1 - 8 # last 8 bytes are padding, use it to store data
data = (
self._pulse_info, source, self._pulse_destination,
self._pulse_length,
self._pulse_stride, next1, pins_to_set, pins_to_clear
)
self._physmem.write(self.__current_address, "8I", data)
self.__current_address = next_cb
def finalize_stream(self):
""" Mark last added block as the last one.
"""
cnc/hal_virtual.py
+14 -1
View File
@@ -37,7 +37,19 @@ def move_linear(delta, velocity):
dx, dy, dz, de = 0, 0, 0, 0
mx, my, mz, me = 0, 0, 0, 0
st = time.time()
for tx, ty, tz, te in generator:
direction_found = False
for dir, tx, ty, tz, te in generator:
if dir:
direction_found = True
assert (tx < 0 and delta.x < 0) or (tx > 0 and delta.x > 0) \
or delta.x == 0
assert (ty < 0 and delta.y < 0) or (ty > 0 and delta.y > 0) \
or delta.y == 0
assert (tz < 0 and delta.z < 0) or (tz > 0 and delta.z > 0) \
or delta.z == 0
assert (te < 0 and delta.e < 0) or (te > 0 and delta.e > 0) \
or delta.e == 0
continue
if tx is not None:
if tx > mx:
mx = tx
@@ -87,6 +99,7 @@ def move_linear(delta, velocity):
f = list(x for x in (tx, ty, tz, te) if x is not None)
assert f.count(f[0]) == len(f), "fast forwarded pulse detected"
pt = time.time()
assert direction_found, "direction not found"
assert ix / STEPPER_PULSES_PER_MM_X == abs(delta.x), "x wrong number of pulses"
assert iy / STEPPER_PULSES_PER_MM_Y == abs(delta.y), "y wrong number of pulses"
assert iz / STEPPER_PULSES_PER_MM_Z == abs(delta.z), "z wrong number of pulses"
cnc/pulses.py
+50 -25
View File
@@ -42,6 +42,7 @@ class PulseGenerator(object):
self._iteration_y = 0
self._iteration_z = 0
self._iteration_e = 0
self._iteration_direction = None
self._acceleration_time_s = 0.0
self._linear_time_s = 0.0
self._2Vmax_per_a = 0.0
@@ -54,10 +55,11 @@ class PulseGenerator(object):
during movement
linear_time_s: time for uniform movement, it is total movement
time minus acceleration and braking time
max_axis_velocity_mm_per_sec: maximum velocity of any of axis during
movement. Even if whole movement is
accelerated, this value should be
calculated as top velocity.
max_axis_velocity_mm_per_sec: maximum velocity of any of axis
during movement. Even if whole
movement is accelerated, this
value should be calculated as top
velocity.
"""
raise NotImplemented
@@ -67,10 +69,13 @@ class PulseGenerator(object):
must be expressed in terms of position, i.e. t = S / V for linear,
where S - distance would be increment on motor minimum step.
:param ix: number of pulse for X axis.
:param iy: number of pulse for X axis.
:param iz: number of pulse for X axis.
:param ie: number of pulse for X axis.
:return: time for each axis or None if movement for axis is finished.
:param iy: number of pulse for Y axis.
:param iz: number of pulse for Z axis.
:param ie: number of pulse for E axis.
:return: Two tuples. First is tuple is directions for each axis,
positive means forward, negative means reverse. Second is
tuple of times for each axis in us or None if movement for
axis is finished.
"""
raise NotImplemented
@@ -87,6 +92,7 @@ class PulseGenerator(object):
self._iteration_y = 0
self._iteration_z = 0
self._iteration_e = 0
self._iteration_direction = None
logging.debug(', '.join("%s: %s" % i for i in vars(self).items()))
return self
@@ -116,7 +122,8 @@ class PulseGenerator(object):
# V on start braking is Vlinear = Taccel * a = Tbreaking * a
# Vmax * Tpseudo = Tbreaking * a * t - a * t^2 / 2
return 2.0 * self._acceleration_time_s + self._linear_time_s \
- math.sqrt(self._acceleration_time_s ** 2 - self._2Vmax_per_a * bt)
- math.sqrt(self._acceleration_time_s ** 2
- self._2Vmax_per_a * bt)
def __next__(self):
# for python3
@@ -124,14 +131,26 @@ class PulseGenerator(object):
def next(self):
""" Iterate pulses.
:return: Tuple of three values for each axis which represent time for
the next pulse. If there is no pulses left None will be
returned.
:return: Tuple of five values:
- first is boolean value, if it is True, motors direction
should be changed and next pulse should performed in
this direction.
- values for all machine axises. For direction update,
positive values means forward movement, negative value
means reverse movement. For normal pulse, values are
represent time for the next pulse in microseconds.
This iteration strictly guarantees that next pulses time will
not be earlier in time then current. If there is no pulses
left StopIteration will be raised.
"""
tx, ty, tz, te = self._interpolation_function(self._iteration_x,
self._iteration_y,
self._iteration_z,
self._iteration_e)
dir, (tx, ty, tz, te) = self._interpolation_function(self._iteration_x,
self._iteration_y,
self._iteration_z,
self._iteration_e)
# check if direction update:
if dir != self._iteration_direction:
self._iteration_direction = dir
return (True,) + dir
# check condition to stop
if tx is None and ty is None and tz is None and te is None:
raise StopIteration
@@ -165,7 +184,7 @@ class PulseGenerator(object):
te = am
self._iteration_e += 1
return tx, ty, tz, te
return False, tx, ty, tz, te
def total_time_s(self):
""" Get total time for movement.
@@ -190,23 +209,29 @@ class PulseGeneratorLinear(PulseGenerator):
velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_mm)
# acceleration time
self.acceleration_time_s = self.max_velocity_mm_per_sec.find_max() \
/ STEPPER_MAX_ACCELERATION_MM_PER_S2
/ STEPPER_MAX_ACCELERATION_MM_PER_S2
# check if there is enough space to accelerate and brake, adjust time
# S = a * t^2 / 2
if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \
> distance_total_mm:
self.acceleration_time_s = math.sqrt(distance_total_mm /
STEPPER_MAX_ACCELERATION_MM_PER_S2)
STEPPER_MAX_ACCELERATION_MM_PER_S2)
self.linear_time_s = 0.0
# V = a * t -> V = 2 * S / t, take half of total distance for acceleration and braking
self.max_velocity_mm_per_sec = self._distance_mm / self.acceleration_time_s
# V = a * t -> V = 2 * S / t, take half of total distance for
# acceleration and braking
self.max_velocity_mm_per_sec = self._distance_mm \
/ self.acceleration_time_s
else:
# calculate linear time
linear_distance_mm = distance_total_mm\
- self.acceleration_time_s ** 2 \
* STEPPER_MAX_ACCELERATION_MM_PER_S2
linear_distance_mm = distance_total_mm \
- self.acceleration_time_s ** 2 \
* STEPPER_MAX_ACCELERATION_MM_PER_S2
self.linear_time_s = linear_distance_mm \
/ self.max_velocity_mm_per_sec.length()
self._direction = math.copysign(1, delta_mm.x), \
math.copysign(1, delta_mm.y), \
math.copysign(1, delta_mm.z), \
math.copysign(1, delta_mm.e) \
def _get_movement_parameters(self):
""" Return movement parameters, see super class for details.
@@ -236,4 +261,4 @@ class PulseGeneratorLinear(PulseGenerator):
self.max_velocity_mm_per_sec.z)
t_e = self.__linear(ie / STEPPER_PULSES_PER_MM_E, self._distance_mm.e,
self.max_velocity_mm_per_sec.e)
return t_x, t_y, t_z, t_e
return self._direction, (t_x, t_y, t_z, t_e)
tests/test_pulses.py
+40 -6
View File
@@ -26,7 +26,9 @@ class TestPulses(unittest.TestCase):
0, 0, 0),
self.v)
i = 0
for px, py, pz, pe in g:
for dir, px, py, pz, pe in g:
if dir:
continue
i += 1
self.assertEqual(px, 0)
self.assertEqual(py, None)
@@ -40,7 +42,9 @@ class TestPulses(unittest.TestCase):
1.0 / STEPPER_PULSES_PER_MM_E),
self.v)
i = 0
for px, py, pz, pe in g:
for dir, px, py, pz, pe in g:
if dir:
continue
i += 1
self.assertEqual(px, 0)
self.assertEqual(py, 0)
@@ -65,7 +69,9 @@ class TestPulses(unittest.TestCase):
m = Coordinates(2, 4, 0, 0)
g = PulseGeneratorLinear(m, self.v)
i = 0
for px, py, pz, pe in g:
for dir, px, py, pz, pe in g:
if dir:
continue
if i % 2 == 0:
self.assertNotEqual(px, None)
else:
@@ -86,7 +92,9 @@ class TestPulses(unittest.TestCase):
iz = 0
ie = 0
t = -1
for px, py, pz, pe in g:
for dir, px, py, pz, pe in g:
if dir:
continue
if px is not None:
ix += 1
if py is not None:
@@ -112,7 +120,9 @@ class TestPulses(unittest.TestCase):
g = PulseGeneratorLinear(m, self.v)
i = 0
lx = 0
for px, py, pz, pe in g:
for dir, px, py, pz, pe in g:
if dir:
continue
if i == 2:
at = px - lx
if i == TABLE_SIZE_X_MM * STEPPER_PULSES_PER_MM_X / 2:
@@ -124,5 +134,29 @@ class TestPulses(unittest.TestCase):
self.assertGreater(at, lt)
self.assertGreater(bt, lt)
def test_directions(self):
# Check if directions are set up correctly.
m = Coordinates(1, -2, 3, -4)
g = PulseGeneratorLinear(m, self.v)
dir_found = False
for dir, px, py, pz, pe in g:
if dir:
# should be once
self.assertFalse(dir_found)
dir_found = True
# check dirs
self.assertTrue(px > 0 and py < 0 and pz > 0 and pe < 0)
m = Coordinates(-1, 2, -3, 4)
g = PulseGeneratorLinear(m, self.v)
dir_found = False
for dir, px, py, pz, pe in g:
if dir:
# should be once
self.assertFalse(dir_found)
dir_found = True
# check dirs
self.assertTrue(px < 0 and py > 0 and pz < 0 and pe > 0)
if __name__ == '__main__':
unittest.main()
unittest.main()