zleveler/zprobe.py at master · 3DSTB/zleveler · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
#!/usr/bin/python3

# virtual serial port to the printer
# socat  pty,link=/dev/ttyACM0,raw  tcp:192.168.1.60:23
# ls -al /dev/ttyACM0
# rwxrwxrwx 1 root root 10 Sij  6 22:32 /dev/ttyACM0 -> /dev/pts/6
# chown your_username:your_username /dev/pts/6

import re, math
import numpy
import scipy
import scipy.spatial
import scipy.interpolate
import matplotlib
import matplotlib.pyplot

import inspect
import sys
import requests

import getopt
import os.path

import serial
import time

zhigh_nl = 5.0 # travel to newline Y with this high
zhigh = 1.0 # travel to next Y with this high
zstart = 0.4
zlow = -0.4
zstep = -0.05
repeat = 3 # repeat measurement N times, averaging

#zstart = 0.5
#zlow = -0.5
#zstep = -0.1
delay0 = 10.0 # s to initially setup
delaynl = 4.0 # s newline delay
zdelay1 = 0.7 # s initial delay for z to move to first point
zdelay = 0.01 # s delay for small read endstop status
serdelay = 0.2 # s serial delay to read response

xmin=0
xmax=95.001
xstep=5
xoffset=1 # added to x

ymax=95
ymin=-0.001
ystep=-5
yoffset=2 # added to y

zoffset=-0.20 # added to z

def gcode_html(printer, cmd):
  r = requests.get("http://" + printer + "/set?code=" + cmd)
  return r.text

def gcode(printer, cmd):
  r = printer.write(cmd.encode('ascii')+b"\r")
  time.sleep(serdelay)
  response = printer.read(1)
  response += printer.read(printer.inWaiting())
  return response

printer_port="/dev/ttyACM0"
printer_fd=serial.Serial(printer_port, 115200, timeout=1)
print(printer_fd.name)
f = printer_fd

az  = {} # empty associative Z-level array
an  = {} # empty associative count array

output_filename="zlevel.xyz"
output_fd=open(os.path.expanduser(output_filename), "w")
zlevel = output_fd

if 1 == 1:
    gcode(f,"G90; absolute positioning")
    gcode(f,"M203 Z2; fast Z feedrate 2 mm/s (120 mm/s)")
    gcode(f,"M140 S57; heat bed to 57'C")
    gcode(f,"G28 X Y; go home XY but not Z")
    gcode(f,"G0 X50 Y50; go to center of the bed")
    gcode(f,"G28 Z; go home Z now at center of the bed")
    gcode(f,"M119; read endstop status")
    gcode(f,"G0 Z%.2f; safety lift Z above" % zhigh_nl)
    time.sleep(delay0)

# with output_fd as zlevel:
n = 0
while n < repeat:
    x = xmin
    while x <= xmax:
      y = ymax
      while y >= ymin:
        # print("X%.2f Y%.2f " % (x,y))
        gcode(f, "G0 X%.2f Y%0.2f" % (x,y))
        if y == ymax:
          time.sleep(delaynl)
        if 1 == 1: # the measurement
          z = zstart # set to start of probing point
          zswitch = 0
          while z > zlow and zswitch == 0:
            gcode(f, "G0 Z%.2f" % z)
            # print("Z%.2f" % z)
            if z == zstart:
              time.sleep(zdelay1)
            else:
              time.sleep(zdelay)
            endstop=gcode(f,"M119")
            # print(endstop)
            if b"z_stop: TRIGGERED" in endstop or b"z_min:H" in endstop:
              zswitch = 1
              # fill associative array
              aindex = "X%.2f Y%.2f" % (x+xoffset,y+yoffset)
              if aindex in an:
                az[aindex] += z+zoffset
                an[aindex] += 1
              else:
                az[aindex] = z+zoffset
                an[aindex] = 1
              print("%s Z%.2f # avg=%.2f n=%d" % (aindex,z+zoffset,az[aindex]/an[aindex],an[aindex]))
            z += zstep
        y += ystep
        if y >= ymin:
          gcode(f, "G0 Z%.2f" % zhigh) # small lift in the same Y line
        else:
          gcode(f, "G0 Z%.2f" % zhigh_nl) # big lift, newline will follow
        time.sleep(zdelay1)
      x += xstep
      # after each completed Y-line, write Z-level file
      output_fd=open(os.path.expanduser(output_filename), "w")
      with output_fd as zlevel:
        for index in sorted(an):
          zlevel.write("%s Z%.2f\n" % (index,az[index]/an[index]))
        zlevel.close()
    n += 1

if 1 == 1: # leaving at center of hotbed
    gcode(f,"G0 X50 Y50; go to center of the bed")
    gcode(f,"M140 S0; heat bed off")
    gcode(f,"G92;relative positioning")
    gcode(f,"M84;motors off")

print("zprobe completed. '%s' written." % output_filename)