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() *****"