def __init__(self, platform=None, top=False):
'''
platform -- pass test platform
top -- trigger synthesis of module
'''
self.top = top
self.platform = platform
self.divider = platform.clks[platform.hfosc_div]
self.order = platform.poldegree
self.bit_shift = bit_shift(platform)
self.motors = platform.motors
self.max_steps = int(MOVE_TICKS / 2) # Nyquist
# inputs
self.coeff = Array()
for _ in range(self.motors):
self.coeff.extend([
Signal(signed(self.bit_shift + 1)),
Signal(signed(self.bit_shift + 1)),
Signal(signed(self.bit_shift + 1))
][:self.order])
self.start = Signal()
self.ticklimit = Signal(MOVE_TICKS.bit_length())
# output
self.busy = Signal()
self.dir = Array(Signal() for _ in range(self.motors))
self.step = Array(Signal() for _ in range(self.motors))
class TestPlatform:
name = 'Test'
stepspermm = {'x': 400, 'y': 400}
clks = {0: 1} # dictionary to determine clock divider, e.g. movement.py
hfosc_div = 0 # selects clock speed on UP5K and clk divider
poldegree = 2 # degree of polynomal
laser_bits = 1
laser_axis = 'y'
laser_var = {
'RPM': 2000,
'FACETS': 4,
'SINGLE_LINE': False,
'TICKSINFACET': 20,
'BITSINSCANLINE': 3,
'LASERTICKS': 4,
'SINGLE_FACET': False,
'DIRECTION': 0
}
motors = len(stepspermm.keys())
steppers = [StepperRecord()] * motors
laserhead = LaserscannerRecord()
bldc = BLDCRecord()
leds = Array(Signal() for _ in range(3))
def __init__(self):
self.memdepth = wordsinmove(self) * 2 + 1
def __init__(self, platform, top=False):
"""
platform -- pass test platform
top -- True if top module
"""
self.platform = platform
self.top = top
self.enable_prism = Signal()
self.leds = Array(Signal() for _ in range(3))
self.hall = Array(Signal() for _ in range(3))
# powers bridges motor
self.uL = Signal()
self.uH = Signal()
self.vL = Signal()
self.vH = Signal()
self.wL = Signal()
self.wH = Signal()
def __init__(self, platform, top=False):
"""
platform -- pass test platform
top -- trigger synthesis of module
"""
self.platform = platform
self.top = top
self.spi = SPIBus()
self.position = Array(
Signal(signed(64)) for _ in range(platform.motors))
self.pinstate = Signal(8)
self.read_commit = Signal()
self.read_en = Signal()
self.read_discard = Signal()
self.dispatcherror = Signal()
self.parse = Signal()
self.read_data = Signal(MEMWIDTH)
self.empty = Signal()
def elaborate(self, platform):
m = Module()
# add 1 MHz clock domain
cntr = Signal(range(self.divider))
# pos
max_bits = (self.max_steps << self.bit_shift).bit_length()
cntrs = Array(
Signal(signed(max_bits + 1)) for _ in range(len(self.coeff)))
assert max_bits <= 64
ticks = Signal(MOVE_TICKS.bit_length())
if self.top:
steppers = [res for res in get_all_resources(platform, "stepper")]
assert len(steppers) != 0
for idx, stepper in enumerate(steppers):
m.d.comb += [
stepper.step.eq(self.step[idx]),
stepper.dir.eq(self.dir[idx])
]
else:
self.ticks = ticks
self.cntrs = cntrs
# steps
for motor in range(self.motors):
m.d.comb += self.step[motor].eq(cntrs[motor *
self.order][self.bit_shift])
# directions
counter_d = Array(
Signal(signed(max_bits + 1)) for _ in range(self.motors))
for motor in range(self.motors):
m.d.sync += counter_d[motor].eq(cntrs[motor * self.order])
# negative case --> decreasing
with m.If(counter_d[motor] > cntrs[motor * self.order]):
m.d.sync += self.dir[motor].eq(0)
# positive case --> increasing
with m.Elif(counter_d[motor] < cntrs[motor * self.order]):
m.d.sync += self.dir[motor].eq(1)
with m.FSM(reset='RESET', name='polynomen'):
with m.State('RESET'):
m.next = 'WAIT_START'
m.d.sync += self.busy.eq(0)
with m.State('WAIT_START'):
with m.If(self.start):
for motor in range(self.motors):
coef0 = motor * self.order
step_bit = self.bit_shift + 1
m.d.sync += [
cntrs[coef0].eq(cntrs[coef0][:step_bit]),
counter_d[motor].eq(counter_d[motor][:step_bit])
]
for degree in range(1, self.order):
m.d.sync += cntrs[coef0 + degree].eq(0)
m.d.sync += self.busy.eq(1)
m.next = 'RUNNING'
with m.Else():
m.d.sync += self.busy.eq(0)
with m.State('RUNNING'):
with m.If((ticks < self.ticklimit)
& (cntr >= self.divider - 1)):
m.d.sync += [ticks.eq(ticks + 1), cntr.eq(0)]
for motor in range(self.motors):
order = self.order
idx = motor * order
op3, op2, op1 = 0, 0, 0
if order > 2:
op3 += 3 * 2 * self.coeff[idx + 2] + cntrs[idx + 2]
op2 += cntrs[idx + 2]
op1 += self.coeff[idx + 2] + cntrs[idx + 2]
m.d.sync += cntrs[idx + 2].eq(op3)
if order > 1:
op2 += (2 * self.coeff[idx + 1] + cntrs[idx + 1])
m.d.sync += cntrs[idx + 1].eq(op2)
op1 += (self.coeff[idx + 1] + self.coeff[idx] +
cntrs[idx + 1] + cntrs[idx])
m.d.sync += cntrs[idx].eq(op1)
with m.Elif(ticks < self.ticklimit):
m.d.sync += cntr.eq(cntr + 1)
with m.Else():
m.d.sync += ticks.eq(0)
m.next = 'WAIT_START'
return m
class Polynomal(Elaboratable):
""" Sets motor states using a polynomal algorithm
A polynomal up to 3 order, i.e. c*t^3+b*t^2+a*t,
is evaluated under the assumption that t starts at 0
and has a maximum of say 10_000 ticks.
The polynomal describes the stepper position of a single axis.
A counter is used to capture the state of the polynomal.
If a given bit, denoted by bitshift, of the counter changes,
a step is sent to the motor.
In every tick the step can at most increase
with one count.
Non step part of base Counters are kept after segment.
Higher orders, velocity etc are removed.
This code requires a lot of LUT, only order 2 is supported on UP5k
It is assumed that the user can completely determine
the outcome of the calculation.
To ascertain step accuracy, c is submitted with a very high accuracy.
For third order, this requires 41 bit wide numbers
and is a "weakness" in the code.
The code might be sped up via Horner's method and the use of DSPs.
The current code does not require a DSP.
Assumptions:
max ticks per move is 10_000
update frequency motor is 1 MHz
I/O signals:
I: coeff -- polynomal coefficients
I: start -- start signal
O: busy -- busy signal
O: finished -- finished signal
O: total steps -- total steps executed in move
O: dir -- direction; 1 is postive and 0 is negative
O: step -- step signal
"""
def __init__(self, platform=None, top=False):
'''
platform -- pass test platform
top -- trigger synthesis of module
'''
self.top = top
self.platform = platform
self.divider = platform.clks[platform.hfosc_div]
self.order = platform.poldegree
self.bit_shift = bit_shift(platform)
self.motors = platform.motors
self.max_steps = int(MOVE_TICKS / 2) # Nyquist
# inputs
self.coeff = Array()
for _ in range(self.motors):
self.coeff.extend([
Signal(signed(self.bit_shift + 1)),
Signal(signed(self.bit_shift + 1)),
Signal(signed(self.bit_shift + 1))
][:self.order])
self.start = Signal()
self.ticklimit = Signal(MOVE_TICKS.bit_length())
# output
self.busy = Signal()
self.dir = Array(Signal() for _ in range(self.motors))
self.step = Array(Signal() for _ in range(self.motors))
def elaborate(self, platform):
m = Module()
# add 1 MHz clock domain
cntr = Signal(range(self.divider))
# pos
max_bits = (self.max_steps << self.bit_shift).bit_length()
cntrs = Array(
Signal(signed(max_bits + 1)) for _ in range(len(self.coeff)))
assert max_bits <= 64
ticks = Signal(MOVE_TICKS.bit_length())
if self.top:
steppers = [res for res in get_all_resources(platform, "stepper")]
assert len(steppers) != 0
for idx, stepper in enumerate(steppers):
m.d.comb += [
stepper.step.eq(self.step[idx]),
stepper.dir.eq(self.dir[idx])
]
else:
self.ticks = ticks
self.cntrs = cntrs
# steps
for motor in range(self.motors):
m.d.comb += self.step[motor].eq(cntrs[motor *
self.order][self.bit_shift])
# directions
counter_d = Array(
Signal(signed(max_bits + 1)) for _ in range(self.motors))
for motor in range(self.motors):
m.d.sync += counter_d[motor].eq(cntrs[motor * self.order])
# negative case --> decreasing
with m.If(counter_d[motor] > cntrs[motor * self.order]):
m.d.sync += self.dir[motor].eq(0)
# positive case --> increasing
with m.Elif(counter_d[motor] < cntrs[motor * self.order]):
m.d.sync += self.dir[motor].eq(1)
with m.FSM(reset='RESET', name='polynomen'):
with m.State('RESET'):
m.next = 'WAIT_START'
m.d.sync += self.busy.eq(0)
with m.State('WAIT_START'):
with m.If(self.start):
for motor in range(self.motors):
coef0 = motor * self.order
step_bit = self.bit_shift + 1
m.d.sync += [
cntrs[coef0].eq(cntrs[coef0][:step_bit]),
counter_d[motor].eq(counter_d[motor][:step_bit])
]
for degree in range(1, self.order):
m.d.sync += cntrs[coef0 + degree].eq(0)
m.d.sync += self.busy.eq(1)
m.next = 'RUNNING'
with m.Else():
m.d.sync += self.busy.eq(0)
with m.State('RUNNING'):
with m.If((ticks < self.ticklimit)
& (cntr >= self.divider - 1)):
m.d.sync += [ticks.eq(ticks + 1), cntr.eq(0)]
for motor in range(self.motors):
order = self.order
idx = motor * order
op3, op2, op1 = 0, 0, 0
if order > 2:
op3 += 3 * 2 * self.coeff[idx + 2] + cntrs[idx + 2]
op2 += cntrs[idx + 2]
op1 += self.coeff[idx + 2] + cntrs[idx + 2]
m.d.sync += cntrs[idx + 2].eq(op3)
if order > 1:
op2 += (2 * self.coeff[idx + 1] + cntrs[idx + 1])
m.d.sync += cntrs[idx + 1].eq(op2)
op1 += (self.coeff[idx + 1] + self.coeff[idx] +
cntrs[idx + 1] + cntrs[idx])
m.d.sync += cntrs[idx].eq(op1)
with m.Elif(ticks < self.ticklimit):
m.d.sync += cntr.eq(cntr + 1)
with m.Else():
m.d.sync += ticks.eq(0)
m.next = 'WAIT_START'
return m
def elaborate(self, platform):
m = Module()
# Parser
parser = SPIParser(self.platform)
m.submodules.parser = parser
# Busy used to detect move or scanline in action
# disabled "dispatching"
busy = Signal()
# Polynomal Move
polynomal = Polynomal(self.platform)
m.submodules.polynomal = polynomal
if platform:
board_spi = platform.request("debug_spi")
spi = synchronize(m, board_spi)
laserheadpins = platform.request("laserscanner")
steppers = [res for res in get_all_resources(platform, "stepper")]
bldc = platform.request("bldc")
leds = [res.o for res in get_all_resources(platform, "led")]
assert len(steppers) != 0
else:
platform = self.platform
self.spi = SPIBus()
self.parser = parser
self.pol = polynomal
spi = synchronize(m, self.spi)
self.laserheadpins = platform.laserhead
self.steppers = steppers = platform.steppers
self.busy = busy
laserheadpins = platform.laserhead
bldc = platform.bldc
leds = platform.leds
# Local laser signal clones
enable_prism = Signal()
lasers = Signal(2)
# Laserscan Head
if self.simdiode:
laserhead = DiodeSimulator(platform=platform, addfifo=False)
lh = laserhead
m.d.comb += [
lh.enable_prism_in.eq(enable_prism | lh.enable_prism),
lh.laser0in.eq(lasers[0]
| lh.lasers[0]),
laserhead.laser1in.eq(lasers[1]
| lh.lasers[1])
]
else:
laserhead = Laserhead(platform=platform)
m.d.comb += laserhead.photodiode.eq(laserheadpins.photodiode)
m.submodules.laserhead = laserhead
if platform.name == 'Test':
self.laserhead = laserhead
# polynomal iterates over count
coeffcnt = Signal(range(len(polynomal.coeff) + 1))
# Prism motor
prism_driver = Driver(platform)
m.submodules.prism_driver = prism_driver
# connect prism motor
for idx in range(len(leds)):
m.d.comb += leds[idx].eq(prism_driver.leds[idx])
m.d.comb += prism_driver.enable_prism.eq(enable_prism)
m.d.comb += [
bldc.uL.eq(prism_driver.uL),
bldc.uH.eq(prism_driver.uH),
bldc.vL.eq(prism_driver.vL),
bldc.vH.eq(prism_driver.vH),
bldc.wL.eq(prism_driver.wL),
bldc.wH.eq(prism_driver.wH)
]
m.d.comb += [
prism_driver.hall[0].eq(bldc.sensor0),
prism_driver.hall[1].eq(bldc.sensor1),
prism_driver.hall[2].eq(bldc.sensor2)
]
# connect laserhead
m.d.comb += [
# TODO: fix removal
# laserheadpins.pwm.eq(laserhead.pwm),
# laserheadpins.en.eq(laserhead.enable_prism | enable_prism),
laserheadpins.laser0.eq(laserhead.lasers[0] | lasers[0]),
laserheadpins.laser1.eq(laserhead.lasers[1] | lasers[1]),
]
# connect Parser
m.d.comb += [
self.read_data.eq(parser.read_data),
laserhead.read_data.eq(parser.read_data),
laserhead.empty.eq(parser.empty),
self.empty.eq(parser.empty),
parser.read_commit.eq(self.read_commit | laserhead.read_commit),
parser.read_en.eq(self.read_en | laserhead.read_en),
parser.read_discard.eq(self.read_discard | laserhead.read_discard)
]
# connect motors
for idx, stepper in enumerate(steppers):
step = (polynomal.step[idx] & ((stepper.limit == 0) | stepper.dir))
if idx != (list(platform.stepspermm.keys()).index(
platform.laser_axis)):
direction = polynomal.dir[idx]
m.d.comb += [
stepper.step.eq(step),
stepper.dir.eq(direction),
parser.pinstate[idx].eq(stepper.limit)
]
# connect the motor in which the laserhead moves to laser core
else:
m.d.comb += [
parser.pinstate[idx].eq(stepper.limit),
stepper.step.eq((step & (~laserhead.process_lines))
| (laserhead.step
& (laserhead.process_lines))),
stepper.dir.eq(
(polynomal.dir[idx] & (~laserhead.process_lines))
| (laserhead.dir & (laserhead.process_lines)))
]
m.d.comb += (parser.pinstate[len(steppers):].eq(
Cat(laserhead.photodiode_t, laserhead.synchronized)))
# update position
stepper_d = Array(Signal() for _ in range(len(steppers)))
for idx, stepper in enumerate(steppers):
pos = parser.position[idx]
m.d.sync += stepper_d[idx].eq(stepper.step)
with m.If(stepper.limit == 1):
m.d.sync += parser.position[idx].eq(0)
# assuming position is signed
# TODO: this might eat LUT, optimize
pos_max = pow(2, pos.width - 1) - 2
with m.Elif((pos > pos_max) | (pos < -pos_max)):
m.d.sync += parser.position[idx].eq(0)
with m.Elif((stepper.step == 1) & (stepper_d[idx] == 0)):
with m.If(stepper.dir):
m.d.sync += pos.eq(pos + 1)
with m.Else():
m.d.sync += pos.eq(pos - 1)
# Busy signal
m.d.comb += busy.eq(polynomal.busy | laserhead.process_lines)
# connect spi
m.d.comb += parser.spi.connect(spi)
# pins you can write to
pins = Cat(lasers, enable_prism, laserhead.synchronize)
with m.FSM(reset='RESET', name='dispatcher'):
with m.State('RESET'):
m.next = 'WAIT_INSTRUCTION'
m.d.sync += pins.eq(0)
with m.State('WAIT_INSTRUCTION'):
m.d.sync += [self.read_commit.eq(0), polynomal.start.eq(0)]
with m.If((self.empty == 0) & parser.parse & (busy == 0)):
m.d.sync += self.read_en.eq(1)
m.next = 'PARSEHEAD'
# check which instruction we r handling
with m.State('PARSEHEAD'):
byte0 = self.read_data[:8]
m.d.sync += self.read_en.eq(0)
with m.If(byte0 == INSTRUCTIONS.MOVE):
m.d.sync += [
polynomal.ticklimit.eq(self.read_data[8:]),
coeffcnt.eq(0)
]
m.next = 'MOVE_POLYNOMAL'
with m.Elif(byte0 == INSTRUCTIONS.WRITEPIN):
m.d.sync += [
pins.eq(self.read_data[8:]),
self.read_commit.eq(1)
]
m.next = 'WAIT'
with m.Elif((byte0 == INSTRUCTIONS.SCANLINE)
| (byte0 == INSTRUCTIONS.LASTSCANLINE)):
m.d.sync += [
self.read_discard.eq(1),
laserhead.synchronize.eq(1),
laserhead.expose_start.eq(1)
]
m.next = 'SCANLINE'
with m.Else():
m.next = 'ERROR'
m.d.sync += parser.dispatcherror.eq(1)
with m.State('MOVE_POLYNOMAL'):
with m.If(coeffcnt < len(polynomal.coeff)):
with m.If(self.read_en == 0):
m.d.sync += self.read_en.eq(1)
with m.Else():
m.d.sync += [
polynomal.coeff[coeffcnt].eq(self.read_data),
coeffcnt.eq(coeffcnt + 1),
self.read_en.eq(0)
]
with m.Else():
m.next = 'WAIT'
m.d.sync += [polynomal.start.eq(1), self.read_commit.eq(1)]
with m.State('SCANLINE'):
m.d.sync += [
self.read_discard.eq(0),
laserhead.expose_start.eq(0)
]
m.next = 'WAIT'
# NOTE: you need to wait for busy to be raised
# in time
with m.State('WAIT'):
m.d.sync += polynomal.start.eq(0)
m.next = 'WAIT_INSTRUCTION'
# NOTE: system never recovers user must reset
with m.State('ERROR'):
m.next = 'ERROR'
return m