anbei mein Code für eine 3D Drucker Software (ist noch nicht ganz fertig, aber auf einem guten Weg). Sie kann 4 Motoren quasi-gleichzeitig steuern (per Extrapolation).
Ich habe zur Zeit noch einen komische Rechenpower Anstieg: Wenn ich eine GCode Zeile habe, die zum Beispiel so aussieht: "G1 X67 Y67 E20.1 F7800" (X 67 mm, Y 67 mm und 20.1 mm, bei 7800 mm/sek), dann rechnet mein Pi das runter und steuert die 3 Motoren "gleichzeitig" (leider langsamer, als ich es erwarten würde, weiß noch nicht genau, warum. Vielleicht, weil es ja nur "mehr oder weniger" gleichzeitig ist...).
Wenn nun eine GCode Zeile folgendermaßen ausschaut: "G1 X92.150 Y93.154 F7800.000", dann hängt er sich auf, die Motoren drehen sich nicht und der Pi braucht ewig. WinSCP und Putty sagen beide, die Verbindung wäre verloren.
Ich habe das ganze auch schon laufen lassen, ohne dass die Motoren mit Strom versorgt wären, und der kleine Computer braucht trotzdem ewig. Der RAM Verbrauch schießt von 13 % auf 95 % + SWAP Auslagerung => Es ist ein software-seitiges Rechenproblem. Allerdings finde ich in meinem Code nirgends das verantwortliche Teilstück.
Vielleicht hat ja von euch einer eine Idee
VG
elchico
PS: Die Funktion IJPosition ist bisher nur "show" und hat noch keine richtige Funktion...
Code: Alles auswählen
#####################################################
# #
# G code interpreter and executer for 3D Printer using Raspberry Pi #
# #
# #
# #
#####################################################
import RPi.GPIO as GPIO
import time
from numpy import pi, sin, cos, sqrt, arccos, arcsin, abs
file = 'test2.gcode' #file name of the Gcode commands
GPIO.setwarnings(False)
GPIO.cleanup()
class Step_Motor(object):
def __init__(self, pins, resolution, mm_round, mode=3, phase=0):
"""Initialise the motor object.
pins -- a list of 4 integers referring to the GPIO pins that the IN1, IN2
IN3 and IN4 pins of the ULN2003 board are wired to
"""
self.P1 = pins[0]
self.P2 = pins[1]
self.P3 = pins[2]
self.P4 = pins[3]
self.resolution = resolution
self_dis = mm_round
self.mode = mode
self.phase=phase
self.deg_per_step = self.resolution # for half-step drive (mode 3)
self.steps_per_rev = int(360 / self.deg_per_step) # 4096
self.step_angle = 0 # Assume the way it is pointing is zero degrees
self.position = 0
for p in pins:
GPIO.setup(p, GPIO.OUT)
GPIO.output(p, 0)
def _set_rpm(self, rpm):
"""Set the turn speed in RPM."""
self._rpm = self.resolution
# T is the amount of time to stop between signals
self._T = (60.0 / self._rpm) / self.steps_per_rev
# This means you can set "rpm" as if it is an attribute and
# behind the scenes it sets the _T attribute
rpm = property(lambda self: self._rpm, _set_rpm)
def move(self, direction, steps, delay=0.001):
#phase_seq=[[1,1,0,0],[0,1,1,0],[0,0,1,1],[1,0,0,1]] #full step sequence. maximum torque
phase_seq=[[1,0,0,0],[1,1,0,0],[0,1,0,0],[0,1,1,0],[0,0,1,0],[0,0,1,1],[0,0,0,1],[1,0,0,1]] #half-step sequence. double resolution. But the torque of the stepper motor is not constant
num_phase=len(phase_seq);
for i in range(steps):
next_phase=(self.phase+direction) % num_phase;
GPIO.output(self.P1,phase_seq[next_phase][0]);
GPIO.output(self.P2,phase_seq[next_phase][1]);
GPIO.output(self.P3,phase_seq[next_phase][2]);
GPIO.output(self.P4,phase_seq[next_phase][3]);
self.phase=next_phase;
time.sleep(delay);
self.position = self.position + (direction*steps)
def clear(self):
GPIO.output(self.P1, 0)
GPIO.output(self.P2, 0)
GPIO.output(self.P3, 0)
GPIO.output(self.P4, 0)
def params(): #returns list_params = [MX, MY, MZ, MF, dx, dy, dz, df, print_speed, fan, heatbed]
list_params=[]
GPIO.setmode(GPIO.BCM)
pins_xyzf=[(22,23,24,27), (12,16,20,21), (11,14,15,7), (25,26,4,17)] #pin number for a1,a2,b1,b2. a1 and a2 form coil A; b1 and b2 form coil B
degx=5.625/64 #degree per step
degy=5.625/64
degz=5.625/64
degf=5.625/64
round_x=67 #mm/round = mm/360
round_y=67
round_z=67
round_f=67
dx=round_x/(360/degx) #mm/step
dy=round_y/(360/degy)
dz=round_z/(360/degz)
df=round_f/(360/degf)
rpm_x = 18 #rounds/min
rpm_y = 18
rpm_z = 18
rpm_f = 18
#extruder_heat_on=7
heatbed=8 #heatbed
speed_filament=5 #filament release speed
fan=18 #Fan
#free pins: 2, 3, 5, 6, 9, 10
MX=Step_Motor(pins_xyzf[0], degx, dx)
MY=Step_Motor(pins_xyzf[1], degy, dy)
MZ=Step_Motor(pins_xyzf[2], degz, dz)
MF=Step_Motor(pins_xyzf[3], degf, df)
MX.rpm=[rpm_x]
MY.rpm=[rpm_y]
MZ.rpm=[rpm_z]
MF.rpm=[rpm_f]
MZ_Home_position = 0.5 #Home Position of MZ in mm
MZ.position=int(MZ_Home_position/dz)
GPIO.setup(fan,GPIO.OUT)
GPIO.setup(fan, False)
GPIO.setup(heatbed,GPIO.OUT)
GPIO.output(heatbed,False)
print_speed=min(((360/degx)*rpm_x/60), ((360/degy)*rpm_y/60),((360/degf)*rpm_f/60)) #max. step/sec
list_params = [MX, MY, MZ, MF, dx, dy, dz, df, print_speed, fan, heatbed]
return list_params
def control(stepper1, step1, stepper2, step2, stepper3, step3, stepperf, stepf,df, velo): #calculates least common multiplier and directions and calls class-function 'step_motor.move'
def LCM(a,b,c): #returns smallest common number
d = b #a&b
lst = [a,b,c]
if 0 in lst:
lst[lst.index(0)]=1
a = lst[0]
b = lst[1]
c = lst[2]
while d%a != 0:
d=d+b
e = d
while d%c != 0: #and c
e=e+d
return e
def sign(a): #returns the sign of number a
if a>0:
return 1;
elif a<0:
return -1;
else:
return 0;
def Motor_Step(stepper1, step1, stepper2, step2, stepper3, step3, stepperf, stepf, velo):
#control stepper motor 1 and 2 simultaneously
#stepper1 and stepper2 are objects of Bipolar_Stepper_Motor class
#direction is reflected in the polarity of [step1] or [step2]
dir1=sign(step1) #get direction from the polarity of argument [step]
dir2=sign(step2)
dir3=sign(step3)
dirf=sign(stepf)
step1=int(abs(round(step1))) #absolute value of steps
step2=int(abs(round(step2)))
step3=int(abs(round(step3)))
stepf=int(round(abs(stepf))) #Feedrate in steps/sec
#[total_micro_step] total number of micro steps
#stepper motor 1 will move one step every [micro_step1] steps
#stepper motor 2 will move one step every [micro_step2] steps
#first motor 3 will move (MZ), then filament-motor will start and work continuously and last, the 2D-motors will move
if step1==0 and step2==0 and stepf==0:
total_micro_step=0
elif step1==0 and stepf==0 and step2!=0:
total_micro_step=step2;
micro_step2=1;
micro_step1=total_micro_step+100
micro_stepf=total_micro_step+100
elif step1==0 and step2==0 and stepf!=0:
total_micro_step=stepf;
micro_stepf=1;
micro_step2=total_micro_step+100
micro_step1=total_micro_step+100
elif step1==0 and step2!=0 and stepf!=0:
total_micro_step=LCM(step1,step2,stepf)
micro_step2=total_micro_step/step2
micro_stepf=total_micro_step/stepf
micro_step1=total_micro_step+100
elif step2==0 and stepf==0 and step1!=0:
total_micro_step=step1;
micro_step1=1;
micro_step2=total_micro_step+100
micro_stepf=total_micro_step+100
elif step2==0 and step1!=0 and stepf!=0:
total_micro_step=LCM(step1,step2,stepf)
micro_step1=total_micro_step/step1
micro_stepf=total_micro_step/stepf
micro_step2=total_micro_step+100
elif stepf==0 and step1!=0 and step2!=0:
total_micro_step=LCM(step1,step2,stepf)
micro_step1=total_micro_step/step1
micro_step2=total_micro_step/step2
micro_stepf=total_micro_step+100
else:
total_micro_step=LCM(step1,step2,stepf)
micro_step1=total_micro_step/step1
micro_step2=total_micro_step/step2
micro_stepf=total_micro_step/stepf
t = max(step1,step2,stepf)/velo+step3/velo #Calculation time per line
print "time:", t
if step3 != 0:
for i in range (1,step3+1):
stepper3.move(dir3,1) #first MZ
else:
pass
if total_micro_step==0 or velo==0:
print "No Steps or no Velocity."
return 0
else:
dt=1/velo #time delay every micro_step
for i in range(1,total_micro_step+1): #i is the iterator for the micro_step. i cannot start from 0
time_laps=0;
micro_step=0
if (i % micro_step1)==0: #motor 1 need to turn one step
stepper1.move(dir1,1,dt/3);
time_laps+=dt/3
micro_step=1
if (i % micro_step2)==0: #motor 2 need to turn one step
stepper2.move(dir2,1,dt/3);
time_laps+=dt/3
micro_step=1
if (i % micro_stepf)==0: #motor 2 need to turn one step
stepperf.move(dirf,1,dt/3);
time_laps+=dt/3
micro_step=1
if micro_step==1:
time.sleep(dt-time_laps)
else:
pass
Motor_Step(stepper1, step1, stepper2, step2, stepper3, step3, stepperf, stepf, velo)
return 0;
def XYZFEposition(lines,old_x,old_y,old_z,old_f,old_velo,quot): #calculates new position of MX & MY & MZ & Feedrate f_rate
#given a movement command line, return the X Y Z position and Feedrate
try: #x_pos
xchar_loc=lines.index('X');
i=xchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
x_pos=float(lines[xchar_loc+1:i])
except:
x_pos=old_x
try: #y_pos
ychar_loc=lines.index('Y');
i=ychar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
y_pos=float(lines[ychar_loc+1:i]);
except:
y_pos=old_y
try: #z_pos
zchar_loc=lines.index('Z');
i=zchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
z_pos=float(lines[zchar_loc+1:i]);
except:
z_pos=old_z
try: #f_rate
echar_loc=lines.index('E');
i=echar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
f_rate=float(lines[echar_loc+1:i]);
except:
f_rate=0
try: #Velocity
vchar_loc=lines.index('F');
i=vchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
velo=float(lines[vchar_loc+1:i]);
except:
velo=old_velo/quot
print "-----------------------------------------------------------------"
if f_rate == 0:
print 'No printer, fast movement'
else:
print 'Printer on'
print "X-Pos:", old_x,"-->",x_pos,"mm \nY-Pos:", old_y,"-->",y_pos,"mm \nZ-Pos:", old_z,"-->",z_pos,"mm \nFeedrate:", f_rate, "mm"
return x_pos,y_pos,z_pos,f_rate,velo
def IJposition(lines): #calculates new position for circular movement
#given a G02 or G03 movement command line, return the I J position
ichar_loc=lines.index('I');
i=ichar_loc+1;
while (47<ord(lines[i])<58)|(lines[i]=='.')|(lines[i]=='-'):
i+=1;
i_pos=float(lines[ichar_loc+1:i]);
jchar_loc=lines.index('J');
i=jchar_loc+1;
while (47<ord(lines[i])<58)|(lines[i]=='.')|(lines[i]=='-'):
i+=1;
j_pos=float(lines[jchar_loc+1:i]);
return i_pos,j_pos;
def moveto(MX,x_pos,dx,MY,y_pos,dy,MZ,z_pos,dz,MF,f_rate,df,velo): #3D movement + f_rate + Velocity
#Move to (x_pos,y_pos, z_pos, f_rate) with Velocity (velo)
stepx = int(x_pos/dx)-MX.position
stepy = int(y_pos/dy)-MY.position
stepz = int(z_pos/dz)-MZ.position
stepf = int(f_rate/df)
velo = (velo/60)/min(dx,dy,dz,df)
print"Velocity:", velo,"steps/sec"
if stepx==0 and stepy==0 and stepz==0 and stepf==0:
return 0
else:
control(MX,stepx,MY,stepy,MZ,stepz,MF,stepf,df,velo);
return 0;
def velocity(filename, print_speed,dx,dy,dz,df): #looking for velocities and compare with max_poss. velocity
velocities = []
for lines in open(filename, 'r'):
if lines[0:2]=='G1' or lines[0:3]=='G01' or lines[0:3]=='G00' or lines[0:2]=='G0' or lines[0:3]=='G02' or lines[0:3]=='G03' or lines[0:2]=='G2' or lines[0:2]=='G3':
if 'F' in lines:
vchar_loc=lines.index('F');
i=vchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
v = int(round(float(lines[vchar_loc+1:i])))
velocities.append(v)
else:
pass
else:
pass
if len(velocities) == 0:
print "No velocity given!"
return 0
else:
velocities.sort()
maximum_mm = int(velocities.pop(len(velocities)-1))/60
max_poss = print_speed*min(dx,dy,dz,df)
if maximum_mm == max_poss:
quot == 1
elif maximum_mm > max_poss:
quot = max_poss/maximum_mm
quot = quot - 0.1*quot
elif maximum_mm < max_poss:
quot = max_poss/maximum_mm
quot = quot - 0.1*quot
return quot
def total_time(filename,MX,MY,MZ,MF,dx,dy,dz,df,old_x,old_y,old_z,old_f,quot): #Calculation of total time required
T_total = 0
old_velo = 0
for lines in open(filename,'r'):
if lines[0:3]=='G1 ' or lines[0:3]=='G01' or lines[0:3]=='G00' or lines[0:3]=='G0 ' or lines[0:3]=='G02' or lines[0:3]=='G03' or lines[0:3]=='G2 ' or lines[0:3]=='G3 ':
try: #Velocity
vchar_loc=lines.index('F');
i=vchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
velo=float(lines[vchar_loc+1:i])
old_velo=velo
except:
velo=old_velo
try: #x_pos
xchar_loc=lines.index('X');
i=xchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
x_pos=float(lines[xchar_loc+1:i])
except:
x_pos=old_x
try: #y_pos
ychar_loc=lines.index('Y');
i=ychar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
y_pos=float(lines[ychar_loc+1:i]);
except:
y_pos=old_y
try: #z_pos
zchar_loc=lines.index('Z');
i=zchar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
z_pos=float(lines[zchar_loc+1:i]);
except:
z_pos=old_z
try: #f_rate
echar_loc=lines.index('E');
i=echar_loc+1
while i < len(lines) and lines[i] in ("0","1","2","3","4","5","6","7","8","9","-", "."):
i += 1
f_rate=float(lines[echar_loc+1:i]);
except:
f_rate=0
velo = velo*quot
stepx = int(round(x_pos/dx))-MX.position;
stepy = int(round(y_pos/dy))-MY.position;
stepz = int(round(z_pos/dz))-MZ.position;
stepf = int(round(f_rate/df))
velo = (velo/60)/(min(dx,dy,dz,df))
step1=int(abs(stepx)) #absolute value of steps
step2=int(abs(stepy))
step3=int(abs(stepz))
f_rate=int(round(abs(stepf)/60)) #Feedrate in steps/sec
max_distance=max(step1,step2,f_rate)
if step3==0:
T_total=T_total+(max_distance/velo)
else:
T_total=T_total+(max_distance/velo)+step3/velo
else:
pass
print "-----------------------------------------------------------------"
print "Total time", (T_total/60), "min"
def gcode_interpreter(filename, parameters): #reading and executing G code
MX = parameters[0]
MY = parameters[1]
MZ = parameters[2]
MF = parameters[3]
dx = parameters[4]
dy = parameters[5]
dz = parameters[6]
df = parameters[7]
print_speed = parameters[8]
fan = parameters[9]
heatbed = parameters[10]
old_x=MX.position
old_y=MY.position
old_z=float(round(MZ.position*dz,1))
old_f=0
old_velo=0
quot = velocity(filename, print_speed,dx,dy,dz,df)
total_time(filename,MX,MY,MZ,MF,dx,dy,dz,df,old_x,old_y,old_z,old_f,quot)
for lines in open(filename,'r'): #start Gcode-program
if lines[0]=='G': #movement
if lines[0:3]=='G90': #start gcode interpreting
print "-----------------------------------------------------------------"
print "-----------------------------------------------------------------"
print "-----------------------------------------------------------------"
print 'start, absolute coordinates';
elif lines[0:3]=='G0 ' or lines[0:3]=='G00': #rapid linear move
[x_pos,y_pos,z_pos,f_rate,velo]=XYZFEposition(lines,old_x,old_y,old_z,old_f,old_velo,quot)
velo = velo*quot
moveto(MX,x_pos,dx,MY,y_pos,dy,MZ,z_pos,dz,MF,f_rate,df,velo)
old_x=x_pos
old_y=y_pos
old_z=z_pos
old_velo=velo
elif lines[0:3]=='G1 ' or lines[0:3]=='G01': #rapid linear move
[x_pos,y_pos,z_pos,f_rate,velo]=XYZFEposition(lines,old_x,old_y,old_z,old_f,old_velo,quot)
velo = velo*quot
moveto(MX,x_pos,dx,MY,y_pos,dy,MZ,z_pos,dz,MF,f_rate,df,velo)
old_x=x_pos
old_y=y_pos
old_z=z_pos
old_f=f_rate
old_velo=velo
elif lines[0:3]=='G02' or lines[0:3]=='G03' or lines[0:3]=='G2 ' or lines[0:3]=='G3 ': #circular interpolation
pass
elif lines[0:3]=='G4 ' or lines[0:3]=='G04': #wait
try:
index_P = lines.index(P)
sleep_time=0
for i in range (len(lines), index_P+1):
expo = 0
sleep_time=(sleep_time+lines[i]^expo)/1000
expo+=1
time.sleep(sleep_time)
except:
index_S = lines.index(S)
sleep_time=0
for i in range (len(lines), index_S+1):
expo = 0
sleep_time=sleep_time+lines[i]^expo
expo+=1
time.sleep(sleep_time)
elif lines[0:3]=='G21': #working in mm
print 'Working in mm';
elif lines[0:3]=='G20': #cannot work in inch
print 'Wrong Unit of length'
break
elif lines[0:3]=='G28': #Move home
[x_pos,y_pos,z_pos,f_rate,velo]=((0),(0),(0.5),0,(print_speed*60*min(dx,dy,dz,df)))
print "-----------------------------------------------------------------"
print "Move Home"
moveto(MX,x_pos,dx,MY,y_pos,dy,MZ,z_pos,dz,MF,f_rate,df,velo)
elif lines[0]=='M': #options
if lines[0:3]=='M00' or lines[0:2]=='M0' or lines[0:3]=='M01' or lines[0:2]=='M1': #printer off
MX.clear()
MY.clear()
MZ.clear()
MF.clear()
elif lines[0:3]=='M02' or lines[0:2]=='M2': #Program ends
print 'Program finished';
elif lines[0:4]=='M104' or lines[0:4]=='M109': #Extruder Temp
try:
index_S = lines.index(S)
extruder_temp=0
for i in range (len(lines), index_S+1):
expo = 0
extruder_temp=extruder_temp+lines[i]^expo
expo+=1
#setting extruder temp
except:
print 'Error: Extruder Temp not accessible'
break
elif lines[0:4]=='M106': #Fan On
index_S = lines.index(S)
fan_speed=0
for i in range (len(lines), index_S+1):
expo = 0
fan_speed=fan_speed+lines[i]^expo
expo+=1
GPIO.output(fan, True)
elif lines[0:4]=='M107': #Fan Off
GPIO.output(fan, False)
elif lines[0:4]=='M140' or lines[0:4]=='M190': #Heatbed on
index_S = lines.index(S)
heatbed_temp=0
for i in range (len(lines), index_S+1):
expo = 0
heatbed_temp=heatbed_temp+lines[i]^expo
expo+=1
if heatbed_temp == 0:
GPIO.output(heatbed, False)
else:
GPIO.output(heatbed, True) #wait until it is hot
else: #Comments, Tools (not supported) in GCode = ignore
pass
GPIO.output(fan, False)
GPIO.output(heatbed, False)
time.sleep(2)
GPIO.cleanup()
def main():
try:
list_params = params() #parameters, motors, heatbed, speed, steps (microstep or full step)
gcode_interpreter(file, list_params)
except KeyboardInterrupt:
pass
if __name__=='__main__':
main()