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

refactoring

Browse Source
This commit is contained in:
Nikolay Khabarov
2017-06-12 00:52:11 +03:00
parent 0fe98f4cca
commit 98bf7e914e
23 changed files with 448 additions and 405 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cnc/hal_raspberry/hal.py
+11 -24
View File
@@ -1,23 +1,9 @@
import logging
import time
from cnc.hal_raspberry import rpgpio
from cnc.pulses import PulseGeneratorLinear
from cnc.coordinates import Coordinates
from cnc.pulses import *
from cnc.config import *
# Stepper motors channel for RPIO
STEPPER_CHANNEL = 0
# Since there is no way to add pulses and then start cycle in RPIO,
# use this delay to start adding pulses to cycle. It can be easily
# solved by modifying RPIO in a way of adding method to start cycle
# explicitly.
RPIO_START_DELAY_US = 200000
# Since RPIO generate cycles in loop, use this delay to stop RPIO
# It can be removed if RPIO would allow to run single shot cycle.
RPIO_STOP_DELAY_US = 5000000
US_IN_SECONDS = 1000000
gpio = rpgpio.GPIO()
@@ -29,8 +15,9 @@ STEP_PIN_MASK_Y = 1 << STEPPER_STEP_PIN_Y
STEP_PIN_MASK_Z = 1 << STEPPER_STEP_PIN_Z
STEP_PIN_MASK_E = 1 << STEPPER_STEP_PIN_E
def init():
""" Initialize GPIO pins and machine itself, including callibration if
""" Initialize GPIO pins and machine itself, including calibration if
needed. Do not return till all procedures are completed.
"""
gpio.init(STEPPER_STEP_PIN_X, rpgpio.GPIO.MODE_OUTPUT)
@@ -53,7 +40,7 @@ def init():
gpio.set(STEPPER_DIR_PIN_Z)
pins = STEP_PIN_MASK_X | STEP_PIN_MASK_Y | STEP_PIN_MASK_Z
dma.clear()
dma.add_pulse(pins, STEPPER_PULSE_LINGTH_US)
dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
st = time.time()
max_pulses_left = int(1.2 * max(STEPPER_PULSES_PER_MM_X,
STEPPER_PULSES_PER_MM_Y,
@@ -66,15 +53,15 @@ def init():
if (STEP_PIN_MASK_X & pins) != 0 and gpio.read(ENDSTOP_PIN_X) == 0:
pins &= ~STEP_PIN_MASK_X
dma.clear()
dma.add_pulse(pins, STEPPER_PULSE_LINGTH_US)
dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
if (STEP_PIN_MASK_Y & pins) != 0 and gpio.read(ENDSTOP_PIN_Y) == 0:
pins &= ~STEP_PIN_MASK_Y
dma.clear()
dma.add_pulse(pins, STEPPER_PULSE_LINGTH_US)
dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
if (STEP_PIN_MASK_Z & pins) != 0 and gpio.read(ENDSTOP_PIN_Z) == 0:
pins &= ~STEP_PIN_MASK_Z
dma.clear()
dma.add_pulse(pins, STEPPER_PULSE_LINGTH_US)
dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
if pins == 0:
break
dma.run(False)
@@ -122,8 +109,8 @@ def move(generator):
is_ran = False
instant = INSTANT_RUN
st = time.time()
for dir, tx, ty, tz, te in generator:
if dir: # set up directions
for direction, tx, ty, tz, te in generator:
if direction: # set up directions
pins_to_set = 0
pins_to_clear = 0
if tx > 0:
@@ -160,11 +147,11 @@ def move(generator):
pins |= STEP_PIN_MASK_E
if k - prev > 0:
dma.add_delay(k - prev)
dma.add_pulse(pins, STEPPER_PULSE_LINGTH_US)
dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
# TODO not a precise way! pulses will set in queue, instead of crossing
# if next pulse start during pulse length. Though it almost doesn't
# matter for pulses with 1-2us length.
prev = k + STEPPER_PULSE_LINGTH_US
prev = k + STEPPER_PULSE_LENGTH_US
# instant run handling
if not is_ran and instant:
if k > 500000: # wait at least 500 ms is uploaded
cnc/hal_raspberry/rpgpio.py
+65 -63
View File
@@ -20,7 +20,7 @@ class GPIO(object):
"""
self._mem = PhysicalMemory(PERI_BASE + GPIO_REGISTER_BASE)
def _pullupdn(self, pin, mode):
def _pull_up_dn(self, pin, mode):
p = self._mem.read_int(GPIO_PULLUPDN_OFFSET)
p &= ~3
if mode == self.MODE_INPUT_PULLUP:
@@ -28,49 +28,49 @@ class GPIO(object):
elif mode == self.MODE_INPUT_PULLDOWN:
p |= 1
self._mem.write_int(GPIO_PULLUPDN_OFFSET, p)
addr = 4 * int(pin / 32) + GPIO_PULLUPDNCLK_OFFSET
self._mem.write_int(addr, 1 << (pin % 32))
address = 4 * int(pin / 32) + GPIO_PULLUPDNCLK_OFFSET
self._mem.write_int(address, 1 << (pin % 32))
p = self._mem.read_int(GPIO_PULLUPDN_OFFSET)
p &= ~3
self._mem.write_int(GPIO_PULLUPDN_OFFSET, p)
self._mem.write_int(addr, 0)
self._mem.write_int(address, 0)
def init(self, pin, mode):
""" Initialize or re-initialize GPIO pin.
:param pin: pin number.
:param mode: one of MODE_* variables in this class.
"""
addr = 4 * int(pin / 10) + GPIO_FSEL_OFFSET
v = self._mem.read_int(addr)
address = 4 * int(pin / 10) + GPIO_FSEL_OFFSET
v = self._mem.read_int(address)
v &= ~(7 << ((pin % 10) * 3)) # input value
if mode == self.MODE_OUTPUT:
v |= (1 << ((pin % 10) * 3)) # output value, base on input
self._mem.write_int(addr, v)
self._mem.write_int(address, v)
else:
self._mem.write_int(addr, v)
self._pullupdn(pin, mode)
self._mem.write_int(address, v)
self._pull_up_dn(pin, mode)
def set(self, pin):
""" Set pin to HIGH state.
:param pin: pin number.
"""
addr = 4 * int(pin / 32) + GPIO_SET_OFFSET
self._mem.write_int(addr, 1 << (pin % 32))
address = 4 * int(pin / 32) + GPIO_SET_OFFSET
self._mem.write_int(address, 1 << (pin % 32))
def clear(self, pin):
""" Set pin to LOW state.
:param pin: pin number.
"""
addr = 4 * int(pin / 32) + GPIO_CLEAR_OFFSET
self._mem.write_int(addr, 1 << (pin % 32))
address = 4 * int(pin / 32) + GPIO_CLEAR_OFFSET
self._mem.write_int(address, 1 << (pin % 32))
def read(self, pin):
""" Read pin current value.
:param pin: pin number.
:return: integer value 0 or 1.
"""
addr = 4 * int(pin / 32) + GPIO_INPUT_OFFSET
v = self._mem.read_int(addr)
address = 4 * int(pin / 32) + GPIO_INPUT_OFFSET
v = self._mem.read_int(address)
v &= 1 << (pin % 32)
if v == 0:
return 0
@@ -102,20 +102,20 @@ class DMAGPIO(DMAProto):
self._clock = PhysicalMemory(PERI_BASE + CM_BASE)
# pre calculated variables for control blocks
self._delay_info = DMA_TI_NO_WIDE_BURSTS | DMA_SRC_IGNORE \
| DMA_TI_PER_MAP(DMA_TI_PER_MAP_PWM) \
| DMA_TI_DEST_DREQ
self._delay_info = (DMA_TI_NO_WIDE_BURSTS | DMA_SRC_IGNORE
| DMA_TI_PER_MAP(DMA_TI_PER_MAP_PWM)
| DMA_TI_DEST_DREQ)
self._delay_destination = PHYSICAL_PWM_BUS + PWM_FIFO
self._delay_stride = 0
self._pulse_info = DMA_TI_NO_WIDE_BURSTS | DMA_TI_TDMODE \
| DMA_TI_WAIT_RESP
self._pulse_info = (DMA_TI_NO_WIDE_BURSTS | DMA_TI_TDMODE
| DMA_TI_WAIT_RESP)
self._pulse_destination = PHYSICAL_GPIO_BUS + GPIO_SET_OFFSET
# YLENGTH is transfers count and XLENGTH size of each transfer
self._pulse_length = DMA_TI_TXFR_LEN_YLENGTH(2) \
| DMA_TI_TXFR_LEN_XLENGTH(4)
self._pulse_stride = DMA_TI_STRIDE_D_STRIDE(12) \
| DMA_TI_STRIDE_S_STRIDE(4)
self._pulse_length = (DMA_TI_TXFR_LEN_YLENGTH(2)
| DMA_TI_TXFR_LEN_XLENGTH(4))
self._pulse_stride = (DMA_TI_STRIDE_D_STRIDE(12)
| DMA_TI_STRIDE_S_STRIDE(4))
def add_pulse(self, pins_mask, length_us):
""" Add single pulse at the current position.
@@ -126,9 +126,9 @@ class DMAGPIO(DMAProto):
:param length_us: length in us.
"""
next_cb = self.__current_address + 3 * self._DMA_CONTROL_BLOCK_SIZE
if next_cb > self._physmem.get_size():
if next_cb > self._phys_memory.get_size():
raise MemoryError("Out of allocated memory.")
next3 = next_cb + self._physmem.get_bus_address()
next3 = next_cb + self._phys_memory.get_bus_address()
next2 = next3 - self._DMA_CONTROL_BLOCK_SIZE
next1 = next2 - self._DMA_CONTROL_BLOCK_SIZE
@@ -137,16 +137,17 @@ class DMAGPIO(DMAProto):
source3 = next3 - 8
data = (
# control block 1 - set
self._pulse_info, source1, self._pulse_destination,
self._pulse_length,
self._pulse_stride, next1, pins_mask, 0,
self._pulse_length, self._pulse_stride, next1, pins_mask, 0,
# control block 2 - delay
self._delay_info, 0, self._delay_destination, length2,
self._delay_stride, next2, 0, 0,
# control block 3 - clear
self._pulse_info, source3, self._pulse_destination,
self._pulse_length,
self._pulse_stride, next3, 0, pins_mask
self._pulse_length, self._pulse_stride, next3, 0, pins_mask
)
self._physmem.write(self.__current_address, "24I", data)
self._phys_memory.write(self.__current_address, "24I", data)
self.__current_address = next_cb
def add_delay(self, delay_us):
@@ -154,16 +155,16 @@ class DMAGPIO(DMAProto):
:param delay_us: delay in us.
"""
next_cb = self.__current_address + self._DMA_CONTROL_BLOCK_SIZE
if next_cb > self._physmem.get_size():
if next_cb > self._phys_memory.get_size():
raise MemoryError("Out of allocated memory.")
next1 = self._physmem.get_bus_address() + next_cb
next1 = self._phys_memory.get_bus_address() + next_cb
source = next1 - 8 # last 8 bytes are padding, use it to store data
length = delay_us << 4 # * 16
data = (
self._delay_info, source, self._delay_destination, length,
self._delay_stride, next1, 0, 0
)
self._physmem.write(self.__current_address, "8I", data)
self._phys_memory.write(self.__current_address, "8I", data)
self.__current_address = next_cb
def add_set_clear(self, pins_to_set, pins_to_clear):
@@ -172,23 +173,23 @@ class DMAGPIO(DMAProto):
: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():
if next_cb > self._phys_memory.get_size():
raise MemoryError("Out of allocated memory.")
next1 = self._physmem.get_bus_address() + next_cb
next1 = self._phys_memory.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._pulse_length, self._pulse_stride, next1,
pins_to_set, pins_to_clear
)
self._physmem.write(self.__current_address, "8I", data)
self._phys_memory.write(self.__current_address, "8I", data)
self.__current_address = next_cb
def finalize_stream(self):
""" Mark last added block as the last one.
"""
self._physmem.write_int(self.__current_address + 20
- self._DMA_CONTROL_BLOCK_SIZE, 0)
self._phys_memory.write_int(self.__current_address + 20
- self._DMA_CONTROL_BLOCK_SIZE, 0)
logging.info("DMA took {}MB of memory".
format(round(self.__current_address / 1024.0 / 1024.0, 2)))
@@ -201,9 +202,10 @@ class DMAGPIO(DMAProto):
self._clock.write_int(CM_PWM_CNTL, CM_PASSWORD | CM_SRC_PLLD) # disable
while (self._clock.read_int(CM_PWM_CNTL) & CM_CNTL_BUSY) != 0:
time.sleep(0.00001) # 10 us, wait until BUSY bit is clear
self._clock.write_int(CM_PWM_DIV, CM_PASSWORD | CM_DIV_VALUE(5)) # 100MHz
self._clock.write_int(CM_PWM_CNTL, CM_PASSWORD | CM_SRC_PLLD |
CM_CNTL_ENABLE)
self._clock.write_int(CM_PWM_DIV,
CM_PASSWORD | CM_DIV_VALUE(5)) # 100MHz
self._clock.write_int(CM_PWM_CNTL,
CM_PASSWORD | CM_SRC_PLLD | CM_CNTL_ENABLE)
self._pwm.write_int(PWM_RNG1, 100)
self._pwm.write_int(PWM_DMAC, PWM_DMAC_ENAB
@@ -220,9 +222,9 @@ class DMAGPIO(DMAProto):
raise RuntimeError("Nothing was added.")
# fix 'next' field in previous control block
if loop:
self._physmem.write_int(self.__current_address + 20
- self._DMA_CONTROL_BLOCK_SIZE,
self._physmem.get_bus_address())
self._phys_memory.write_int(self.__current_address + 20
- self._DMA_CONTROL_BLOCK_SIZE,
self._phys_memory.get_bus_address())
else:
self.finalize_stream()
self.run_stream()
@@ -267,17 +269,17 @@ class DMAPWM(DMAProto):
self.__add_control_block(i * self._DMA_CONTROL_BLOCK_SIZE,
GPIO_CLEAR_OFFSET)
# loop
self._physmem.write_int((self._TOTAL_NUMBER_OF_BLOCKS - 1)
* self._DMA_CONTROL_BLOCK_SIZE + 20,
self._physmem.get_bus_address())
self._phys_memory.write_int((self._TOTAL_NUMBER_OF_BLOCKS - 1)
* self._DMA_CONTROL_BLOCK_SIZE + 20,
self._phys_memory.get_bus_address())
self._gpio = PhysicalMemory(PERI_BASE + GPIO_REGISTER_BASE)
def __add_control_block(self, address, offset):
ba = self._physmem.get_bus_address() + address
ba = self._phys_memory.get_bus_address() + address
data = (
DMA_TI_NO_WIDE_BURSTS | DMA_TI_WAIT_RESP
| DMA_TI_DEST_INC | DMA_TI_SRC_INC, # info
ba + self._DMA_DATA_OFFSET, # source, last 8 bytes are padding, use it to store data
ba + self._DMA_DATA_OFFSET, # source, use padding for storing data
PHYSICAL_GPIO_BUS + offset, # destination
4, # length
0, # stride
@@ -285,7 +287,7 @@ class DMAPWM(DMAProto):
0, # padding, uses as data storage
0 # padding
)
self._physmem.write(address, "8I", data)
self._phys_memory.write(address, "8I", data)
def add_pin(self, pin, duty_cycle):
""" Add pin to PMW with specified duty cycle.
@@ -302,14 +304,14 @@ class DMAPWM(DMAProto):
self._gpio.write_int(GPIO_SET_OFFSET, 1 << pin)
self._clear_pins[pin] = self._DMA_DATA_OFFSET
else:
value = self._physmem.read_int(self._DMA_DATA_OFFSET)
value = self._phys_memory.read_int(self._DMA_DATA_OFFSET)
value |= 1 << pin
self._physmem.write_int(self._DMA_DATA_OFFSET, value)
clear_address = block_number * self._DMA_CONTROL_BLOCK_SIZE \
+ self._DMA_DATA_OFFSET
value = self._physmem.read_int(clear_address)
self._phys_memory.write_int(self._DMA_DATA_OFFSET, value)
clear_address = (block_number * self._DMA_CONTROL_BLOCK_SIZE
+ self._DMA_DATA_OFFSET)
value = self._phys_memory.read_int(clear_address)
value |= 1 << pin
self._physmem.write_int(clear_address, value)
self._phys_memory.write_int(clear_address, value)
self._clear_pins[pin] = clear_address
if not self.is_active():
super(DMAPWM, self)._run_dma()
@@ -321,12 +323,12 @@ class DMAPWM(DMAProto):
assert 0 <= pin < 32
if pin in self._clear_pins.keys():
address = self._clear_pins[pin]
value = self._physmem.read_int(address)
value = self._phys_memory.read_int(address)
value &= ~(1 << pin)
self._physmem.write_int(address, value)
value = self._physmem.read_int(self._DMA_DATA_OFFSET)
self._phys_memory.write_int(address, value)
value = self._phys_memory.read_int(self._DMA_DATA_OFFSET)
value &= ~(1 << pin)
self._physmem.write_int(self._DMA_DATA_OFFSET, value)
self._phys_memory.write_int(self._DMA_DATA_OFFSET, value)
del self._clear_pins[pin]
self._gpio.write_int(GPIO_CLEAR_OFFSET, 1 << pin)
if len(self._clear_pins) == 0 and self.is_active():
cnc/hal_raspberry/rpgpio_private.py
+28 -46
View File
@@ -57,32 +57,18 @@ DMA_CS_END = 1 << 1
DMA_CS_ACTIVE = 1 << 0
DMA_TI_PER_MAP_PWM = 5
DMA_TI_PER_MAP_PCM = 2
def DMA_TI_PER_MAP(x):
return x << 16
def DMA_TI_TXFR_LEN_YLENGTH(y):
return (y & 0x3fff) << 16
def DMA_TI_TXFR_LEN_XLENGTH(x):
return x & 0xffff
def DMA_TI_STRIDE_D_STRIDE(x):
return (x & 0xffff) << 16
def DMA_TI_STRIDE_S_STRIDE(x):
return x & 0xffff
def DMA_CS_PRIORITY(x):
return (x & 0xf) << 16
def DMA_CS_PANIC_PRIORITY(x):
return (x & 0xf) << 20
DMA_TI_PER_MAP = (lambda x: x << 16)
DMA_TI_TXFR_LEN_YLENGTH = (lambda y: (y & 0x3fff) << 16)
DMA_TI_TXFR_LEN_XLENGTH = (lambda x: x & 0xffff)
DMA_TI_STRIDE_D_STRIDE = (lambda x: (x & 0xffff) << 16)
DMA_TI_STRIDE_S_STRIDE = (lambda x: x & 0xffff)
DMA_CS_PRIORITY = (lambda x: (x & 0xf) << 16)
DMA_CS_PANIC_PRIORITY = (lambda x: (x & 0xf) << 20)
# hardware PWM controller registers
PWM_BASE = 0x0020C000
PHYSICAL_PWM_BUS = 0x7E000000 + PWM_BASE
PWM_CTL= 0x00
PWM_CTL = 0x00
PWM_DMAC = 0x08
PWM_RNG1 = 0x10
PWM_RNG2 = 0x20
@@ -95,12 +81,8 @@ PWM_CTL_CLRF = 1 << 6
PWM_CTL_USEF1 = 1 << 5
PWM_CTL_USEF2 = 1 << 13
PWM_DMAC_ENAB = 1 << 31
def PWM_DMAC_PANIC(x):
return x << 8
def PWM_DMAC_DREQ(x):
return x
PWM_DMAC_PANIC = (lambda x: x << 8)
PWM_DMAC_DREQ = (lambda x: x)
# clock manager module
CM_BASE = 0x00101000
@@ -113,14 +95,13 @@ CM_CNTL_ENABLE = 1 << 4
CM_CNTL_BUSY = 1 << 7
CM_SRC_OSC = 1 # 19.2 MHz
CM_SRC_PLLC = 5 # 1000 MHz
CM_SRC_PLLD = 6 # 500 MHz
CM_SRC_HDMI = 7 # 216 MHz
def CM_DIV_VALUE(x):
return x << 12
CM_SRC_PLLD = 6 # 500 MHz
CM_SRC_HDMI = 7 # 216 MHz
CM_DIV_VALUE = (lambda x: x << 12)
class PhysicalMemory(object):
# noinspection PyArgumentList,PyArgumentList
def __init__(self, phys_address, size=PAGE_SIZE):
""" Create object which maps physical memory to Python's mmap object.
:param phys_address: based address of physical memory
@@ -128,14 +109,14 @@ class PhysicalMemory(object):
self._size = size
phys_address -= phys_address % PAGE_SIZE
fd = self._open_dev("/dev/mem")
self._rmap = mmap.mmap(fd, size, flags=mmap.MAP_SHARED,
prot=mmap.PROT_READ | mmap.PROT_WRITE,
offset=phys_address)
self._memmap = mmap.mmap(fd, size, flags=mmap.MAP_SHARED,
prot=mmap.PROT_READ | mmap.PROT_WRITE,
offset=phys_address)
self._close_dev(fd)
atexit.register(self.cleanup)
def cleanup(self):
self._rmap.close()
self._memmap.close()
@staticmethod
def _open_dev(name):
@@ -149,13 +130,13 @@ class PhysicalMemory(object):
os.close(fd)
def write_int(self, address, int_value):
ctypes.c_uint32.from_buffer(self._rmap, address).value = int_value
ctypes.c_uint32.from_buffer(self._memmap, address).value = int_value
def write(self, address, fmt, data):
struct.pack_into(fmt, self._rmap, address, *data)
struct.pack_into(fmt, self._memmap, address, *data)
def read_int(self, address):
return ctypes.c_uint32.from_buffer(self._rmap, address).value
return ctypes.c_uint32.from_buffer(self._memmap, address).value
def get_size(self):
return self._size
@@ -177,8 +158,8 @@ class CMAPhysicalMemory(PhysicalMemory):
if self._handle == 0:
raise OSError("No memory to allocate with /dev/vcio")
# lock memory
self._busmem = self._send_data(0x3000d, [self._handle])
if self._busmem == 0:
self._bus_memory = self._send_data(0x3000d, [self._handle])
if self._bus_memory == 0:
# memory should be freed in __del__
raise OSError("Failed to lock memory with /dev/vcio")
# print("allocate {} at {} (bus {})".format(size,
@@ -206,10 +187,10 @@ class CMAPhysicalMemory(PhysicalMemory):
return data[5]
def get_bus_address(self):
return self._busmem
return self._bus_memory
def get_phys_address(self):
return self._busmem & ~0xc0000000
return self._bus_memory & ~0xc0000000
class DMAProto(object):
@@ -219,7 +200,7 @@ class DMAProto(object):
"""
self._DMA_CHANNEL = dma_channel
# allocate buffer for control blocks
self._physmem = CMAPhysicalMemory(memory_size)
self._phys_memory = CMAPhysicalMemory(memory_size)
# prepare dma registers memory map
self._dma = PhysicalMemory(PERI_BASE + DMA_BASE)
@@ -228,7 +209,8 @@ class DMAProto(object):
"""
address = 0x100 * self._DMA_CHANNEL
self._dma.write_int(address + DMA_CS, DMA_CS_END)
self._dma.write_int(address + DMA_CONBLK_AD, self._physmem.get_bus_address())
self._dma.write_int(address + DMA_CONBLK_AD,
self._phys_memory.get_bus_address())
cs = DMA_CS_PRIORITY(7) | DMA_CS_PANIC_PRIORITY(7) | DMA_CS_DISDEBUG
self._dma.write_int(address + DMA_CS, cs)
cs |= DMA_CS_ACTIVE