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