def getMaxAllowedAccelVectorNoAdv5(self):
accelVector = self.direction5.scale(_MAX_ACCELERATION)
return abs(accelVector.constrain(PrinterProfile.getMaxAxisAcceleration()) or accelVector)
def calibrateFilSensor(args, parser):
def writeDataSet(f, data):
for tup in data:
f.write("%f %f\n" % tup)
f.write("E\n")
planner = parser.planner
printer = planner.printer
maxFeedrate = args.feedrate or 15
eAccel = PrinterProfile.getMaxAxisAcceleration()[A_AXIS]
printer.commandInit(args)
ddhome.home(parser, args.fakeendstop)
# Disable flowrate limit
printer.sendCommandParamV(CmdEnableFRLimit, [packedvalue.uint8_t(0)])
# Disable temp-flowrate limit
util.downloadDummyTempTable(printer)
# XXX todo set filament sensor calibration factor to 1.0:
assert (
0
) # printer.sendCommandParamV(CmdSetFRCalFactor, [packedvalue.float_t(1.0)])
feedrate = 0.25
startPos = parser.getPos()[A_AXIS]
calFile = open("calibrateFilSensor.json", "w")
calFile.write('{\n "filSensorCalibration": [\n')
calValues = []
while feedrate <= maxFeedrate:
tStartup = util.getStartupTime(feedrate)
tAccel = feedrate / eAccel
accelDistance = 5 * feedrate
print "accelDistance: ", accelDistance
current_position = parser.getPos()
apos = current_position[A_AXIS]
printer.sendPrinterInit()
parser.execute_line(
"G0 F%d %s%f" %
(feedrate * 60, util.dimNames[A_AXIS], apos + accelDistance))
planner.finishMoves()
printer.sendCommand(CmdEOT)
printer.sendCommandParamV(CmdMove, [MoveTypeNormal])
status = printer.getStatus()
while status["slippage"] > 1.5:
print "wait for startup..."
status = printer.getStatus()
time.sleep(0.1)
data = []
t = time.time()
while status["state"] != StateIdle:
tMeasure = time.time() - t
if tMeasure >= tStartup:
data.append(status["slippage"])
status = printer.getStatus()
time.sleep(0.01)
# Cut the last tAccel:
cut = int(tAccel / 0.01 + 1)
del data[-cut:]
# Average of ratio for this speed
grip = sum(data) / len(data)
# Calibration value, targetSpeed/measuredSensorSpeed, this ist the value
# to multiply the measured sensor speed to get the real speed:
print "speed:", feedrate, "ratio:", grip
calValues.append((feedrate, grip))
feedrate += 0.25
f = open("calibrateFilSensor.gnuplot", "w")
f.write("""
set grid
set yrange [0:%f]
plot '-' using 1:2 with linespoints title 'ratio'
""" % max(1.5, feedrate * 2))
writeDataSet(f, calValues)
f.write("pause mouse close\n")
calFile.write(",\n".join(
map(lambda tup: " [%f, %f]" % tup, calValues)))
calFile.write("\n ]\n")
calFile.write("}\n")
calFile.close()
printer.sendPrinterInit()
parser.execute_line("G0 F%d %s%f" %
(maxFeedrate * 60, util.dimNames[A_AXIS], startPos))
planner.finishMoves()
printer.sendCommand(CmdEOT)
printer.sendCommandParamV(CmdMove, [MoveTypeNormal])
printer.waitForState(StateIdle)
# Enable flowrate limit
printer.sendCommandParamV(CmdEnableFRLimit, [packedvalue.uint8_t(1)])
def planTravelAcceleration(self, move):
if debugMoves:
print "***** Start planTravelAcceleration() *****"
move.pprint("planTravelAcceleration")
move.state = 2
allowedAccel = allowedDecel = move.getMaxAllowedAccelNoAdv5()
#
# Check if the speed difference between startspeed and endspeed can be done with
# this acceleration in this distance
#
startSpeedS = move.startSpeed.speed().feedrate
endSpeedS = move.endSpeed.speed().feedrate
deltaSpeedS = endSpeedS - startSpeedS
if abs(deltaSpeedS) > 0.001:
ta = abs(deltaSpeedS) / allowedAccel
if deltaSpeedS >= 0:
# acceleration
sa = util.accelDist(startSpeedS, allowedAccel, ta)
else:
# deceleration
sa = util.accelDist(startSpeedS, -allowedAccel, ta)
if (sa - move.distance5) > 0.001:
print "VStart %f mm/s kann nicht innerhalb von %f mm auf Endgeschwindigkeit %f mm/s gebracht werden!" % (
startSpeedS, move.distance5, endSpeedS)
print "Dafür werden %f mm benötigt" % sa
assert (0)
#
# Compute distance to accel from start speed to nominal speed:
#
ta = 0.0
sa = 0.0
deltaStartSpeedS = move.topSpeed.speed().feedrate - startSpeedS
maxAccel = PrinterProfile.getMaxAxisAcceleration()
if deltaStartSpeedS:
ta = deltaStartSpeedS / allowedAccel
# print "accel time (for %f mm/s): %f [s]" % (deltaStartSpeedS, ta)
# debug Check axxis acceleration
deltaSpeedV = move.direction5.scale(deltaStartSpeedS)
for dim in range(5):
dimAccel = abs(deltaSpeedV[dim]) / ta
if (dimAccel / maxAccel[dim]) > 1.001:
print "dim %d verletzt max accel: " % dim, dimAccel, " > ", maxAccel[
dim]
assert (0)
#end debug
sa = util.accelDist(startSpeedS, allowedAccel, ta)
#
# Compute distance to decel from nominal speed to endspeed:
#
tb = 0.0
sb = 0.0
deltaEndSpeedS = move.topSpeed.speed().feedrate - endSpeedS # [mm/s]
if deltaEndSpeedS:
tb = deltaEndSpeedS / allowedAccel # [s]
# print "decel time (for %f mm/s): %f [s]" % (deltaEndSpeedS, tb)
# debug Check axxis acceleration
deltaSpeedV = move.direction5.scale(deltaEndSpeedS)
for dim in range(5):
dimDecel = abs(deltaSpeedV[dim]) / tb
if (dimDecel / maxAccel[dim]) > 1.001:
print "dim %d verletzt max accel: " % dim, dimDecel, " [mm/s] > ", maxAccel[
dim], " [mm/s]"
assert (0)
# end debug
sb = util.accelDist(endSpeedS, allowedAccel, tb)
# print "e_distance: %f, sbeschl, sbrems: %f, %f" % (move.e_distance, sa, sb)
if move.distance5 < (sa + sb):
#
# Strecke zu kurz, Trapez nicht möglich, geschwindigkeit muss abgesenkt werden.
#
if debugMoves:
print "Trapez nicht möglich: s: %f, sbeschl (%f) + sbrems (%f) = %f" % (
move.distance5, sa, sb, sa + sb)
# ???
assert (sa > 0 and sb > 0)
topSpeed = move.topSpeed.speed()
sa = (2 * allowedAccel * move.distance5 - pow(startSpeedS, 2) +
pow(endSpeedS, 2)) / (4 * allowedAccel)
sb = move.distance5 - sa
if debugMoves:
print "sbeschl, sbrems neu: %f, %f" % (sa, sb)
#
# Geschwindigkeit, die auf strecke sa mit erreicht werden kann
#
v = math.sqrt(2 * allowedAccel * sa + pow(startSpeedS, 2))
# debug, test
v2 = math.sqrt(2 * allowedAccel * sb + pow(endSpeedS, 2))
# print "move.feedrate neu: %f (test: %f, diff: %f)" % (v, v2, abs(v - v2))
assert (abs(v - v2) < 0.001)
deltaSpeedS = v - startSpeedS # [mm/s]
# Handle rounding errors
if deltaSpeedS < 0:
assert (deltaSpeedS > -0.000001)
deltaSpeedS = 0
v = startSpeedS
ta = deltaSpeedS / allowedAccel
# print "ta: ", ta, deltaSpeedS
deltaSpeedS = v - endSpeedS # [mm/s]
# Handle rounding errors
if deltaSpeedS < 0:
assert (deltaSpeedS > -0.000001)
deltaSpeedS = 0
v = endSpeedS
tb = deltaSpeedS / allowedAccel
# print "tb: ", tb, deltaSpeedS
topSpeed.feedrate = v
move.topSpeed.setSpeed(
topSpeed, "planTravelAcceleration - max reachable topspeed")
move.setDuration(ta, 0, tb)
if debugMoves:
move.pprint("planTravelAcceleration")
print
print "***** End planTravelAcceleration() *****"
return
#
# Strecke reicht aus, um auf nominal speed zu beschleunigen
#
# print "ta: ", ta, deltaStartSpeedS, sa
# print "tb: ", tb, deltaEndSpeedS, sb
nominalSpeed = move.topSpeed.speed().feedrate # [mm/s]
slin = move.distance5 - (sa + sb)
tlin = slin / nominalSpeed
# print "tlin: ", tlin, slin
move.setDuration(ta, tlin, tb)
if debugMoves:
move.pprint("planTravelAcceleration")
print
print "***** End planTravelAcceleration() *****"
