def __eq__(self, other):
if other == None:
return False
for dim in range(5):
if not circaf(self[dim], other[dim], 0.000001):
return False
return True
def feedrate3(self):
f = self.len3()
if circaf(f, 0, 0.000001):
print "emove"
assert(0)
return f
def g0(self, line, values):
# print "g0", values
feedrate = self.feedrate
if 'F' in values:
feedrate = values['F']
if not ("X" in values or "Y" in values or "Z" in values or "A" in values or "B" in values):
# Nothing to move, F-Only gcode or invalid
assert("F" in values)
self.feedrate = feedrate
return
curGcodePos = self.getGcodePos()
curRealPos = self.getRealPos()
newGcodePos = curGcodePos.copy()
newRealPos = curRealPos.copy()
# print "curGcodePos: ", newGcodePos
# print "curRealPos: ", newRealPos
displacement_vector = VVector()
displacement_vector_steps = [0, 0, 0, 0, 0]
eOnlyByGcode = True
for dim in range(5):
dimC = dimNames[dim]
if dimC not in values:
continue
gDiff = values[dimC] - curGcodePos[dim]
if gDiff and dimC in ['X', 'Y', 'Z']:
eOnlyByGcode = False
rDiff = values[dimC] - curRealPos[dim]
nSteps = (int)((rDiff * self.steps_per_mm[dim]) + 0.5)
displacement_vector_steps[dim] = nSteps
# debug
# delta = nSteps * self.mm_per_step[dim] - rDiff
# print "values: ", dimC, rDiff, nSteps, delta
# end debug
rDiff = nSteps * self.mm_per_step[dim]
displacement_vector[dim] = rDiff
newGcodePos[dimC] = values[dimC]
newRealPos[dim] = curRealPos[dim] + rDiff
#
# Check if zero or small length:
#
if displacement_vector_steps == [0, 0, 0, 0, 0]:
# Skip this very small move, the delta of this move is not lost,
# since it is included in the next absolute gcode command.
# Current position has not been updated, yet (see self.state.set_position(values))
# at the end of this method.
# But respect a possible feedrate change:
if not eOnlyByGcode:
self.feedrate = feedrate
return
# Get head move distance:
move_distance = vectorDistance(curRealPos[:3], newRealPos[:3])
if move_distance != 0:
assert(not eOnlyByGcode)
if debugMoves:
for dim in range(5):
assert(circaf(newRealPos[dim] - curRealPos[dim], displacement_vector[dim], 0.000001))
feedrateVector = displacement_vector._setLength(feedrate).constrain(self.maxFeedrateVector)
if feedrateVector:
feedrate = feedrateVector.len5()
# print "pos:", stepped_point, "[steps]"
# print "displacement_vector:", displacement_vector, "[mm]"
# print "feedrate:", feedrate, "[mm/s]"
self.planner.addMove(Move(
line,
# stepped_point=stepped_point,
displacement_vector=displacement_vector,
displacement_vector_steps=displacement_vector_steps,
feedrate=feedrate, # mm/s
))
if not eOnlyByGcode:
self.feedrate = feedrate
# else:
# print "NOT storing new feedrate of E-Only move '%s'!" % comment
# print "newGcodePos: ", newGcodePos
# print "newRealPos: ", newRealPos
self.setGcodePos(newGcodePos)
self.setRealPos(newRealPos)
def planSteps(self, move):
if debugMoves:
print "***** Start planSteps() *****"
move.pprint("PlanSTeps:")
move.state = 3
dirBits = 0
abs_displacement_vector_steps = []
leadAxis = 0
leadAxis_value = 0
for i in range(5):
dirBits += (move.displacement_vector_steps_raw()[i] >= 0) << i
adjustedDisplacement = move.displacement_vector_steps_adjusted(NozzleProfile, MatProfile, PrinterProfile)
s = abs(adjustedDisplacement[i])
if s > leadAxis_value:
leadAxis = i
leadAxis_value = s
abs_displacement_vector_steps.append(s)
if dirBits != self.printer.curDirBits:
move.stepData.setDirBits = True
move.stepData.dirBits = dirBits
self.printer.curDirBits = dirBits
steps_per_mm = PrinterProfile.getStepsPerMM(leadAxis)
#
# Bresenham's variables
#
deltaLead = abs_displacement_vector_steps[leadAxis]
move.stepData.setBresenhamParameters(leadAxis, abs_displacement_vector_steps)
#
# Create a list of stepper pulses
#
# debnegtimer
nominalSpeed = abs( move.getReachedSpeedV()[leadAxis] ) # [mm/s]
reachedSpeedV = move.getReachedSpeedV()
reachedSpeedFr = reachedSpeedV.len5()
# debnegtimer
accel = abs(move.getAllowedAccelVector()[leadAxis])
accel_steps_per_square_second = accel * steps_per_mm
# startSpeed = move.getStartFr()
# debnegtimer
v0 = abs(move.getFeedrateV(move.getStartFr())[leadAxis]) # [mm/s]
# endSpeed = move.getEndFr()
# debnegtimer
v1 = abs(move.getFeedrateV(move.getEndFr())[leadAxis]) # [mm/s]
# debnegtimer
steps_per_second_0 = steps_per_second_accel = v0 * steps_per_mm
steps_per_second_1 = steps_per_second_deccel = v1 * steps_per_mm
steps_per_second_nominal = nominalSpeed * steps_per_mm
#
# Acceleration variables
#
# tAccel = 0 # [s], sum of all acceeration steptimes
# tAccel mit der initialen zeitspanne vorbelegen, da wir bereits im
# ersten schleifendurchlauf (d.h. ab t=0) eine beschleunigung haben wollen.
tAccel = 1.0 / steps_per_second_0 # [s], sum of all acceeration steptimes
tDeccel = 1.0 / steps_per_second_1
stepNr = 0
if debugPlot:
accelPlotData = []
deccelPlotData = []
if not circaf(move.getStartFr(), reachedSpeedFr, 0.1) and not circaf(move.getEndFr(), reachedSpeedFr, 0.1):
#
# Acceleration ramp on both sides.
#
# Ramp up both sides in parallel to not exeed available steps
#
done = False
while not done and stepNr < deltaLead:
done = True
#
# Compute acceleration timer values
#
if steps_per_second_accel < steps_per_second_nominal:
#
# Compute timer value
#
steps_per_second_accel = min(steps_per_second_0 + tAccel * accel_steps_per_square_second, steps_per_second_nominal)
dt = 1.0 / steps_per_second_accel
timerValue = int(fTimer / steps_per_second_accel)
# print "dt: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
if debugPlot:
if move.eOnly:
accelPlotData.append((steps_per_second_accel/steps_per_mm, 2, dt))
else:
accelPlotData.append((steps_per_second_accel/steps_per_mm, 1, dt))
tAccel += dt
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addAccelPulse(timerValue)
stepNr += 1
done = False
if stepNr == deltaLead:
break
#
# Compute ramp down (in reverse), decceleration
#
if steps_per_second_deccel < steps_per_second_nominal:
#
# Compute timer value
#
steps_per_second_deccel = min(steps_per_second_1 + tDeccel * accel_steps_per_square_second, steps_per_second_nominal)
dt = 1.0 / steps_per_second_deccel
timerValue = int(fTimer / steps_per_second_deccel)
# print "dt: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue, ", v: ", steps_per_second_deccel/steps_per_mm
if debugPlot:
if move.eOnly:
deccelPlotData.append((steps_per_second_deccel/steps_per_mm, 2, dt))
else:
deccelPlotData.append((steps_per_second_deccel/steps_per_mm, 1, dt))
tDeccel += dt
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addDeccelPulse(timerValue)
stepNr += 1
done = False
elif not circaf(move.getStartFr(), reachedSpeedFr, 0.1):
#
# Acceleration only
#
#
# Compute acceleration timer values
#
while steps_per_second_accel < steps_per_second_nominal and stepNr < deltaLead:
#
# Compute timer value
#
steps_per_second_accel = min(steps_per_second_0 + tAccel * accel_steps_per_square_second, steps_per_second_nominal)
dt = 1.0 / steps_per_second_accel
timerValue = int(fTimer / steps_per_second_accel)
# print "dt: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
if debugPlot:
if move.eOnly:
accelPlotData.append((steps_per_second_accel/steps_per_mm, 2, dt))
else:
accelPlotData.append((steps_per_second_accel/steps_per_mm, 1, dt))
tAccel += dt
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second_accel, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addAccelPulse(timerValue)
stepNr += 1
else:
#
# Decceleration only
#
#
# Compute ramp down (in reverse), decceleration
#
while steps_per_second_deccel < steps_per_second_nominal and stepNr < deltaLead:
#
# Compute timer value
#
steps_per_second_deccel = min(steps_per_second_1 + tDeccel * accel_steps_per_square_second, steps_per_second_nominal)
dt = 1.0 / steps_per_second_deccel
timerValue = int(fTimer / steps_per_second_deccel)
# print "dt: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue, ", v: ", steps_per_second_deccel/steps_per_mm
if debugPlot:
if move.eOnly:
deccelPlotData.append((steps_per_second_deccel/steps_per_mm, 2, dt))
else:
deccelPlotData.append((steps_per_second_deccel/steps_per_mm, 1, dt))
tDeccel += dt
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second_deccel, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addDeccelPulse(timerValue)
stepNr += 1
#
# Linear phase
#
timerValue = fTimer / steps_per_second_nominal
move.stepData.setLinTimer(timerValue)
if debugPlot:
self.plotfile.write("# Acceleration:\n")
for (speed, color, dt) in accelPlotData:
self.plotfile.write("%f %f %d\n" % (self.plottime, speed, color))
self.plottime += dt
self.plotfile.write("# Linear top:\n")
self.plotfile.write("%f %f 0\n" % (self.plottime, steps_per_second_nominal/steps_per_mm))
self.plottime += timerValue / fTimer
self.plotfile.write("# Decceleration:\n")
deccelPlotData.reverse()
for (speed, color, dt) in deccelPlotData:
self.plotfile.write("%f %f %d\n" % (self.plottime, speed, color))
self.plottime += dt
if debugMoves:
print "# of steps for move: ", deltaLead
move.pprint("move:")
print
move.stepData.checkLen(deltaLead)
if debugMoves:
print "***** End planSteps() *****"
return
# XXXXXXXXXXXXXX old, unused ...........
#
# Compute acceleration timer values
#
# while v0 and steps_per_second < steps_per_second_nominal:
# lastTimer = None
while steps_per_second < steps_per_second_nominal: # and stepNr < deltaLead:
assert(stepNr < deltaLead)
#
# Compute timer value
#
steps_per_second = min(steps_per_second_0 + tAccel * accel_steps_per_square_second, steps_per_second_nominal)
dt = 1.0 / steps_per_second
timerValue = int(fTimer / steps_per_second)
# print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
if debugPlot:
if move.eOnly:
self.plotfile.write("%f %f 2\n" % (self.plottime, steps_per_second/steps_per_mm))
else:
self.plotfile.write("%f %f 1\n" % (self.plottime, steps_per_second/steps_per_mm))
self.plottime += dt
tAccel += dt
# debnegtimer
# if timerValue <= 0:
# print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
# assert(0)
# debtimeroverflow
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addAccelPulse(timerValue)
stepNr += 1
# if lastTimer:
# assert((lastTimer - timerValue) < maxTimerValue16)
# lastTimer = timerValue
# Benutze als timer wert für die lineare phase den letzen timerwert der
# beschleunigungsphase falls es diese gab. Sonst:
# berechne timervalue ausgehend von linear feedrate:
# if not timerValue:
# timerValue = fTimer / steps_per_second
# dt = 1.0 / steps_per_second
# print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
timerValue = fTimer / steps_per_second_nominal
move.stepData.setLinTimer(timerValue)
if debugPlot:
self.plotfile.write("# Linear top:\n")
self.plotfile.write("%f %f 0\n" % (self.plottime, steps_per_second/steps_per_mm))
self.plottime += timerValue / fTimer
self.plotfile.write("# Decceleration:\n")
#
# Compute ramp down (in reverse), decceleration
#
tDeccel = 1.0 / steps_per_second_nominal
steps_per_second = steps_per_second_nominal
steps_per_second_1 = v1 * steps_per_mm
# lastTimer = None
while steps_per_second > steps_per_second_1: # and stepNr < deltaLead:
if stepNr >= deltaLead:
print "stepNr, deltaLead:", stepNr, deltaLead
assert(stepNr < deltaLead)
#
# Compute timer value
#
steps_per_second = max(steps_per_second_nominal - tDeccel * accel_steps_per_square_second, steps_per_second_1)
dt = 1.0 / steps_per_second
timerValue = int(fTimer / steps_per_second)
# print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue, ", v: ", steps_per_second/steps_per_mm
if debugPlot:
if move.eOnly:
self.plotfile.write("%f %f 2\n" % (self.plottime, steps_per_second/steps_per_mm))
else:
self.plotfile.write("%f %f 1\n" % (self.plottime, steps_per_second/steps_per_mm))
self.plottime += dt
tDeccel += dt
# debnegtimer
# assert(timerValue > 0)
if timerValue > maxTimerValue24:
move.pprint("PlanSTeps:")
print "v0: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
print "dt: ", dt*1000000, "[uS]", steps_per_second, "[steps/s], timerValue: ", timerValue
assert(0)
move.stepData.addDeccelPulse(timerValue)
stepNr += 1
# if lastTimer:
# assert((timerValue - lastTimer) < maxTimerValue16)
# lastTimer = timerValue
if debugMoves:
print "# of steps for move: ", deltaLead
move.pprint("move:")
print
move.stepData.checkLen(deltaLead)
if debugMoves:
print "***** End planSteps() *****"
