kopia lustrzana https://github.com/adliechty/realWorldGcodeSender
1450 wiersze
58 KiB
Python
1450 wiersze
58 KiB
Python
#TODO: use interpolateCornersCharuco to get better accuracy on corner detection
|
|
|
|
# import the necessary packages
|
|
import numpy as np
|
|
import argparse
|
|
import cv2
|
|
import time
|
|
import math
|
|
from svgpathtools import Path, Line, QuadraticBezier, CubicBezier, Arc
|
|
from svgpathtools import svg2paths, wsvg, svg2paths2, polyline
|
|
#import matplotlib
|
|
#matplotlib.use('GTK3Agg')
|
|
from matplotlib import pyplot as plt
|
|
from matplotlib.widgets import TextBox
|
|
from matplotlib.backend_bases import MouseButton
|
|
from copy import deepcopy
|
|
from pygcode import Machine, GCodeRapidMove, GCodeFeedRate, GCodeLinearMove, GCodeUseMillimeters
|
|
import pygcode
|
|
from pygcode.gcodes import MODAL_GROUP_MAP
|
|
import re
|
|
|
|
import sys
|
|
#sys.path.insert(1, 'C:\\Git\\gerbil\\')
|
|
#sys.path.insert(1, 'C:\\Git\\gcode_machine\\')
|
|
sys.path.insert(1, '../svgToGCode/')
|
|
from svgToGCode import cncPathsClass
|
|
from svgToGCode import cncGcodeGeneratorClass
|
|
from svgToGCode import Point3D
|
|
from svgToGCode import signedArea
|
|
|
|
from gerbil import Gerbil
|
|
import serial.tools.list_ports
|
|
|
|
import threading
|
|
import functools
|
|
|
|
|
|
global boxWidth
|
|
global rightBoxRef
|
|
global leftBoxRef
|
|
global bedSize
|
|
global bedViewSizePixels
|
|
global rightSlope
|
|
global leftSlope
|
|
global materialThickness
|
|
global cutterDiameter
|
|
|
|
materialThickness = 0.471
|
|
cutterDiameter = 0.125
|
|
|
|
|
|
#right side, 2.3975" from bed, 2.38" near wall. 0.326 from end of bed
|
|
#left side 3.2" from bed. 3.1375 near wall. 0.3145" from end of bed
|
|
#5.2195" / 7 = 0.745643" per box
|
|
|
|
boxWidth = 0.745642857 * 1.01
|
|
bedSize = Point3D(-35.0, -35.0, -3.75)
|
|
#These are distances from machine origin (0,0,0), right, back, upper corner.
|
|
rightBoxRef = Point3D(4.0-.17, -34.0-0.2, bedSize.Z + 2.3975 - materialThickness)
|
|
leftBoxRef = Point3D(-39.0-.17, -34.0-0.2, bedSize.Z + 3.2 - materialThickness)
|
|
|
|
#This is the height of the bottom box from the bed at the far end (near Y 0)
|
|
#as the reference squares may not be perfectly level to the bed
|
|
rightBoxFarHeight = 2.38 - materialThickness
|
|
leftBoxFarHeight = 3.1375 - materialThickness
|
|
|
|
#There are 20 boxes, slope is divided by 20
|
|
rightSlope = (rightBoxFarHeight - (rightBoxRef.Z - bedSize.Z)) / 20.0
|
|
leftSlope = (leftBoxFarHeight - (leftBoxRef.Z - bedSize.Z)) / 20.0
|
|
|
|
bedViewSizePixels = 1400
|
|
|
|
#First ID is upper right, which is most positive Z and most positice Y
|
|
# Z, Y
|
|
global idToLocDict
|
|
idToLocDict = {0 :[2,21],
|
|
1 :[2,19],
|
|
2 :[2,17],
|
|
3 :[2,16],
|
|
4 :[2,13],
|
|
5 :[2,11],
|
|
6 :[2, 9],
|
|
7 :[2, 7],
|
|
8 :[2, 5],
|
|
9 :[2, 3],
|
|
10:[2, 1],
|
|
11:[1, 20],
|
|
12:[1, 18],
|
|
13:[1, 16],
|
|
14:[1, 14],
|
|
15:[1, 12],
|
|
16:[1, 10],
|
|
17:[1, 8],
|
|
18:[1, 6],
|
|
19:[1, 4],
|
|
20:[1, 2],
|
|
21:[1, 0],
|
|
22:[0, 21],
|
|
23:[0, 19],
|
|
24:[0, 17],
|
|
25:[0, 15],
|
|
26:[0, 13],
|
|
27:[0, 11],
|
|
28:[0, 9],
|
|
29:[0, 7],
|
|
30:[0, 5],
|
|
31:[0, 3],
|
|
32:[0, 1],
|
|
33:[0, 20],
|
|
34:[0, 18],
|
|
35:[0, 16],
|
|
36:[0, 14],
|
|
37:[0, 12],
|
|
38:[0, 10],
|
|
39:[0, 8],
|
|
40:[0, 6],
|
|
41:[0, 4],
|
|
42:[0, 2],
|
|
43:[0, 0],
|
|
44:[1, 21],
|
|
45:[1, 19],
|
|
46:[1, 17],
|
|
47:[1, 15],
|
|
48:[1, 13],
|
|
49:[1, 11],
|
|
50:[1, 9],
|
|
51:[1, 7],
|
|
52:[1, 5],
|
|
53:[1, 3],
|
|
54:[1, 1],
|
|
55:[2, 20],
|
|
56:[2, 18],
|
|
57:[2, 16],
|
|
58:[2, 14],
|
|
59:[2, 12],
|
|
60:[2, 10],
|
|
61:[2, 8],
|
|
62:[2, 6],
|
|
63:[2, 4],
|
|
64:[2, 2],
|
|
65:[2, 0]}
|
|
|
|
|
|
####################################################################################
|
|
# Should put these in a shared libary
|
|
####################################################################################
|
|
def distanceXY(p1, p2):
|
|
return ((p1.X - p2.X)**2 + (p1.Y - p2.Y)**2)**0.5
|
|
|
|
|
|
def lineOrCurveToPoints3D(lineOrCurve, pointsPerCurve):
|
|
if isinstance(lineOrCurve,Line):
|
|
#print(lineOrCurve)
|
|
return [Point3D(lineOrCurve.bpoints()[0].real, lineOrCurve.bpoints()[0].imag), \
|
|
Point3D(lineOrCurve.bpoints()[1].real, lineOrCurve.bpoints()[1].imag)]
|
|
elif isinstance(lineOrCurve, CubicBezier):
|
|
points3D = []
|
|
for i in range(int(pointsPerCurve)):
|
|
complexPoint = lineOrCurve.point(i / (pointsPerCurve - 1.0))
|
|
points3D.append(Point3D(complexPoint.real, complexPoint.imag, None))
|
|
return points3D
|
|
elif isinstance(lineOrCurve, Arc):
|
|
points3D = []
|
|
for i in range(int(pointsPerCurve) * 10):
|
|
complexPoint = lineOrCurve.point(i / (pointsPerCurve * 10 - 1.0))
|
|
points3D.append(Point3D(complexPoint.real, complexPoint.imag, None))
|
|
return points3D
|
|
|
|
else:
|
|
print("unsuported type: " + str(lineOrCurve))
|
|
quit()
|
|
|
|
def pathToPoints3D(path, pointsPerCurve):
|
|
prevEnd = None
|
|
points3D = []
|
|
for lineOrCurve in path:
|
|
curPoints3D = lineOrCurveToPoints3D(lineOrCurve, pointsPerCurve)
|
|
#check that the last line ending starts the beginning of the next line.
|
|
#print(curPoints3D)
|
|
if prevEnd != None and distanceXY(curPoints3D[0], prevEnd) > 0.01:
|
|
print(curPoints3D[0])
|
|
print(prevEnd)
|
|
print("A SVG path must be contiguous, one line ending and beginning on the next line. Make a seperate path out of non contiguous lines")
|
|
quit()
|
|
elif prevEnd == None:
|
|
#first line, store both beginning point and end point
|
|
points3D.extend(curPoints3D)
|
|
else:
|
|
#add to point list except first one as it was verified to be same as ending of last
|
|
points3D.extend(curPoints3D[1:])
|
|
prevEnd = curPoints3D[-1]
|
|
return points3D
|
|
|
|
def centers(x1, y1, x2, y2, r):
|
|
q = ((x2 - x1) ** 2 + (y2 - y1) ** 2) ** 0.5
|
|
x3 = (x1 + x2) / 2
|
|
y3 = (y1 + y2) / 2
|
|
|
|
xx = (r ** 2 - (q / 2) ** 2) ** 0.5 * (y1 - y2) / q
|
|
yy = (r ** 2 - (q / 2) ** 2) ** 0.5 * (x2 - x1) / q
|
|
return ((x3 + xx, y3 + yy), (x3 - xx, y3 - yy))
|
|
|
|
def define_circle(p1, p2, p3):
|
|
"""
|
|
Returns the center and radius of the circle passing the given 3 points.
|
|
In case the 3 points form a line, returns (None, infinity).
|
|
"""
|
|
temp = p2[0] * p2[0] + p2[1] * p2[1]
|
|
bc = (p1[0] * p1[0] + p1[1] * p1[1] - temp) / 2
|
|
cd = (temp - p3[0] * p3[0] - p3[1] * p3[1]) / 2
|
|
det = (p1[0] - p2[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p2[1])
|
|
|
|
if abs(det) < 1.0e-6:
|
|
return (None, np.inf)
|
|
|
|
# Center of circle
|
|
cx = (bc*(p2[1] - p3[1]) - cd*(p1[1] - p2[1])) / det
|
|
cy = ((p1[0] - p2[0]) * cd - (p2[0] - p3[0]) * bc) / det
|
|
|
|
radius = np.sqrt((cx - p1[0])**2 + (cy - p1[1])**2)
|
|
return ((cx, cy), radius)
|
|
|
|
def arcToPoints2(startX, startY, endX, endY, midX, midY):
|
|
points = []
|
|
(centerX, centerY), radius = define_circle((startX, startY), (midX, midY), (endX, endY))
|
|
startAngle = math.atan2(startY - centerY, startX - centerX)
|
|
endAngle = math.atan2(endY - centerY, endX - centerX)
|
|
counterClockWise = 1
|
|
if signedArea([Point3D(startX, startY), Point3D(midX, midY), Point3D(endX, endY)]) < 0:
|
|
counterClockWise = -1
|
|
#for angle in np.arange(startAngle, endAngle, (clockWise * -2 + 1) * 0.1):
|
|
angle = startAngle
|
|
stop = False
|
|
while stop == False:
|
|
x = math.cos(angle) * radius + centerX
|
|
y = math.sin(angle) * radius + centerY
|
|
points.append(Point3D(x, y))
|
|
prevAngle = angle
|
|
angle = (angle + counterClockWise * 0.1) % (2 * math.pi)
|
|
angleDiff = ((endAngle - angle + 3 * math.pi) % (2 * math.pi)) - math.pi
|
|
#time.sleep(0.05)
|
|
#print(angleDiff)
|
|
if counterClockWise == -1:
|
|
stop = angleDiff <= 0.1 and angleDiff >=0
|
|
else:
|
|
stop = angleDiff >= -0.1 and angleDiff <= 0
|
|
|
|
x = math.cos(endAngle) * radius + centerX
|
|
y = math.sin(endAngle) * radius + centerY
|
|
points.append(Point3D(x, y))
|
|
return points
|
|
|
|
def arcToPoints(startX, startY, endX, endY, i, j, clockWise, curZ):
|
|
points = []
|
|
centerX = startX + i
|
|
centerY = startY + j
|
|
radius = math.dist([centerX, centerY], [startX, startY])
|
|
startAngle = math.atan2(startY - centerY, startX - centerX)
|
|
endAngle = math.atan2(endY - centerY, endX - centerX)
|
|
for angle in np.arange(startAngle, endAngle, (clockWise * -2 + 1) * 0.1):
|
|
x = math.cos(angle) * radius + centerX
|
|
y = math.sin(angle) * radius + centerY
|
|
points.append(Point3D(x, y, curZ))
|
|
x = math.cos(endAngle) * radius + centerX
|
|
y = math.sin(endAngle) * radius + centerY
|
|
points.append(Point3D(x, y, curZ))
|
|
return points
|
|
|
|
def rotate(origin, point, angle):
|
|
"""
|
|
Rotate a point counterclockwise by a given angle around a given origin.
|
|
|
|
The angle should be given in radians.
|
|
"""
|
|
ox, oy = origin
|
|
px, py = point
|
|
|
|
qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
|
|
qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
|
|
return qx, qy
|
|
|
|
####################################################################################
|
|
# OverlayGcode class
|
|
####################################################################################
|
|
class OverlayGcode:
|
|
def __init__(self, cv2Overhead, gCodeFile = None, svgFile = None, enableSender = True):
|
|
global bedViewSizePixels
|
|
global bedSize
|
|
|
|
self.bedViewSizePixels = bedViewSizePixels
|
|
self.bedSize = bedSize
|
|
self.xOffset = 0
|
|
self.yOffset = 0
|
|
self.rotation = 0
|
|
self.cv2Overhead = cv2.cvtColor(cv2Overhead, cv2.COLOR_BGR2RGB)
|
|
self.move = False
|
|
self.previewNextDrawnPoint = False
|
|
self.mouseX = 0
|
|
self.mouseY = 0
|
|
self.camRefCenter = [0,0]
|
|
|
|
self.startArc = None
|
|
self.endArc = None
|
|
|
|
self.refPlateMeasuredLoc = [0.0, 0.0]
|
|
self.camRefCenter = [0.0, 0.0]
|
|
|
|
fig, ax = plt.subplots()
|
|
fig.tight_layout()
|
|
plt.subplots_adjust(bottom=0.01, right = 0.99)
|
|
plt.axis([self.bedViewSizePixels,0, self.bedViewSizePixels, 0])
|
|
plt.rcParams['keymap.back'].remove('c') # we use c for circle
|
|
plt.rcParams['keymap.save'].remove('s') # we use s for send
|
|
plt.rcParams['keymap.pan'].remove('p') # we use s for send
|
|
#Generate matplotlib plot from opencv image
|
|
self.matPlotImage = plt.imshow(self.cv2Overhead)
|
|
###############################################
|
|
# Generate controls for plot
|
|
###############################################
|
|
#xAxes = plt.axes([0.01, 0.8, 0.2, 0.04])
|
|
#self.xBox = TextBox(xAxes, "xOffset (in)", initial="0")
|
|
#label = self.xBox.ax.get_children()[1] # label is a child of the TextBox axis
|
|
#label.set_position([0.5,1]) # [x,y] - change here to set the position
|
|
#label.set_horizontalalignment('center')
|
|
#label.set_verticalalignment('bottom')
|
|
#self.xBox.on_submit(self.onUpdateXOffset)
|
|
|
|
#yAxes = plt.axes([0.01, 0.7, 0.2, 0.04])
|
|
#self.yBox = TextBox(yAxes, "yOffset (in)", initial="0")
|
|
#label = self.yBox.ax.get_children()[1] # label is a child of the TextBox axis
|
|
#label.set_position([0.5,1]) # [x,y] - change here to set the position
|
|
#label.set_horizontalalignment('center')
|
|
#label.set_verticalalignment('bottom')
|
|
#self.yBox.on_submit(self.onUpdateYOffset)
|
|
|
|
#rAxes = plt.axes([0.01, 0.6, 0.2, 0.04])
|
|
#self.rBox = TextBox(rAxes, "rotation (deg)", initial="0")
|
|
#label = self.rBox.ax.get_children()[1] # label is a child of the TextBox axis
|
|
#label.set_position([0.5,1]) # [x,y] - change here to set the position
|
|
#label.set_horizontalalignment('center')
|
|
#label.set_verticalalignment('bottom')
|
|
#self.rBox.on_submit(self.onUpdateRotation)
|
|
|
|
cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
|
|
cid = fig.canvas.mpl_connect('button_release_event', self.onrelease)
|
|
cid = fig.canvas.mpl_connect('motion_notify_event', self.onmousemove)
|
|
cid = fig.canvas.mpl_connect('key_press_event', self.onkeypress)
|
|
|
|
#Create object to handle controlling the CNC machine and sending the g codes to it
|
|
self.gCodeFile = gCodeFile
|
|
if enableSender:
|
|
self.sender = GCodeSender()
|
|
|
|
|
|
self.points = []
|
|
self.drawnPoints = []
|
|
self.laserPowers = []
|
|
self.machine = Machine()
|
|
|
|
|
|
############################################################################
|
|
# If generating cut paths from an SVG File
|
|
############################################################################
|
|
self.svgFile = svgFile
|
|
if svgFile != None:
|
|
#Generate cncPaths object based on svgFile
|
|
#These are in mm
|
|
self.cncPaths = cncPathsClass(inputSvgFile = svgFile,
|
|
pointsPerCurve = 30,
|
|
distPerTab = 7.87,
|
|
tabWidth = 0.25,
|
|
cutterDiameter = cutterDiameter,
|
|
convertSvfToIn = True
|
|
)
|
|
|
|
#Order cuts from inside holes to outside borders
|
|
self.cncPaths.orderCncHolePathsFirst()
|
|
self.cncPaths.orderPartialCutsFirst()
|
|
|
|
self.pathIndex = -1
|
|
self.pathOffsets = []
|
|
for path in self.cncPaths.cncPaths:
|
|
self.pathOffsets.append([0.0, 0.0])
|
|
|
|
|
|
elif gCodeFile != None:
|
|
with open(gCodeFile, 'r') as fh:
|
|
for line_text in fh.readlines():
|
|
line = pygcode.Line(line_text)
|
|
prevPos = self.machine.pos
|
|
self.machine.process_block(line.block)
|
|
|
|
######################################
|
|
# First determine machine motion mode and power
|
|
######################################
|
|
motion = str(self.machine.mode.modal_groups[MODAL_GROUP_MAP['motion']])
|
|
sCode = str(self.machine.mode.modal_groups[MODAL_GROUP_MAP['spindle_speed']])
|
|
power = sCode.split('S')[1]
|
|
#Make rapid movements 0 laser power
|
|
if motion == "G00" or motion == "G0":
|
|
self.laserPowers.append(0.0)
|
|
else:
|
|
self.laserPowers.append(float(power) / 100.0)
|
|
|
|
######################################
|
|
# Determine machine current unit, convert to inches
|
|
######################################
|
|
unit = str(self.machine.mode.modal_groups[MODAL_GROUP_MAP['units']])
|
|
x = None
|
|
if unit == "G20":
|
|
if motion == "G02" or motion == "G2" or motion == "G03" or motion == "G3":
|
|
beforeComment = line_text.split("(")[0]
|
|
resultX = re.search('X[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultY = re.search('Y[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultI = re.search('I[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultJ = re.search('J[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
if resultX != None:
|
|
x = float(resultX.group()[1:])
|
|
y = float(resultY.group()[1:])
|
|
i = float(resultI.group()[1:])
|
|
j = float(resultJ.group()[1:])
|
|
clockWise = "G02" in motion or "G2" in motion
|
|
else:
|
|
self.points.append(Point3D(self.machine.pos.X, self.machine.pos.Y, self.machine.pos.Z))
|
|
else:
|
|
if motion == "G02" or motion == "G2" or motion == "G03" or motion == "G3":
|
|
resultX = re.search('X[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultY = re.search('Y[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultI = re.search('I[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
resultJ = re.search('J[+-]?([0-9]*[.])?[0-9]+', beforeComment)
|
|
if resultX != None:
|
|
x = float(resultX.group()[1:]) / 25.4
|
|
y = float(resultY.group()[1:]) / 25.4
|
|
i = float(resultI.group()[1:]) / 25.4
|
|
j = float(resultJ.group()[1:]) / 25.4
|
|
clockWise = "G02" in motion or "G2" in motion
|
|
else:
|
|
self.points.append(Point3D(self.machine.pos.X / 25.4, self.machine.pos.Y / 25.4, self.machine.pos.Z / 25.4))
|
|
if x != None:
|
|
self.points.extend(arcToPoints(prevPos.X, prevPos.Y, self.machine.pos.X, self.machine.pos.Y, i, j, clockWise, self.machine.pos.Z))
|
|
self.laserPowers.extend([self.laserPowers[-1]] * (len(self.points) - len(self.laserPowers)))
|
|
else:
|
|
print("Must use either svg or gCode file")
|
|
quit()
|
|
|
|
|
|
self.updateOverlay()
|
|
|
|
def set_ref_loc(self, refPixels):
|
|
print("refPixels: " + str(refPixels))
|
|
|
|
refPoints = []
|
|
for refPixel in refPixels:
|
|
refPoints.append( self._pixel_to_inches(refPixel[0], refPixel[1]))
|
|
self.refPoints = refPoints
|
|
|
|
avgX = (refPoints[0][0] + refPoints[1][0] + refPoints[2][0] + refPoints[3][0]) / 4.0
|
|
avgY = (refPoints[0][1] + refPoints[1][1] + refPoints[2][1] + refPoints[3][1]) / 4.0
|
|
self.camRefCenter = [avgX, avgY]
|
|
# set measured ref plate location whenever reference plat is moved
|
|
self.refPlateMeasuredLoc = self.camRefCenter + [0]
|
|
print("Ref Points = " + str(refPoints))
|
|
angle = getBoxAngle(refPoints)
|
|
print("Angle: " + str(angle * 180 / math.pi))
|
|
|
|
|
|
def phyPointsToPixels(self, transformedPoints):
|
|
global rightBoxRef, leftBoxRef
|
|
|
|
# Bed drawn from Y = 0 to Y = 35, but from X at left support beam and right support beam with ref boxes on them.
|
|
self.offsetPoints(transformedPoints, -rightBoxRef.X, 0)
|
|
self.scalePoints(transformedPoints, \
|
|
self.bedViewSizePixels / (leftBoxRef.X - rightBoxRef.X), \
|
|
self.bedViewSizePixels / self.bedSize.Y)
|
|
|
|
def scalePoints(self, points, scaleX, scaleY):
|
|
for point in points:
|
|
point.X = point.X * scaleX
|
|
point.Y = point.Y * scaleY
|
|
|
|
def offsetPoints(self, points, X, Y):
|
|
for point in points:
|
|
point.X = point.X + X
|
|
point.Y = point.Y + Y
|
|
|
|
def rotatePoints(self, points, origin, angle):
|
|
for point in points:
|
|
point.X, point.Y = rotate(origin, [point.X, point.Y], angle)
|
|
|
|
def overlaySvgOrGcode(self, image, xOff = 0, yOff = 0, rotation = 0):
|
|
"""
|
|
image is opencv image
|
|
xOff is in inches
|
|
yOff is in inches
|
|
rotation is in degrees
|
|
"""
|
|
global cutterDiameter
|
|
toolWidth = round(abs(cutterDiameter * self.bedViewSizePixels / (leftBoxRef.X - rightBoxRef.X)))
|
|
#convert to radians
|
|
rotation = rotation * math.pi / 180
|
|
overlay = image.copy()
|
|
|
|
if self.gCodeFile != None:
|
|
#Make copy of points before transforming them
|
|
transformedPoints = deepcopy(self.points)
|
|
self.offsetPoints(transformedPoints, xOff, yOff)
|
|
self.rotatePoints(transformedPoints, [xOff, yOff], rotation)
|
|
else:
|
|
transformedPoints = []
|
|
self.laserPowers = []
|
|
for cncPath, offset in zip(self.cncPaths.cncPaths, self.pathOffsets):
|
|
newPoints = deepcopy(cncPath.points3D)
|
|
|
|
self.offsetPoints(newPoints, offset[0] , offset[1])
|
|
transformedPoints.extend(newPoints)
|
|
if cncPath.color[1] == 0:
|
|
self.laserPowers.extend([0] + [1] * (len(cncPath.points3D) - 1))
|
|
else:
|
|
self.laserPowers.extend([cncPath.color[1] / 255] * len(cncPath.points3D))
|
|
self.rotatePoints(transformedPoints, [offset[0], offset[1]], rotation)
|
|
|
|
#Then convert to pixel location
|
|
self.phyPointsToPixels(transformedPoints)
|
|
prevPoint = None
|
|
for point, laserPower in zip(transformedPoints, self.laserPowers):
|
|
newPoint = (int(point.X), int(point.Y))
|
|
if prevPoint is not None:
|
|
cv2.line(overlay, prevPoint, newPoint, (int(laserPower * 255), 0, 0), toolWidth)
|
|
prevPoint = newPoint
|
|
|
|
transformedPoints = deepcopy(self.drawnPoints)
|
|
self.phyPointsToPixels(transformedPoints)
|
|
prevPoint = None
|
|
for point in transformedPoints:
|
|
newPoint = (int(point.X), int(point.Y))
|
|
if prevPoint is not None:
|
|
cv2.line(overlay, prevPoint, newPoint, (0, 255, 0), toolWidth)
|
|
prevPoint = newPoint
|
|
return overlay
|
|
|
|
def updateOverlay(self):
|
|
overlay = self.overlaySvgOrGcode(self.cv2Overhead, self.xOffset, self.yOffset, self.rotation)
|
|
self.matPlotImage.set_data(overlay)
|
|
self.matPlotImage.figure.canvas.draw()
|
|
|
|
def onUpdateXOffset(self, text):
|
|
if self.xOffset == float(text):
|
|
return
|
|
self.xOffset = float(text)
|
|
self.updateOverlay()
|
|
|
|
def onUpdateYOffset(self, text):
|
|
if self.yOffset == float(text):
|
|
return
|
|
self.yOffset = float(text)
|
|
self.updateOverlay()
|
|
|
|
def onUpdateRotation(self, text):
|
|
if self.rotation == float(text):
|
|
return
|
|
self.rotation = float(text)
|
|
self.updateOverlay()
|
|
|
|
def onmousemove(self, event):
|
|
self.move = True
|
|
self.mouseX = event.xdata
|
|
self.mouseY = event.ydata
|
|
if self.previewNextDrawnPoint:
|
|
# if finishing drawing an arch, then preview that instead of a line
|
|
if self.startArc != None and self.endArc != None:
|
|
x, y = self._mouse_pos_inches()
|
|
radius = math.dist([x,y], [self.startArc.X, self.startArc.Y])
|
|
newPoints = arcToPoints2(self.startArc.X, self.startArc.Y, self.endArc.X, self.endArc.Y, x, y)
|
|
print(newPoints)
|
|
#newPoints = arcToPoints(self.startArc.X, self.startArc.Y, self.endArc.X, self.endArc.Y, x - self.startArc.X, y - self.startArc.Y, False, 0)
|
|
numNewPoints = len(newPoints)
|
|
self.drawnPoints.extend(newPoints)
|
|
self.updateOverlay()
|
|
#remove added points as this is just a preview
|
|
del self.drawnPoints[-numNewPoints:]
|
|
|
|
|
|
else:
|
|
x, y = self._mouse_pos_inches()
|
|
#provision to preview first point
|
|
firstPoint = False
|
|
if len(self.drawnPoints) == 0:
|
|
self.drawnPoints.append(Point3D(x, y, 0))
|
|
firstPoint = True
|
|
self.drawnPoints.append(Point3D(x, y, 0))
|
|
self.updateOverlay()
|
|
self.drawnPoints.pop()
|
|
if firstPoint:
|
|
self.drawnPoints.pop()
|
|
|
|
def _pixel_to_inches(self, x, y):
|
|
x = x / self.bedViewSizePixels * (leftBoxRef.X - rightBoxRef.X)
|
|
x = x + rightBoxRef.X
|
|
y = y / self.bedViewSizePixels * self.bedSize.Y
|
|
# Make reference plate measured center location in image be exact measured location
|
|
x = x - self.camRefCenter[0] + self.refPlateMeasuredLoc[0]
|
|
y = y - self.camRefCenter[1] + self.refPlateMeasuredLoc[1]
|
|
return x, y
|
|
def _mouse_pos_inches(self):
|
|
print(self.mouseX)
|
|
print(self.bedViewSizePixels)
|
|
return self._pixel_to_inches(self.mouseX, self.mouseY)
|
|
|
|
def onkeypress(self, event):
|
|
x, y = self._mouse_pos_inches()
|
|
print(event.key)
|
|
# if sending a g code file
|
|
if event.key == 'g':
|
|
self.sender.send_file(self.gCodeFile, self.xOffset, self.yOffset, self.rotation)
|
|
# if sending an svf file
|
|
elif event.key == 's':
|
|
# perform transfomrations that were done in GUI on actual points in the path
|
|
rotation = self.rotation * math.pi / 180
|
|
for path, offset in zip(self.cncPaths.cncPaths, self.pathOffsets):
|
|
print("before: " + str(path.points3D[0]) + " offset: " + str(offset))
|
|
self.offsetPoints(path.points3D, offset[0] , offset[1])
|
|
self.rotatePoints(path.points3D, [self.pathOffsets[-1][0], self.pathOffsets[-1][1]], rotation)
|
|
print("after: " + str(path.points3D[0]))
|
|
|
|
# Before sending add tabs
|
|
#self.cncPaths.addTabs()
|
|
#self.cncPaths.ModifyPointsFromTabLocations()
|
|
|
|
self.sender.send_svf(self.cncPaths)
|
|
elif event.key == 'n':
|
|
self.pathIndex = self.pathIndex + 1
|
|
elif event.key == 'p':
|
|
self.pathIndex = self.pathIndex - 1
|
|
|
|
elif event.key == 'h':
|
|
self.sender.home_machine()
|
|
|
|
elif event.key == 'z':
|
|
#Find X, Y, and Z position of the aluminum reference block on the work piece
|
|
#sepcify the X and Y estimated position of the reference block
|
|
#self.refPlateMeasuredLoc = self.sender.zero_on_refPlate(self.refPoints)
|
|
#Just probe z height for now for demo
|
|
self.sender.zero_on_refPlate(self.refPoints, True)
|
|
print("refPlateMeasuredLoc: " + str(self.refPlateMeasuredLoc))
|
|
print("camRefCenter: " + str(self.camRefCenter))
|
|
|
|
elif event.key == 'm':
|
|
self.sender.absolute_move(x, y, feed = 300)
|
|
|
|
elif event.key == 'd':
|
|
# first d turns on preview
|
|
if not self.previewNextDrawnPoint:
|
|
self.previewNextDrawnPoint = True
|
|
return
|
|
self.drawnPoints.append(Point3D(x, y, 0))
|
|
self.updateOverlay()
|
|
print(self.drawnPoints)
|
|
elif event.key == 'c':
|
|
if self.startArc == None:
|
|
self.startArc = self.drawnPoints[-1]#Point3D(x, y)
|
|
self.endArc = Point3D(x, y)
|
|
else:
|
|
print(self.startArc)
|
|
print(self.endArc)
|
|
newPoints = arcToPoints2(self.startArc.X, self.startArc.Y, self.endArc.X, self.endArc.Y, x, y)
|
|
print("newPoints:" + str(newPoints))
|
|
self.drawnPoints.extend(newPoints)
|
|
self.startArc = None
|
|
self.endArc = None
|
|
self.updateOverlay()
|
|
# erase one drawn point
|
|
elif event.key == 'e':
|
|
if len(self.drawnPoints) > 0:
|
|
self.drawnPoints.pop()
|
|
self.updateOverlay()
|
|
# Erase all drawn points
|
|
elif event.key == 'E':
|
|
self.drawnPoints = []
|
|
self.updateOverlay()
|
|
elif event.key == 'C':
|
|
# offset G codes by workspace zero as G codes send relative to workspace offset
|
|
offset = Point3D(-self.refPlateMeasuredLoc[0], \
|
|
-self.refPlateMeasuredLoc[1])
|
|
self.sender.send_drawnPoints(offset, self.drawnPoints)
|
|
elif event.key == 'shift':
|
|
self.shiftHeld = True
|
|
print("shift")
|
|
|
|
# if a non drawing key was pushed then exit drawing preveiw mode
|
|
if event.key != 'd' and event.key.lower() != 'e' and event.key != 'shift' and event.key != 'c':
|
|
if self.previewNextDrawnPoint:
|
|
self.previewNextDrawnPoint = False
|
|
self.updateOverlay()
|
|
if event.key != 'c':
|
|
self.startArc = None
|
|
self.endArc = None
|
|
|
|
|
|
def onclick(self, event):
|
|
self.move = False
|
|
|
|
def onrelease(self, event):
|
|
global matPlotImage
|
|
#If clicking outside region, or mouse moved since released then return
|
|
|
|
if event.x < 260 or self.move == True:
|
|
return
|
|
pixelsToOrigin = np.array([event.xdata, event.ydata])
|
|
print("event x,y: " + str(pixelsToOrigin))
|
|
print("mouse x,y: " + str([self.mouseX, self.mouseY]))
|
|
xIn, yIn = self._mouse_pos_inches()
|
|
if event.button == MouseButton.RIGHT:
|
|
#xIn = pixelsToOrigin[0] / self.bedViewSizePixels * self.bedSize.X
|
|
#yIn = pixelsToOrigin[1] / self.bedViewSizePixels * self.bedSize.Y
|
|
self.rotation = math.atan2(yIn - self.yOffset, xIn - self.xOffset)
|
|
self.rotation = self.rotation - math.pi/2.0
|
|
self.rotation = self.rotation * 180 / math.pi
|
|
|
|
else:
|
|
self.xOffset = xIn
|
|
self.yOffset = yIn
|
|
print("xin, yIn: " + str(xIn) + "," + str(yIn))
|
|
print(str(pixelsToOrigin[0] / self.bedViewSizePixels * self.bedSize.X) + "," + \
|
|
str(pixelsToOrigin[1] / self.bedViewSizePixels * self.bedSize.Y))
|
|
#self.xOffset = pixelsToOrigin[0] / self.bedViewSizePixels * self.bedSize.X
|
|
#self.yOffset = pixelsToOrigin[1] / self.bedViewSizePixels * self.bedSize.Y
|
|
# if negative 1 then apply offset to all paths, else just selected path
|
|
if self.pathIndex == -1:
|
|
i = 0
|
|
for path in self.cncPaths.cncPaths:
|
|
minX = 1000000000
|
|
minY = 1000000000
|
|
for point in path.points3D:
|
|
minX = min(minX, point.X)
|
|
minY = min(minY, point.Y)
|
|
|
|
self.pathOffsets[i] = [xIn, yIn]
|
|
i = i + 1
|
|
else:
|
|
minX = 1000000000
|
|
minY = 1000000000
|
|
for point in self.cncPaths.cncPaths[self.pathIndex].points3D:
|
|
minX = min(minX, point.X)
|
|
minY = min(minY, point.Y)
|
|
self.pathOffsets[self.pathIndex] = [xIn - minX, yIn - minY]
|
|
self.updateOverlay()
|
|
self.xBox.set_val(str(self.xOffset))
|
|
self.yBox.set_val(str(self.yOffset))
|
|
self.rBox.set_val(str(self.rotation))
|
|
|
|
def crop_half_vertically(img):
|
|
#cropped_img = image[,int(image.shape[1]/2):int(image.shape[1])]
|
|
#height = img.shape[0]
|
|
width = img.shape[1]
|
|
# Cut the image in half
|
|
width_cutoff = int(width // 2)
|
|
left = img[:, :width_cutoff]
|
|
right = img[:, width_cutoff:]
|
|
return left, right
|
|
|
|
def getBoxAngle(points):
|
|
adjacentPoints = []
|
|
maxDistance = 0
|
|
for point in points[1:]:
|
|
maxDistance = max(maxDistance, math.dist(point, points[0]))
|
|
for point in points[1:]:
|
|
# if an adjacent point (not across from box)
|
|
if math.dist(point, points[0]) != maxDistance:
|
|
print("adjacen points:")
|
|
print(points[0])
|
|
print(point)
|
|
angle = math.atan2(point[1] - points[0][1], point[0] - points[0][0])
|
|
break
|
|
print(angle * 180 / math.pi)
|
|
# a square is square, so we will pick one of the 4 angles 0, + 90, +180, or +270
|
|
if angle < math.pi / 4:
|
|
angle = angle + math.pi
|
|
if angle >= math.pi / 4:
|
|
angle = angle - math.pi / 2
|
|
return angle
|
|
|
|
def sortBoxPoints(points, rightSide = True):
|
|
#First sort by X
|
|
sortedX = sorted(points , key=lambda k: [k[0]])
|
|
#Then sorty by Y left and right most two X set of points
|
|
rightTwoPoints = sorted(sortedX[2:], key=lambda k: [k[1]])
|
|
leftTwoPoints = sorted(sortedX[0:2], key=lambda k: [k[1]])
|
|
if rightSide:
|
|
minZminY = leftTwoPoints[1]
|
|
minZmaxY = leftTwoPoints[0]
|
|
maxZmaxY = rightTwoPoints[0]
|
|
maxZminY = rightTwoPoints[1]
|
|
else:
|
|
minZminY = rightTwoPoints[1]
|
|
minZmaxY = rightTwoPoints[0]
|
|
maxZmaxY = leftTwoPoints[0]
|
|
maxZminY = leftTwoPoints[1]
|
|
|
|
return [minZminY, minZmaxY, maxZmaxY, maxZminY]
|
|
def get_id_loc(image, boxes, ids, ID):
|
|
for box, curID in zip(boxes, ids):
|
|
if curID != ID:
|
|
continue
|
|
boxPoints = box[0]
|
|
boxPointsSorted = np.array(sortBoxPoints(boxPoints))
|
|
return boxPointsSorted
|
|
return None
|
|
|
|
def boxes_to_point_and_location_list(boxes, ids, image, rightSide = False):
|
|
global boxWidth
|
|
global idToLocDict
|
|
pointList = []
|
|
locations = []
|
|
for box, ID in zip(boxes, ids):
|
|
#IDs below 33 are on right side, skip those if looking for left side points
|
|
if (rightSide == False and ID < 33) or \
|
|
(rightSide == True and ID >= 33) or \
|
|
(ID > 65):
|
|
continue
|
|
boxLoc = idToLocDict[ID[0]]
|
|
for boxPoints in box:
|
|
prevX = int(boxPoints[0][0])
|
|
prevY = int(boxPoints[0][1])
|
|
i = 0
|
|
|
|
font = cv2.FONT_HERSHEY_SIMPLEX
|
|
bottomLeftCornerOfText = prevX + 100,prevY
|
|
fontScale = 1
|
|
fontColor = (0,255,255)
|
|
thickness = 3
|
|
lineType = 2
|
|
|
|
cv2.putText(image,str(ID[0]),
|
|
bottomLeftCornerOfText,
|
|
font,
|
|
fontScale,
|
|
fontColor,
|
|
thickness,
|
|
lineType)
|
|
|
|
boxPointsSorted = sortBoxPoints(boxPoints, rightSide)
|
|
for point in boxPointsSorted:
|
|
############################################
|
|
# Generate list of points
|
|
############################################
|
|
pointList.append(point)
|
|
|
|
|
|
############################################
|
|
# Generate point location based on boxWidth and index within box
|
|
############################################
|
|
curLoc = [0,0]
|
|
if i == 0:
|
|
curLoc[0] = boxLoc[0] + 0
|
|
curLoc[1] = boxLoc[1] + 0
|
|
elif i ==1:
|
|
curLoc[0] = boxLoc[0] + 0
|
|
curLoc[1] = boxLoc[1] + 1
|
|
elif i == 2:
|
|
curLoc[0] = boxLoc[0] + 1
|
|
curLoc[1] = boxLoc[1] + 1
|
|
else:
|
|
curLoc[0] = boxLoc[0] + 1
|
|
curLoc[1] = boxLoc[1] + 0
|
|
if rightSide:
|
|
curLoc[0] = curLoc[0] * boxWidth + rightBoxRef.Z + curLoc[1] * rightSlope
|
|
curLoc[1] = curLoc[1] * boxWidth + rightBoxRef.Y
|
|
else:
|
|
curLoc[0] = curLoc[0] * boxWidth + leftBoxRef.Z + curLoc[1] * leftSlope
|
|
curLoc[1] = curLoc[1] * boxWidth + leftBoxRef.Y
|
|
locations.append(curLoc)
|
|
|
|
############################################
|
|
# Display points on image
|
|
############################################
|
|
x= int(point[0])
|
|
y= int(point[1])
|
|
image = cv2.arrowedLine(image, (prevX,prevY), (x,y),
|
|
(0,255,255), 3)
|
|
|
|
prevX = x
|
|
prevY = y
|
|
i = i + 1
|
|
|
|
return np.array(pointList), locations, image
|
|
|
|
def generate_dest_locations(corners, image):
|
|
global boxWidth
|
|
prevX=2000
|
|
prevY=2000
|
|
locations = []
|
|
yIndex = 0
|
|
xIndex = 0
|
|
for corner in corners:
|
|
x,y= corner
|
|
x= int(x)
|
|
y= int(y)
|
|
|
|
#cv2.rectangle(gray, (prevX,prevY),(x,y),(i*3,0,0),-1)
|
|
image = cv2.arrowedLine(image, (prevX,prevY), (x,y),
|
|
(200,0,0), 5)
|
|
locations.append([xIndex * boxWidth, yIndex * boxWidth])
|
|
if xIndex == 2:
|
|
xIndex = 0
|
|
yIndex = yIndex + 1
|
|
else:
|
|
xIndex = xIndex + 1
|
|
prevX = x
|
|
prevY = y
|
|
return locations, image
|
|
|
|
def display_4_lines(pixels, frame, flip=False):
|
|
line1 = tuple(pixels[0][0].astype(np.int))
|
|
line2 = tuple(pixels[1][0].astype(np.int))
|
|
if flip:
|
|
line3 = tuple(pixels[3][0].astype(np.int))
|
|
line4 = tuple(pixels[2][0].astype(np.int))
|
|
else:
|
|
line3 = tuple(pixels[2][0].astype(np.int))
|
|
line4 = tuple(pixels[3][0].astype(np.int))
|
|
cv2.line(frame, line1,line2,(0,255,255),3)
|
|
cv2.line(frame, line2,line3,(0,255,255),3)
|
|
cv2.line(frame, line3,line4,(0,255,255),3)
|
|
cv2.line(frame, line4,line1,(0,255,255),3)
|
|
|
|
class GCodeSender:
|
|
def __init__(self):
|
|
self.event = threading.Event()
|
|
self.dataList = []
|
|
self.eventList = []
|
|
|
|
self.gerbil = Gerbil(self.gerbil_callback)
|
|
self.gerbil.setup_logging()
|
|
|
|
ports = serial.tools.list_ports.comports()
|
|
for p in ports:
|
|
print(p.device)
|
|
|
|
self.gerbil.cnect("COM4", 115200)
|
|
self.gerbil.poll_start()
|
|
self.set_inches()
|
|
|
|
self.plateHeight = 0.472441 # 12mm
|
|
self.plateWidth = 2.75591 # 70mm
|
|
global cutterDiameter
|
|
self.cutterRadius = cutterDiameter / 2.0
|
|
self.distToKnotch = (0.984252**2 + 0.984252**2) ** 0.5 #25mm both directions to the knotch
|
|
|
|
|
|
|
|
|
|
def gerbil_callback(self, eventstring, *data):
|
|
args = []
|
|
#if eventstring != 'on_vars_change' and \
|
|
# eventstring != 'on_progress_percent' and \
|
|
# eventstring != 'on_log' and \
|
|
# eventstring != 'on_write' and \
|
|
# eventstring != 'on_line_sent' and \
|
|
# eventstring != 'on_bufsize_change':
|
|
# print("GERBIL CALLBACK: " + eventstring)
|
|
if eventstring != "on_read":
|
|
return
|
|
print()
|
|
print()
|
|
for d in data:
|
|
args.append(str(d))
|
|
print(d)
|
|
print("args event={} data={}".format(eventstring.ljust(30), ", ".join(args)))
|
|
self.curData = data
|
|
self.curEvent = eventstring
|
|
self.dataList.append(data)
|
|
self.eventList.append(eventstring)
|
|
|
|
#indicate callback is done
|
|
self.event.set()
|
|
|
|
def get_absolute_pos(self):
|
|
self.gerbil.send_immediately("?\n")
|
|
resp = self.waitOnGCodeComplete(">")
|
|
m = re.match("<(.*?),MPos:(.*?),WPos:(.*?)>", resp)
|
|
mpos_parts = m.group(2).split(",")
|
|
return (float(mpos_parts[0]), float(mpos_parts[1]), float(mpos_parts[2]))
|
|
|
|
def home_machine(self):
|
|
self.gerbil.send_immediately("$H\n")
|
|
pass
|
|
|
|
def set_work_coord_offset(self, x = None, y = None, z = None):
|
|
xStr, yStr, zStr = self._get_xyz_string(x, y, z)
|
|
self.gerbil.send_immediately("G54 " + xStr + yStr + zStr + "\n")
|
|
self.gerbil.send_immediately("G54\n")
|
|
|
|
def set_cur_pos_as(self, x = None, y = None, z = None):
|
|
xStr, yStr, zStr = self._get_xyz_string(x, y, z)
|
|
self.gerbil.send_immediately("G92 " + xStr + yStr + zStr + "\n")
|
|
|
|
def probe(self, x = None, y = None, z = None, feed = 5.9):
|
|
xStr, yStr, zStr = self._get_xyz_string(x, y, z)
|
|
self.gerbil.send_immediately("G38.2 " + xStr + yStr + zStr + " F" + str(feed) + "\n")
|
|
PrbResp = self.waitOnGCodeComplete("PRB")
|
|
# Example output: '[PRB:-0.1965,-0.1965,-2.0697:1]'
|
|
print("PrbResp: " + str(PrbResp))
|
|
tmp = PrbResp.split("PRB:")[1]
|
|
tmp = tmp.split(":1")[0]
|
|
numbers = tmp.split(",")
|
|
print("Numbers: " + str(numbers))
|
|
return [float(x) for x in numbers]
|
|
|
|
def probeZSequence(self):
|
|
plateHeight = self.plateHeight
|
|
#Move down medium speed to reference plate
|
|
print("***************************************4")
|
|
print("***************************************5")
|
|
self.probe(z = -2.75, feed = 5.9) # move down by 2.75" until probe hit
|
|
print("***************************************6")
|
|
self.set_cur_pos_as(z = plateHeight) # Set actual 0 to probed location
|
|
print("***************************************7")
|
|
|
|
#Move up, then slowly to reference plate
|
|
self.work_offset_move(z = plateHeight + 0.1, feed=180) # Move just above reference plate
|
|
xyz = self.probe(z = plateHeight-0.05, feed = 1.5)
|
|
self.set_cur_pos_as(z = plateHeight) # Set actual 0 to probed location
|
|
self.work_offset_move(z = plateHeight + 0.5, feed=180) # Move just above reference plate, clearing lip on reference plate
|
|
# return z height of the probe
|
|
return xyz[0], xyz[1], xyz[2] - plateHeight
|
|
|
|
|
|
def probeXYSequence(self, plateAngle):
|
|
cutterRadius = self.cutterRadius
|
|
# Firxt xAxis then yAxis
|
|
for axisAngle in [0, math.pi / 2.0]:
|
|
angle = axisAngle + plateAngle
|
|
|
|
plateHeight = self.plateHeight
|
|
plateWidth = self.plateWidth # total width of touch plate...half of this is distance to center of touch plate
|
|
firstSafeDist = plateWidth * 0.75
|
|
secSafeDist = plateWidth * 0.5 + cutterRadius + 0.1 # can get a little closer second time as we already sensed edge of plate once
|
|
probeToDist = plateWidth * 0.25
|
|
|
|
# Move up
|
|
self.work_offset_move(z = plateHeight + 0.5, feed=100) # Move just above reference plate, clearing lip on reference plate
|
|
#once medium speed, once slow speed
|
|
for feed, dist in zip([5.9, 1.5], [firstSafeDist, secSafeDist]):
|
|
# Move to side of touch plate
|
|
self.work_offset_move(x = math.cos(angle) * dist, y = math.sin(angle) * dist, feed=180)
|
|
# Move below touch plate
|
|
self.work_offset_move(z = plateHeight-0.1, feed=100)
|
|
# Probe to the touch plate
|
|
refPoint = self.probe(x = math.cos(angle) * probeToDist, y = math.sin(angle) * probeToDist, feed=5.9)
|
|
# set this as new side of touch plate
|
|
self.set_cur_pos_as(x = math.cos(angle) * (plateWidth * 0.5 + cutterRadius) , \
|
|
y = math.sin(angle) * (plateWidth * 0.5 + cutterRadius))
|
|
|
|
# Move away from plate and up, then to center of touchplate
|
|
self.work_offset_move(x = math.cos(angle) * secSafeDist, y = math.sin(angle) * secSafeDist, feed = 100)
|
|
self.work_offset_move(z = plateHeight + 0.5, feed=180) # Move just above reference plate, clearing lip on reference plate
|
|
|
|
# we want work coord system to be center of knotch of touch plate, not center of touch plate itself. Move there then make that zero.
|
|
self.work_offset_move(x = math.cos(angle - math.pi/4.0) * self.distToKnotch, y = math.sin(angle - math.pi/4.0) * self.distToKnotch, feed = 400)
|
|
self.set_cur_pos_as(x = 0, y = 0)
|
|
return [refPoint[0] - math.cos(angle) * (plateWidth * 0.5 + cutterRadius) + math.cos(angle - math.pi/4.0) * self.distToKnotch, \
|
|
refPoint[1] - math.sin(angle) * (plateWidth * 0.5 + cutterRadius) + math.sin(angle - math.pi/4.0) * self.distToKnotch]
|
|
|
|
|
|
def probeAngleOfTouchPlate(self, estPlateAngle, x, y):
|
|
plateHeight = self.plateHeight
|
|
plateWidth = self.plateWidth # total width of touch plate...half of this is distance to center of touch plate
|
|
firstSafeDist = plateWidth * 0.75
|
|
secSafeDist = plateWidth * 0.625 # can get a little closer second time as we already sensed edge of plate once
|
|
probeToDist = plateWidth * 0.25
|
|
|
|
|
|
angle = estPlateAngle
|
|
# Move up
|
|
self.work_offset_move(z = plateHeight + 0.5, feed=180) # Move just above reference plate, clearing lip on reference plate
|
|
# Move to side of touch plate and down a quarter of the plate width
|
|
|
|
# this routine does not adjust work offset, so need to always be conservative
|
|
#Probe down a quarter of touch plate first
|
|
#once medium speed, once slow speed
|
|
distAdjust = 0
|
|
for feed, dist in zip([5.9, 1.5], [firstSafeDist, firstSafeDist]):
|
|
self.work_offset_move(x = math.cos(angle) * (dist - distAdjust) + math.cos(angle - math.pi/2.0) * plateWidth * 0.25 , \
|
|
y = math.sin(angle) * (dist - distAdjust) + math.sin(angle - math.pi/2.0) * plateWidth * 0.25, feed=400)
|
|
# Move below touch plate
|
|
self.work_offset_move(z = plateHeight-0.1, feed=180)
|
|
# Probe to the touch plate
|
|
ref1 = self.probe(x = math.cos(angle) * probeToDist + math.cos(angle - math.pi/2.0) * plateWidth * 0.25 , \
|
|
y = math.sin(angle) * probeToDist + math.sin(angle - math.pi/2.0) * plateWidth * 0.25, feed=feed)
|
|
# move just 0.1" away from plate next probe since we know where idge roughy is now
|
|
distAdjust = dist - math.dist([x,y], [ref1[0], ref1[1]]) - 0.05
|
|
print(ref1)
|
|
print("distAdjust: " + str(distAdjust))
|
|
|
|
#Probe up a quarter of touch plate second
|
|
#once medium speed, once slow speed
|
|
distAdjust = 0
|
|
for feed, dist in zip([5.9, 1.5], [firstSafeDist, firstSafeDist]):
|
|
self.work_offset_move(x = math.cos(angle) * (dist - distAdjust) + math.cos(angle + math.pi/2.0) * plateWidth * 0.25 , \
|
|
y = math.sin(angle) * (dist - distAdjust) + math.sin(angle + math.pi/2.0) * plateWidth * 0.25, feed=400)
|
|
# Move below touch plate
|
|
self.work_offset_move(z = plateHeight - 0.1, feed=100)
|
|
# Probe to the touch plate
|
|
ref2 = self.probe(x = math.cos(angle) * probeToDist + math.cos(angle + math.pi/2.0) * plateWidth * 0.25 , \
|
|
y = math.sin(angle) * probeToDist + math.sin(angle + math.pi/2.0) * plateWidth * 0.25, feed=feed)
|
|
distAdjust = dist - math.dist([x,y], [ref2[0], ref2[1]]) - 0.05
|
|
|
|
# Move away from reference plate and up
|
|
self.work_offset_move(x = math.cos(angle) * firstSafeDist + math.cos(angle + math.pi/2.0) * plateWidth * 0.25 , \
|
|
y = math.sin(angle) * firstSafeDist + math.sin(angle + math.pi/2.0) * plateWidth * 0.25, feed=400)
|
|
self.work_offset_move(z = plateHeight + 0.5, feed=180) # Move just above reference plate, clearing lip on reference plate
|
|
|
|
yAxisAngle = math.atan2(ref2[1] - ref1[1], ref2[0] - ref1[0])
|
|
xAxisAngle = yAxisAngle - math.pi / 2.0 % (2 * math.pi)
|
|
print("estPlateAngle:" + str(estPlateAngle))
|
|
print("xAxisAngle: " + str(xAxisAngle))
|
|
return xAxisAngle
|
|
|
|
def probeSequence(self, estPlateAngle, justZ):
|
|
#function assumes spindle is directly above probe plate in estimated middle
|
|
#function returns x, y, z position of center top of plate
|
|
|
|
# First Zero out work coord offset with best we have thus far
|
|
self.set_cur_pos_as(x=0, y=0, z = 0) # probe ony works on work coordinage system, set it to 0 so we know where we are in that
|
|
# first get Z height right
|
|
x, y, z = self.probeZSequence()
|
|
if justZ:
|
|
#reset G92 coordinate system to normal,
|
|
#where current position is actual position is position in work coordinate system
|
|
self.set_cur_pos_as(x = x, y = y, z = self.plateHeight + 0.5)
|
|
self.set_work_coord_offset(x = 0.0, y = 0.0, Z = z - self.plateHeight)
|
|
return [x, y, z]
|
|
plateAngle = self.probeAngleOfTouchPlate(estPlateAngle, x, y)
|
|
xy = self.probeXYSequence(plateAngle)
|
|
print("xy: " + str(xy))
|
|
self.set_work_coord_offset(x, y, z)
|
|
return xy + [z]
|
|
|
|
def zero_on_refPlate(self, refPoints, justZ = False):
|
|
avgX = (refPoints[0][0] + refPoints[1][0] + refPoints[2][0] + refPoints[3][0]) / 4.0
|
|
avgY = (refPoints[0][1] + refPoints[1][1] + refPoints[2][1] + refPoints[3][1]) / 4.0
|
|
angle = getBoxAngle(refPoints)
|
|
|
|
|
|
self.flushGcodeRespQue()
|
|
self.set_inches()
|
|
self.absolute_move(None, None, -0.25, feed = 180) # Move close to Z limit
|
|
# move 1.75" away from charuco marker bottom left
|
|
self.absolute_move(avgX + 1.335*math.cos(math.pi*5/4), avgY + 1.335*math.sin(math.pi*5/4), None, feed = 300) # Move above estimated ref plate
|
|
|
|
print("avgXY: " + str(avgX) + " " + str(avgY))
|
|
#first test out zero angle, then test out actual angle
|
|
#return self.probeSequence(0)
|
|
|
|
return self.probeSequence(angle, justZ)
|
|
|
|
def waitOnGCodeComplete(self, gCode):
|
|
resp = None
|
|
while resp == None:
|
|
while len(self.dataList) == 0:
|
|
self.event.wait()
|
|
print("curData:" + str(self.dataList[0]))
|
|
for data in self.dataList:
|
|
print(" " + str(data))
|
|
if gCode in str(data):
|
|
resp = data
|
|
#Remove item from list
|
|
self.dataList.pop(0)
|
|
self.eventList.pop(0)
|
|
#time.sleep(1)
|
|
print("resp: " + str(resp))
|
|
print("Found: " + str(resp[0]))
|
|
if isinstance(resp[0], dict):
|
|
return resp[0][gCode]
|
|
else:
|
|
return resp[0]
|
|
|
|
def flushGcodeRespQue(self):
|
|
self.dataList = []
|
|
self.eventList = []
|
|
|
|
def _get_xyz_string(self, x = None, y = None, z = None):
|
|
if x == None:
|
|
xStr = ""
|
|
else:
|
|
xStr = " X" + str(x)
|
|
|
|
if y == None:
|
|
yStr = ""
|
|
else:
|
|
yStr = " Y" + str(y)
|
|
|
|
if z == None:
|
|
zStr = ""
|
|
else:
|
|
zStr = " Z" + str(z)
|
|
return xStr, yStr, zStr
|
|
|
|
def absolute_move(self, x = None, y = None , z = None, feed = 100):
|
|
xStr, yStr, zStr = self._get_xyz_string(x, y, z)
|
|
fStr = " F" + str(feed)
|
|
self.set_inches()
|
|
self.gerbil.send_immediately("G53 G1" + xStr + yStr + zStr + fStr + "\n")
|
|
|
|
def work_offset_move(self, x = None, y = None , z = None, feed = 100):
|
|
xStr, yStr, zStr = self._get_xyz_string(x, y, z)
|
|
fStr = " F" + str(feed)
|
|
self.gerbil.send_immediately("G1" + xStr + yStr + zStr + fStr + "\n")
|
|
|
|
def send_svf(self, cncPaths):
|
|
global materialThickness
|
|
global cutterDiameter
|
|
|
|
cncGcodeGenerator = cncGcodeGeneratorClass(cncPaths = cncPaths,
|
|
materialThickness = materialThickness,
|
|
depthBelowMaterial = 0.06,
|
|
depthPerPass = 0.107,
|
|
cutFeedRate = 79,
|
|
safeHeight = 0.25,
|
|
tabHeight = 0.12,
|
|
useMM = False # use inches
|
|
)
|
|
cncGcodeGenerator.Generate()
|
|
cncGcodeGenerator.Save("test.nc")
|
|
#asdfasdf
|
|
self.set_inches()
|
|
self.absolute_move(z = -0.5)
|
|
print("SENDING GCODE")
|
|
gCodeStrs = []
|
|
for code in cncGcodeGenerator.gCodes:
|
|
gCodeStrs.append(str(code))
|
|
# put whole file in buffer then run the job
|
|
self.gerbil.write(gCodeStrs)
|
|
self.gerbil.job_run()
|
|
#for gCode in cncGcodeGenerator.gCodes:
|
|
# code = str(gCode) + "\n"
|
|
# #print("CODE:" + code)
|
|
# self.gerbil.stream(code)
|
|
# #self.gerbil.send_immediately(code)
|
|
# time.sleep(0.01)
|
|
|
|
self.absolute_move(z = -0.25)
|
|
|
|
def send_drawnPoints(self, offset, points3D):
|
|
global materialThickness
|
|
global cutterDiameter
|
|
points = deepcopy(points3D)
|
|
for point in points:
|
|
point.X = point.X + offset.X
|
|
point.Y = point.Y + offset.Y
|
|
cncPaths = cncPathsClass(points3D = points,
|
|
pointsPerCurve = 30,
|
|
distPerTab = 8,
|
|
tabWidth = 0.25,
|
|
cutterDiameter = cutterDiameter
|
|
)
|
|
cncGcodeGenerator = cncGcodeGeneratorClass(cncPaths = cncPaths,
|
|
materialThickness = materialThickness,
|
|
depthBelowMaterial = 0.06,
|
|
depthPerPass = 0.1,
|
|
cutFeedRate = 79,
|
|
safeHeight = 1.0,
|
|
tabHeight = 0.12,
|
|
useMM = False # use inches
|
|
)
|
|
cncGcodeGenerator.Generate()
|
|
self.set_inches()
|
|
self.absolute_move(z = -0.25)
|
|
print("SENDING GCODE")
|
|
for gCode in cncGcodeGenerator.gCodes:
|
|
print("CODE: " + str(gCode))
|
|
self.gerbil.stream(str(gCode) + "\n")
|
|
|
|
self.absolute_move(z = -0.25)
|
|
|
|
def set_inches(self):
|
|
self.gerbil.send_immediately("G20\n")
|
|
|
|
def set_mm(self):
|
|
self.gerbil.send_immediately("G21\n")
|
|
|
|
def send_file(self, gCodeFile, xOffset, yOffset, rotation):
|
|
#Set to inches for offset and rotations
|
|
self.set_inches()
|
|
|
|
##########################################
|
|
#Offset work to desired offset
|
|
##########################################
|
|
self.set_work_coord_offset(xOffset, yOffset)
|
|
|
|
##########################################
|
|
#Rotate work to desired rotation
|
|
##########################################
|
|
deg = -rotation * 180 / math.pi
|
|
self.gerbil.send_immediately("G68 X0 Y0 R" + str(deg) + "\n")
|
|
|
|
#Set back to mm, typically the units g code assumes
|
|
self.set_mm()
|
|
|
|
ZFound = False
|
|
with open(gCodeFile, 'r') as fh:
|
|
for line_text in fh.readlines():
|
|
if " Z" in line_text.upper():
|
|
ZFound = True
|
|
break
|
|
|
|
# if Z move found in file (not a laser cutting file), then move cutter away from workspace as first move
|
|
# so that if it was forgotten to do that, the first rapid traverse does not run into the workpiece
|
|
if ZFound:
|
|
self.absolute_move(z = -0.25)
|
|
|
|
with open(gCodeFile, 'r') as fh:
|
|
for line_text in fh.readlines():
|
|
self.gerbil.stream(line_text)
|
|
|
|
# Turn off rotated coordinate system
|
|
self.gerbil.send_immediately("G69\n")
|
|
|
|
|
|
#############################################################################
|
|
# Main
|
|
#############################################################################
|
|
|
|
cap = cv2.VideoCapture(1, cv2.CAP_DSHOW) # Set Capture Device, in case of a USB Webcam try 1, or give -1 to get a list of available devices
|
|
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
|
|
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 800)
|
|
|
|
#Set Width and Height
|
|
# cap.set(3,1280)
|
|
# cap.set(4,720)
|
|
|
|
|
|
# Capture frame-by-frame
|
|
#ret, frame = cap.read()
|
|
file = 'cnc13.jpg'
|
|
frame = cv2.imread(file)
|
|
img = cv2.imread(file)
|
|
|
|
#######################################################################
|
|
# Get grayscale image above threshold
|
|
#######################################################################
|
|
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
|
|
|
########################################
|
|
# Get aruco box information
|
|
########################################
|
|
boxes, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_100))
|
|
#print("boxes")
|
|
#print(boxes)
|
|
#print("ids")
|
|
#print(ids)
|
|
|
|
pixelLoc = [None]*2
|
|
locations = [None]*2
|
|
sideRefLocToOrigPixelLoc = [None]*2
|
|
pixelsAtBed = [None]*2
|
|
refBoxes = [leftBoxRef, rightBoxRef]
|
|
########################################
|
|
# Determine vertical homography at left (i=0) and right (i=1) side of CNC machine
|
|
########################################
|
|
for i in range(0, 2):
|
|
pixelLoc[i], locations[i], frame = boxes_to_point_and_location_list(boxes, ids, frame, i == 1)
|
|
print(ids)
|
|
for location in locations[i]:
|
|
print(location)
|
|
|
|
########################################
|
|
#Determine forward and backward transformation through homography
|
|
########################################
|
|
sideRefLocToOrigPixelLoc[i], status = cv2.findHomography(np.array(locations[i]), np.array(pixelLoc[i]))
|
|
|
|
#############################################################
|
|
# Draw vertical box on left and right vertical region of CNC
|
|
#############################################################
|
|
points = np.array([[refBoxes[i].Z,0],[bedSize.Z,0],[bedSize.Z,bedSize.Y],[refBoxes[i].Z,bedSize.Y]])
|
|
#points = np.array([[refBoxes[i].Z,refBoxes[i].Y + 15.658499997],[bedSize.Z,refBoxes[i].Y + 15.658499997],[bedSize.Z,bedSize.Y],[refBoxes[i].Z,refBoxes[i].Y]])
|
|
pixelsAtBed[i] = cv2.perspectiveTransform(points.reshape(-1,1,2), sideRefLocToOrigPixelLoc[i])
|
|
display_4_lines(pixelsAtBed[i], frame)
|
|
|
|
####################################################################################################
|
|
# Get forward and backward homography from simulated overhead Pixel location to Orig pixel location
|
|
# Makes destination image same size as source image. Reshaped later due to matplot lib speed limitations
|
|
####################################################################################################
|
|
#shape[0] is height. shape[1] is width
|
|
#PixelCorners are [height,0], height, width
|
|
height = float(frame.shape[1])
|
|
width = float(frame.shape[0])
|
|
bedPixelCorners = np.array([[height,0.0],[height,width],[0.0,0.0],[0.0,width]])
|
|
refPixels = np.array([pixelsAtBed[0][1],pixelsAtBed[0][2],pixelsAtBed[1][1],pixelsAtBed[1][2]])
|
|
bedPixelToOrigPixelLoc, status = cv2.findHomography(bedPixelCorners, refPixels)
|
|
origPixelToBedPixelLoc, status = cv2.findHomography(refPixels, bedPixelCorners)
|
|
|
|
#############################################################
|
|
# Draw box on CNC bed
|
|
#############################################################
|
|
pixels = cv2.perspectiveTransform(bedPixelCorners.reshape(-1,1,2), bedPixelToOrigPixelLoc)
|
|
display_4_lines(pixels, frame, flip=True)
|
|
|
|
#############################################################
|
|
# Display bed on original image
|
|
#############################################################
|
|
gray = cv2.resize(frame, (1280, 700))
|
|
cv2.imshow('image',gray)
|
|
cv2.waitKey()
|
|
|
|
######################################################################
|
|
# Warp perspective to perpendicular to bed view, create overlay calss
|
|
######################################################################
|
|
gCodeFile = 'test.nc'
|
|
cv2Overhead = cv2.warpPerspective(frame, origPixelToBedPixelLoc, (frame.shape[1], frame.shape[0]))
|
|
cv2Overhead = cv2.resize(cv2Overhead, (bedViewSizePixels, bedViewSizePixels))
|
|
GCodeOverlay = OverlayGcode(cv2Overhead, \
|
|
svgFile = 'puzzles2.svg', \
|
|
#gCodeFile = gCodeFile, \
|
|
enableSender = False)
|
|
|
|
########################################
|
|
# Detect box location in overhead image
|
|
########################################
|
|
#Change overhead image to gray for box detection
|
|
refPixelLoc = get_id_loc(frame, boxes, ids, 66)
|
|
refPhysicalLoc = cv2.perspectiveTransform(refPixelLoc.reshape(-1,1,2), origPixelToBedPixelLoc)
|
|
touchPlateLocPercent = refPhysicalLoc / [frame.shape[1], frame.shape[0]]
|
|
touchPlateLoc = []
|
|
touchPlatePixels = []
|
|
for a in touchPlateLocPercent:
|
|
touchPlateLoc.append(a[0] * [bedSize.X, bedSize.Y])
|
|
touchPlatePixels.append(a[0] * [bedViewSizePixels, bedViewSizePixels] )
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("")
|
|
print("Touch Plate Loc: " + str(touchPlateLoc))
|
|
GCodeOverlay.set_ref_loc(touchPlatePixels)
|
|
|
|
######################################################################
|
|
# Create a G Code sender now that overlay is created
|
|
######################################################################
|
|
|
|
plt.show()
|
|
|
|
# When everything done, release the capture
|
|
cap.release()
|
|
cv2.destroyAllWindows()
|