diff --git a/.gitignore b/.gitignore index 6a56b95d..d4028018 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,10 @@ ENV/ env.bak/ venv.bak/ +#pyyinstaller +build +dist + #logs log.txt alog.txt @@ -18,3 +22,8 @@ alog.txt __pycache__/ *.py[cod] *$py.class +Services/tp.py +Services/tr.py +Services/pijson.txt +Services/pijsonRT.txt +File/wiicontrol.code-workspace diff --git a/Actions/HoleySimulationKinematics.py b/Actions/HoleySimulationKinematics.py new file mode 100644 index 00000000..b2b7ea11 --- /dev/null +++ b/Actions/HoleySimulationKinematics.py @@ -0,0 +1,606 @@ +import math +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs + + +class Kinematics(MakesmithInitFuncs): + ''' + The Kinematics module relates the lengths of the chains to the position of the cutting head + in X-Y space. + ''' + + l = 310.0 # horizontal distance between sled attach points + s = 139.0 # vertical distance between sled attach points and bit + h = math.sqrt((l / 2) * (l / 2) + s * s) # distance between sled attach point and bit + h3 = 79.0 # distance from bit to sled center of mass + D = 2978.4 # distance between sprocket centers + R = 10.2 # sprocket radius + machineHeight = 1219.2 # this is 4 feet in mm + machineWidth = 2438.4 # this is 8 feet in mm + motorOffsetY = 463.0 # vertical distance from the corner of the work area to the sprocket center + chain1Offset = 0 # number of links +,- that have slipped + chain2Offset = 0 # number of links +,- that have slipped + + x = 2708.4 + y = 270 + + # utility variables + DegPerRad = 360 / (4 * math.atan(1)) + Time = 0 + Mirror = 0 + + # Definition of angles + # Gamma, angle of left chain + # Lamda, angle of right chain + # Phi, tilt of sled + # Theta, angle between chains and bit (corners of triangle formed by l, s, h) + + # Calculation tolerances + MaxError = 0.01 + MaxTries = 10 + DeltaPhi = 0.01 + DeltaY = 0.01 + + # Criterion Computation Variables + Phi = -0.2 + TanGamma = 0 + TanLambda = 0 + Y1Plus = 0 + Y2Plus = 0 + if (l < 1.0): + Theta = 0 + else: + Theta = math.atan(2 * s / l) + Psi1 = Theta - Phi + Psi2 = Theta + Phi + Tries = 0 + Jac = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + Solution = [0, 0, 0] + Crit = [0, 0, 0] + # Offsetx1 = 0 + # Offsetx2 = 0 + # Offsety1 = 0 + # Offsety2 = 0 + # SinPsi1 = 0 + # CosPsi1 = 0 + # SinPsi2 = 0 + # CosPsi2 = 0 + # SinPsi1D = 0 + # CosPsi1D = 0 + # SinPsi2D = 0 + # CosPsi2D = 0 + # MySinPhi = 0 + # MySinPhiDelta = 0 + + # intermediate output + Lambda = 0 + Gamma = 0 + + # Motor axes length to the bit for triangular kinematics + Motor1Distance = 0 # left motor axis distance to sled + Motor2Distance = 0 # right motor axis distance to sled + + # output = chain lengths measured from 12 o'clock + Chain1 = 0 # left chain length + Chain2 = 0 # right chain length + + leftChainTolerance = 0 + rightChainTolerance = 0 + chainDensity = .0013 # N/mm + chainElasticity = 5.1685e-6 # mm/mm/N + sledWeight = 97.9 # N + + i = 0 + + rotationDiskRadius = 100 + chainSagCorrection = 0 + chainOverSprocket = 1 + _xCordOfMotor = D / 2 + _yCordOfMotor = machineHeight / 2 + motorOffsetY + isQuadKinematics = True + + def _verifyValidTarget(self, xTarget, yTarget): + # If the target point is beyond one of the edges of the board, the machine stops at the edge + + if (xTarget < -self.machineWidth / 2): + xTarget = -self.machineWidth / 2 + + elif (xTarget > self.machineWidth / 2): + xTarget = self.machineWidth / 2 + + elif (yTarget > self.machineHeight / 2): + yTarget = self.machineHeight / 2 + + elif (yTarget < -self.machineHeight / 2): + yTarget = -self.machineHeight / 2 + + def recomputeGeometry(self): + ''' + Some variables are computed on class creation for the geometry of the machine to reduce overhead, + calling this function regenerates those values. + ''' + self.h = math.sqrt((self.l / 2) * (self.l / 2) + self.s * self.s) + self.Theta = math.atan(2 * self.s / self.l) + self.Psi1 = self.Theta - self.Phi + self.Psi2 = self.Theta + self.Phi + + self._xCordOfMotor = self.D / 2 + self._yCordOfMotor = self.machineHeight / 2 + self.motorOffsetY + + def inverse(self, xTarget, yTarget): + ''' + + Compute the lengths of chain needed to reach a target XY position + + ''' + if self.isQuadKinematics: + return self.quadrilateralInverse(xTarget, yTarget) + else: + return self.triangularInverse(xTarget, yTarget) + + def triangularInverse(self, xTarget, yTarget): + ''' + + The inverse kinematics (relating an xy coordinate pair to the required chain lengths to hit that point) + function for a triangular set up where the chains meet at a point, or are arranged so that they simulate + meeting at a point. + + ''' + + # Confirm that the coordinates are on the wood + self._verifyValidTarget(xTarget, yTarget) + + Motor1Distance = math.sqrt( + math.pow((-1 * self._xCordOfMotor - xTarget), 2) + math.pow((self._yCordOfMotor - yTarget), 2)) + Motor2Distance = math.sqrt( + math.pow((self._xCordOfMotor - xTarget), 2) + math.pow((self._yCordOfMotor - yTarget), 2)) + + # Calculate the chain angles from horizontal, based on if the chain connects to the sled from the top or bottom of the sprocket + if self.chainOverSprocket == 1: + Chain1Angle = math.asin((self._yCordOfMotor - yTarget) / Motor1Distance) + math.asin( + self.R / Motor1Distance) + Chain2Angle = math.asin((self._yCordOfMotor - yTarget) / Motor2Distance) + math.asin( + self.R / Motor2Distance) + + Chain1AroundSprocket = self.R * Chain1Angle + Chain2AroundSprocket = self.R * Chain2Angle + + xTangent1 = -self._xCordOfMotor + self.R * math.sin(Chain1Angle) + yTangent1 = self._yCordOfMotor + self.R * math.cos(Chain1Angle) + + xTangent2 = self._xCordOfMotor - self.R * math.sin(Chain2Angle) + yTangent2 = self._yCordOfMotor + self.R * math.cos(Chain2Angle) + else: + Chain1Angle = math.asin((self._yCordOfMotor - yTarget) / Motor1Distance) - math.asin( + self.R / Motor1Distance) + Chain2Angle = math.asin((self._yCordOfMotor - yTarget) / Motor2Distance) - math.asin( + self.R / Motor2Distance) + + Chain1AroundSprocket = self.R * (3.14159 - Chain1Angle) + Chain2AroundSprocket = self.R * (3.14159 - Chain2Angle) + + xTangent1 = -self._xCordOfMotor - self.R * math.sin(Chain1Angle) + yTangent1 = self._yCordOfMotor - self.R * math.cos(Chain1Angle) + + xTangent2 = self._xCordOfMotor + self.R * math.sin(Chain2Angle) + yTangent2 = self._yCordOfMotor - self.R * math.cos(Chain2Angle) + + # Calculate the straight chain length from the sprocket to the bit + Chain1Straight = math.sqrt(math.pow(Motor1Distance, 2) - math.pow(self.R, 2)) + Chain2Straight = math.sqrt(math.pow(Motor2Distance, 2) - math.pow(self.R, 2)) + + # TensionDenominator=(x_l y_r- x_r y_l- x_l y_t +x_t y_l +x_r y_t -x_t y_r) + TensionDenominator = ( + xTangent1 * yTangent2 - xTangent2 * yTangent1 - xTangent1 * yTarget + xTarget * yTangent1 + xTangent2 * yTarget - xTarget * yTangent2) + + # Total vertical force is sled weight, plus half the two chain weights + TotalWeight = self.sledWeight + 0.5 * self.chainDensity * (Chain1Straight + Chain2Straight) + + # T_l = -( w *sqrt( pow(x_l -x_t ,2.0)+pow( y_l -y_t ,2.0)) (x_r -x_t)) /TensionDenominator + Tension1 = - (TotalWeight * math.sqrt(math.pow(xTangent1 - xTarget, 2) + math.pow(yTangent1 - yTarget, 2)) * ( + xTangent2 - xTarget)) / TensionDenominator + + # T_r = ( w *sqrt( pow(x_r -x_t ,2.0)+pow( y_r -y_t ,2.0)) (x_l -x_t))/(x_ly_r-x_ry_l-x_ly_t+x_ty_l+x_ry_t-x_ty_r) + Tension2 = (TotalWeight * math.sqrt(math.pow(xTangent2 - xTarget, 2) + math.pow(yTangent2 - yTarget, 2)) * ( + xTangent1 - xTarget)) / TensionDenominator + + HorizontalTension = Tension1 * (xTarget - xTangent1) / Chain1Straight + + # Calculation of horizontal component of tension for two chains should be equal, as validation step + # HorizontalTension1=Tension1*(xTarget-xTangent1)/Chain1Straight + # HorizontalTension2=Tension2*(xTangent2-xTarget)/Chain2Straight + + # Calculation of vertical force due to tension, to validate tension calculation + # VerticalForce=Tension1*(yTangent1-yTarget)/Chain1Straight+Tension2*(yTangent2-yTarget)/Chain2Straight + + a1 = HorizontalTension / self.chainDensity + a2 = HorizontalTension / self.chainDensity + # print("Horizontal Tension ",HorizontalTension) + + # Catenary Equation + Chain1 = math.sqrt( + math.pow(2 * a1 * math.sinh((xTarget - xTangent1) / (2 * a1)), 2) + math.pow(yTangent1 - yTarget, 2)) + Chain2 = math.sqrt( + math.pow(2 * a2 * math.sinh((xTangent2 - xTarget) / (2 * a2)), 2) + math.pow(yTangent2 - yTarget, 2)) + CatenaryDelta1 = Chain1 - Chain1Straight + CatenaryDelta2 = Chain2 - Chain2Straight + # print("Catenary Delta 1 ",CatenaryDelta1) + # print("Catenary Delta 2 ",CatenaryDelta2) + + # Calculate total chain lengths accounting for sprocket geometry and chain sag + Chain1ElasticityDelta = Chain1 - Chain1 / (1 + Tension1 * self.chainElasticity) + Chain2ElasticityDelta = Chain2 - Chain2 / (1 + Tension2 * self.chainElasticity) + # print("Chain1 Elasticity Delta ",Chain1ElasticityDelta) + # print("Chain2 Elasticity Delta ",Chain2ElasticityDelta) + + Chain1 = Chain1AroundSprocket + Chain1 / (1.0 + self.leftChainTolerance / 100.0) / ( + 1 + Tension1 * self.chainElasticity) + Chain2 = Chain2AroundSprocket + Chain2 / (1.0 + self.rightChainTolerance / 100.0) / ( + 1 + Tension2 * self.chainElasticity) + + # Subtract of the virtual length which is added to the chain by the rotation mechanism + Chain1 = Chain1 - self.rotationDiskRadius + Chain2 = Chain2 - self.rotationDiskRadius + + return Chain1, Chain2 + + def quadrilateralInverse(self, xTarget, yTarget): + + ''' + + Take the XY position and return chain lengths. + + ''' + + # Confirm that the coordinates are on the wood + self._verifyValidTarget(xTarget, yTarget) + + # coordinate shift to put (0,0) in the center of the plywood from the left sprocket + self.x = (self.D / 2.0) + xTarget + self.y = (self.machineHeight / 2.0) + self.motorOffsetY - yTarget + + # Coordinates definition: + # x -->, y | + # v + # (0,0) at center of left sprocket + # upper left corner of plywood (270, 270) + + Tries = 0 # initialize + if ( + self.x > self.D / 2.0): # the right half of the board mirrors the left half so all computations are done using left half coordinates. + self.x = self.D - self.x # Chain lengths are swapped at exit if the x,y is on the right half + self.Mirror = True + + else: + self.Mirror = False + + self.TanGamma = self.y / self.x + self.TanLambda = self.y / (self.D - self.x) + self.Y1Plus = self.R * math.sqrt(1 + self.TanGamma * self.TanGamma) + self.Y2Plus = self.R * math.sqrt(1 + self.TanLambda * self.TanLambda) + + self._MyTrig() + # self.Psi1 = self.Theta - self.Phi + # self.Psi2 = self.Theta + self.Phi + # These criteria will be zero when the correct values are reached + # They are negated here as a numerical efficiency expedient + + self.Crit[0] = - self._moment(self.Y1Plus, self.Y2Plus, self.Phi, self.MySinPhi, self.SinPsi1, self.CosPsi1, + self.SinPsi2, self.CosPsi2) + self.Crit[1] = - self._YOffsetEqn(self.Y1Plus, self.x - self.h * self.CosPsi1, self.SinPsi1) + self.Crit[2] = - self._YOffsetEqn(self.Y2Plus, self.D - (self.x + self.h * self.CosPsi2), self.SinPsi2) + + while (Tries <= self.MaxTries): + if (abs(self.Crit[0]) < self.MaxError): + if (abs(self.Crit[1]) < self.MaxError): + if (abs(self.Crit[2]) < self.MaxError): + break + + # estimate the tilt angle that results in zero net _moment about the pen + # and refine the estimate until the error is acceptable or time runs out + + # Estimate the Jacobian components + + self.Jac[0] = (self._moment(self.Y1Plus, self.Y2Plus, self.Phi + self.DeltaPhi, self.MySinPhiDelta, + self.SinPsi1D, self.CosPsi1D, self.SinPsi2D, self.CosPsi2D) + self.Crit[ + 0]) / self.DeltaPhi + self.Jac[1] = (self._moment(self.Y1Plus + self.DeltaY, self.Y2Plus, self.Phi, self.MySinPhi, self.SinPsi1, + self.CosPsi1, self.SinPsi2, self.CosPsi2) + self.Crit[0]) / self.DeltaY + self.Jac[2] = (self._moment(self.Y1Plus, self.Y2Plus + self.DeltaY, self.Phi, self.MySinPhi, self.SinPsi1, + self.CosPsi1, self.SinPsi2, self.CosPsi2) + self.Crit[0]) / self.DeltaY + self.Jac[3] = (self._YOffsetEqn(self.Y1Plus, self.x - self.h * self.CosPsi1D, self.SinPsi1D) + self.Crit[ + 1]) / self.DeltaPhi + self.Jac[4] = (self._YOffsetEqn(self.Y1Plus + self.DeltaY, self.x - self.h * self.CosPsi1, self.SinPsi1) + + self.Crit[1]) / self.DeltaY + self.Jac[5] = 0.0 + self.Jac[6] = (self._YOffsetEqn(self.Y2Plus, self.D - (self.x + self.h * self.CosPsi2D), self.SinPsi2D) + + self.Crit[2]) / self.DeltaPhi + self.Jac[7] = 0.0 + self.Jac[8] = (self._YOffsetEqn(self.Y2Plus + self.DeltaY, self.D - (self.x + self.h * self.CosPsi2D), + self.SinPsi2) + self.Crit[2]) / self.DeltaY + + # solve for the next guess + + buildOutJac = self._moment(self.Y1Plus + self.DeltaY, self.Y2Plus, self.Phi, self.MySinPhi, self.SinPsi1, + self.CosPsi1, self.SinPsi2, self.CosPsi2) + self.Crit[0] + + self._MatSolv() # solves the matrix equation Jx=-Criterion + + # update the variables with the new estimate + + self.Phi = self.Phi + self.Solution[0] + self.Y1Plus = self.Y1Plus + self.Solution[1] # don't allow the anchor points to be inside a sprocket + if (self.Y1Plus < self.R): + self.Y1Plus = self.R + + self.Y2Plus = self.Y2Plus + self.Solution[2] # don't allow the anchor points to be inside a sprocke + if (self.Y2Plus < self.R): + self.Y2Plus = self.R + + self.Psi1 = self.Theta - self.Phi + self.Psi2 = self.Theta + self.Phi + + # evaluate the + # three criterion equations + self._MyTrig() + + self.Crit[0] = - self._moment(self.Y1Plus, self.Y2Plus, self.Phi, self.MySinPhi, self.SinPsi1, self.CosPsi1, + self.SinPsi2, self.CosPsi2) + self.Crit[1] = - self._YOffsetEqn(self.Y1Plus, self.x - self.h * self.CosPsi1, self.SinPsi1) + self.Crit[2] = - self._YOffsetEqn(self.Y2Plus, self.D - (self.x + self.h * self.CosPsi2), self.SinPsi2) + Tries = Tries + 1 # increment itteration count + + if (Tries > self.MaxTries): + print("unable to calculate chain lengths") + + # Variables are within accuracy limits + # perform output computation + + self.Offsetx1 = self.h * self.CosPsi1 + self.Offsetx2 = self.h * self.CosPsi2 + self.Offsety1 = self.h * self.SinPsi1 + self.Offsety2 = self.h * self.SinPsi2 + self.TanGamma = (self.y - self.Offsety1 + self.Y1Plus) / (self.x - self.Offsetx1) + self.TanLambda = (self.y - self.Offsety2 + self.Y2Plus) / (self.D - (self.x + self.Offsetx2)) + self.Gamma = math.atan(self.TanGamma) + self.Lambda = math.atan(self.TanLambda) + + # compute the chain lengths + + if (self.Mirror): + Chain2 = math.sqrt( + (self.x - self.Offsetx1) * (self.x - self.Offsetx1) + (self.y + self.Y1Plus - self.Offsety1) * ( + self.y + self.Y1Plus - self.Offsety1)) - self.R * self.TanGamma + self.R * self.Gamma # right chain length + Chain1 = math.sqrt((self.D - (self.x + self.Offsetx2)) * (self.D - (self.x + self.Offsetx2)) + ( + self.y + self.Y2Plus - self.Offsety2) * ( + self.y + self.Y2Plus - self.Offsety2)) - self.R * self.TanLambda + self.R * self.Lambda # left chain length + else: + Chain1 = math.sqrt( + (self.x - self.Offsetx1) * (self.x - self.Offsetx1) + (self.y + self.Y1Plus - self.Offsety1) * ( + self.y + self.Y1Plus - self.Offsety1)) - self.R * self.TanGamma + self.R * self.Gamma # left chain length + Chain2 = math.sqrt((self.D - (self.x + self.Offsetx2)) * (self.D - (self.x + self.Offsetx2)) + ( + self.y + self.Y2Plus - self.Offsety2) * ( + self.y + self.Y2Plus - self.Offsety2)) - self.R * self.TanLambda + self.R * self.Lambda # right chain length + + aChainLength = Chain1 + bChainLength = Chain2 + # print 'target ({:.2f},{:.2f}) chains {:.3f},{:.3f} chain diff {:.3f} sled angle {:.4f} (degrees{:.4f})'.format(xTarget,yTarget,aChainLength, bChainLength, aChainLength - bChainLength, self.Phi, self.Phi*3.14159) + + return aChainLength, bChainLength + + def forward(self, chainALength, chainBLength, xGuess = -10, yGuess = 10): + ''' + + Take the chain lengths and return an XY position + + ''' + # print("Chain Sag Correction:") + # print(self.chainSagCorrection) + # print("") + # apply any offsets for slipped links + chainALength = chainALength + (self.chain1Offset * self.R) + chainBLength = chainBLength + (self.chain2Offset * self.R) + + guessCount = 0 + + while (1): + + # check our guess + guessLengthA, guessLengthB = self.inverse(xGuess, yGuess) + + aChainError = chainALength - guessLengthA + bChainError = chainBLength - guessLengthB + + # print 'guess {:7.3f} {:7.3f} error {:7.3f} {:7.3f} guesslength {:7.3f} {:7.3f} '.format(xGuess,yGuess,aChainError,bChainError,guessLengthA, guessLengthB) + + # if we've converged on the point...or it's time to give up, exit the loop + if ((abs(aChainError) < .000000001 and abs(bChainError) < .000000001) or guessCount > 5000): + if (guessCount > 5000): + print + "Message: Unable to find valid machine position. Please calibrate chain lengths.", aChainError, bChainError, xGuess, yGuess + return xGuess, yGuess + else: + return xGuess, yGuess + else: + # adjust the guess based on the result + xGuess = xGuess + .1 * aChainError - .1 * bChainError + yGuess = yGuess - .1 * aChainError - .1 * bChainError + guessCount = guessCount + 1 + + def _MatSolv(self): + Sum = 0 + NN = 0 + i = 0 + ii = 0 + J = 0 + JJ = 0 + K = 0 + KK = 0 + L = 0 + M = 0 + N = 0 + + fact = 0 + + b = 0 + while b < 5: + b = b + 1 + + # gaus elimination, no pivot + + N = 3 + NN = N - 1 + i = 1 + while (i <= NN): + J = (N + 1 - i) + JJ = (J - 1) * N - 1 + L = J - 1 + KK = -1 + K = 0 + while (K < L): + fact = self.Jac[KK + J] / self.Jac[JJ + J]; + M = 1 + while (M <= J): + self.Jac[KK + M] = self.Jac[KK + M] - fact * self.Jac[JJ + M] + M = M + 1 + KK = KK + N; + self.Crit[K] = self.Crit[K] - fact * self.Crit[J - 1]; + K = K + 1 + i = i + 1 + + # Lower triangular matrix solver + + self.Solution[0] = self.Crit[0] / self.Jac[0] + ii = N - 1 + + i = 2 + while (i <= N): + M = i - 1; + Sum = self.Crit[i - 1]; + + J = 1 + while (J <= M): + Sum = Sum - self.Jac[ii + J] * self.Solution[J - 1]; + J = J + 1 + + self.Solution[i - 1] = Sum / self.Jac[ii + i]; + ii = ii + N; + + i = i + 1 + + def _moment(self, Y1Plus, Y2Plus, Phi, MSinPhi, MSinPsi1, MCosPsi1, MSinPsi2, + MCosPsi2): # computes net moment about center of mass + '''Temp + Offsetx1 + Offsetx2 + Offsety1 + Offsety2 + Psi1 + Psi2 + TanGamma + TanLambda''' + + self.Psi1 = self.Theta - Phi + self.Psi2 = self.Theta + Phi + + self.Offsetx1 = self.h * MCosPsi1 + self.Offsetx2 = self.h * MCosPsi2 + self.Offsety1 = self.h * MSinPsi1 + self.Offsety2 = self.h * MSinPsi2 + self.TanGamma = (self.y - self.Offsety1 + Y1Plus) / (self.x - self.Offsetx1) + self.TanLambda = (self.y - self.Offsety2 + Y2Plus) / (self.D - (self.x + self.Offsetx2)) + + return self.h3 * MSinPhi + (self.h / (self.TanLambda + self.TanGamma)) * ( + MSinPsi2 - MSinPsi1 + (self.TanGamma * MCosPsi1 - self.TanLambda * MCosPsi2)) + + def _MyTrig(self): + + Phisq = self.Phi * self.Phi + Phicu = self.Phi * Phisq + Phidel = self.Phi + self.DeltaPhi + Phidelsq = Phidel * Phidel + Phidelcu = Phidel * Phidelsq + Psi1sq = self.Psi1 * self.Psi1 + Psi1cu = Psi1sq * self.Psi1 + Psi2sq = self.Psi2 * self.Psi2 + Psi2cu = self.Psi2 * Psi2sq + Psi1del = self.Psi1 - self.DeltaPhi + Psi1delsq = Psi1del * Psi1del + Psi1delcu = Psi1del * Psi1delsq + Psi2del = self.Psi2 + self.DeltaPhi + Psi2delsq = Psi2del * Psi2del + Psi2delcu = Psi2del * Psi2delsq + + self.MySinPhi = -0.1616 * Phicu - 0.0021 * Phisq + 1.0002 * self.Phi + self.MySinPhiDelta = -0.1616 * Phidelcu - 0.0021 * Phidelsq + 1.0002 * Phidel + + self.SinPsi1 = -0.0942 * Psi1cu - 0.1368 * Psi1sq + 1.0965 * self.Psi1 - 0.0241 # sinPsi1 + self.CosPsi1 = 0.1369 * Psi1cu - 0.6799 * Psi1sq + 0.1077 * self.Psi1 + 0.9756 # cosPsi1 + self.SinPsi2 = -0.1460 * Psi2cu - 0.0197 * Psi2sq + 1.0068 * self.Psi2 - 0.0008 # sinPsi2 + self.CosPsi2 = 0.0792 * Psi2cu - 0.5559 * Psi2sq + 0.0171 * self.Psi2 + 0.9981 # cosPsi2 + + self.SinPsi1D = -0.0942 * Psi1delcu - 0.1368 * Psi1delsq + 1.0965 * Psi1del - 0.0241 # sinPsi1 + self.CosPsi1D = 0.1369 * Psi1delcu - 0.6799 * Psi1delsq + 0.1077 * Psi1del + 0.9756 # cosPsi1 + self.SinPsi2D = -0.1460 * Psi2delcu - 0.0197 * Psi2delsq + 1.0068 * Psi2del - 0.0008 # sinPsi2 + self.CosPsi2D = 0.0792 * Psi2delcu - 0.5559 * Psi2delsq + 0.0171 * Psi2del + 0.9981 # cosPsi2 + + def _YOffsetEqn(self, YPlus, Denominator, Psi): + + Temp = ((math.sqrt(YPlus * YPlus - self.R * self.R) / self.R) - ( + self.y + YPlus - self.h * math.sin(Psi)) / Denominator) + return Temp + + def updateSetting(self, firmwareKey, value): + + if firmwareKey == 2: + self.D = float(value) + elif firmwareKey == 13: + self.R = float(value/3.141592/2) + elif firmwareKey == 1: + self.machineHeight = float(value) + elif firmwareKey == 0: + self.machineWidth = float(value) + elif firmwareKey == 3: + self.motorOffsetY = float(value) + elif firmwareKey == 40: + self.leftChainTolerance = float(value) + elif firmwareKey == 41: + self.rightChainTolerance = float(value) + elif firmwareKey == 45: + self.chainElasticity = float(value) + elif firmwareKey == 46: + self.sledWeight = float(value) + elif firmwareKey == 8: + self.rotationDiskRadius = float(value) + elif firmwareKey == 37: + self.chainSagCorrection = float(value) + elif firmwareKey == 38: + self.chainOverSprocket = int(value) + elif firmwareKey == 7: + if int(value) == 1: + self.isQuadKinematics = True + else: + self.isQuadKinematics = False + self._xCordOfMotor = self.D / 2 + self._yCordOfMotor = self.machineHeight / 2 + self.motorOffsetY + + def initializeSettings(self): + self.D = float(self.data.config.getValue("Maslow Settings","motorSpacingX")) + self.R = float(self.data.config.getValue("Computed Settings","distPerRot"))/3.141592/2 + self.machineHeight = float(self.data.config.getValue("Maslow Settings","bedHeight")) + self.machineWidth = float(self.data.config.getValue("Maslow Settings", "bedWidth")) + self.motorOffsetY = float(self.data.config.getValue("Maslow Settings", "motorOffsetY")) + self.leftChainTolerance = float(self.data.config.getValue("Advanced Settings", "leftChainTolerance")) + self.rightChainTolerance = float(self.data.config.getValue("Advanced Settings", "rightChainTolerance")) + self.chainElasticity = float(self.data.config.getValue("Advanced Settings", "chainElasticity")) + self.sledWeight = float(self.data.config.getValue("Maslow Settings", "sledWeight")) + self.rotationDiskRadius = float(self.data.config.getValue("Advanced Settings", "rotationRadius")) + self.chainSagCorrection = float(self.data.config.getValue("Advanced Settings", "chainSagCorrection")) + self.chainOverSprocket = int(self.data.config.getValue("Computed Settings", "chainOverSprocketComputed")) + kinematicsType = int(self.data.config.getValue("Computed Settings", "kinematicsTypeComputed")) + if kinematicsType == 1: + self.isQuadKinematics = True + else: + self.isQuadKinematics = False + + + diff --git a/Actions/actions.py b/Actions/actions.py index eb78df5b..33561e2a 100644 --- a/Actions/actions.py +++ b/Actions/actions.py @@ -1,209 +1,536 @@ + from DataStructures.makesmithInitFuncs import MakesmithInitFuncs +import os import sys -import threading -import re import math import serial.tools.list_ports import glob import json +import time +import re +import zipfile +import threading +import platform +from subprocess import call +from gpiozero.pins.mock import MockFactory +from gpiozero import Device - +''' +This class does most of the heavy lifting in processing messages from the UI client. +''' class Actions(MakesmithInitFuncs): + + Device.pin_factory = MockFactory() + def processAction(self, msg): - if msg["data"]["command"] == "resetChainLengths": - if not self.resetChainLengths(): - self.data.ui_queue.put("Message: Error with resetting chain lengths.") - elif msg["data"]["command"] == "reportSettings": - self.data.gcode_queue.put("$$") - elif msg["data"]["command"] == "home": - if not self.home(): - self.data.ui_queue.put("Message: Error with returning to home.") - elif msg["data"]["command"] == "defineHome": - if self.defineHome(): - ## the gcode file might change the active units so we need to inform the UI of the change. - self.data.ui_queue.put("Action: unitsUpdate gcodeUpdate") - else: - self.data.ui_queue.put("Message: Error with defining home.") - elif msg["data"]["command"] == "defineZ0": - if not self.data.actions.defineZ0(): - self.data.ui_queue.put("Message: Error with defining Z-Axis zero.") - elif msg["data"]["command"] == "stopZ": - if not self.stopZ(): - self.data.ui_queue.put("Message: Error with stopping Z-Axis movement") - elif msg["data"]["command"] == "startRun": - if not self.startRun(): - self.data.ui_queue.put("Message: Error with starting run") - elif msg["data"]["command"] == "stopRun": - if not self.stopRun(): - self.data.ui_queue.put("Message: Error with stopping run") - elif msg["data"]["command"] == "moveToDefault": - if not self.moveToDefault(): - self.data.ui_queue.put( - "Message: Error with moving to default chain lengths" - ) - elif msg["data"]["command"] == "testMotors": - if not self.testMotors(): - self.data.ui_queue.put("Message: Error with testing motors") - elif msg["data"]["command"] == "wipeEEPROM": - if not self.wipeEEPROM(): - self.data.ui_queue.put("Message: Error with wiping EEPROM") - elif msg["data"]["command"] == "pauseRun": - if not self.pauseRun(): - self.data.ui_queue.put("Message: Error with pausing run") - elif msg["data"]["command"] == "resumeRun": - if not self.resumeRun(): - self.data.ui_queue.put("Message: Error with resuming run") - elif msg["data"]["command"] == "returnToCenter": - if not self.returnToCenter(): - self.data.ui_queue.put("Message: Error with returning to center") - elif msg["data"]["command"] == "clearGCode": - if self.clearGCode(): - # send blank gcode to UI - self.data.ui_queue.put("Action: gcodeUpdate") - # socketio.emit("gcodeUpdate", {"data": ""}, namespace="/MaslowCNC") + ''' + When a message comes in via the UI client it gets parsed and processed here. + :param msg: json of message from UI client + :return: + ''' + ''' + if msg["data"]["command"] == "optimizeGCode": + if not self.data.gcodeOptimizer.optimize(): + self.data.ui_queue1.put("Alert", "Alert", "Error with optimizing gcode") + ''' + try: + # Commands allowed during sending gcode. These commands won't screw something up. + if msg["data"]["command"] == "createDirectory": + if not self.createDirectory(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with creating directory.") + elif msg["data"]["command"] == "stopZ": + if not self.stopZ(): + self.data.ui_queue1.put("Alert", "Alert", "Error with stopping Z-Axis movement") + elif msg["data"]["command"] == "stopRun": + if not self.stopRun(): + self.data.ui_queue1.put("Alert", "Alert", "Error with stopping run") + elif msg["data"]["command"] == "pauseRun": + if not self.pauseRun(): + self.data.ui_queue1.put("Alert", "Alert", "Error with pausing run") + elif msg["data"]["command"] == "resumeRun": + if not self.resumeRun(): + self.data.ui_queue1.put("Alert", "Alert", "Error with resuming run") + elif msg["data"]["command"] == "toggleCamera": + if not self.toggleCamera(): + self.data.ui_queue1.put("Alert", "Alert", "Error with toggling camera.") + elif msg["data"]["command"] == "statusRequest": + if msg["data"]["arg"] == "cameraStatus": + if not self.cameraStatus(): + self.data.ui_queue1.put("Alert", "Alert", "Error with toggling camera.") + elif msg["data"]["command"] == "queryCamera": + if not self.queryCamera(): + self.data.ui_queue1.put("Alert", "Alert", "Error with querying camera.") + elif msg["data"]["command"] == "shutdown": + if not self.shutdown(): + self.data.ui_queue1.put("Alert", "Alert", "Error with shutting down.") + elif msg["data"]["command"] == "TurnOffRPI": + if not self.turnOff(): + self.data.ui_queue1.put("Alert", "Alert", "Error with system shutting down.") + elif self.data.uploadFlag == 1: + self.data.ui_queue1.put("Alert", "Alert", "Cannot issue command while sending gcode.") + # Commands not allowed during sending gcode.. if you did these commands, something could screw up. + # If uploadFlag was enabled (see above) then this would never be reached. + + elif msg["data"]["command"] == "move": + if not self.move(msg["data"]["arg"], float(msg["data"]["arg1"])): + self.data.ui_queue1.put("Alert", "Alert", "Error with initiating move.") + elif msg["data"]["command"] == "moveTo": + if not self.moveTo(msg["data"]["arg"], float(msg["data"]["arg1"])): + self.data.ui_queue1.put("Alert", "Alert", "Error with initiating move.") + elif msg["data"]["command"] == "moveZ": + if not self.moveZ(msg["data"]["arg"], float(msg["data"]["arg1"])): + self.data.ui_queue1.put("Alert", "Alert", "Error with initiating Z-Axis move.") + elif msg["data"]["command"] == "reportSettings": + self.data.gcode_queue.put("$$") + elif msg["data"]["command"] == "home": + if not self.home(): + self.data.ui_queue1.put("Alert", "Alert", "Error with returning to home.") + elif msg["data"]["command"] == "defineZ0": + if not self.data.actions.defineZ0(): + self.data.ui_queue1.put("Alert", "Alert", "Error with defining Z-Axis zero.") + elif msg["data"]["command"] == "touchZ": + if not self.touchZ(): + self.data.ui_queue1.put("Alert", "Alert", "Error with touching.") + elif msg["data"]["command"] == "moveToDefault": + if not self.moveToDefault(): + self.data.ui_queue1.put("Alert", "Alert", "Error with moving to default chain lengths") + elif msg["data"]["command"] == "returnToCenter": + if not self.returnToCenter(): + self.data.ui_queue1.put("Alert", "Alert", "Error with returning to center") + elif msg["data"]["command"] == "moveGcodeZ": + if not self.moveGcodeZ(int(msg["data"]["arg"])): + self.data.ui_queue1.put("Alert", "Alert", "Error with moving to Z move") + elif msg["data"]["command"] == "moveGcodeGoto": + if not self.moveGcodeIndex(int(msg["data"]["arg"]), True): + self.data.ui_queue1.put("Alert", "Alert", "Error with moving to Z move") + elif msg["data"]["command"] == "moveGcodeIndex": + if not self.moveGcodeIndex(int(msg["data"]["arg"])): + self.data.ui_queue1.put("Alert", "Alert", "Error with moving to index") + elif msg["data"]["command"] == "macro1": + if not self.macro(1): + self.data.ui_queue1.put("Alert", "Alert", "Error with performing macro") + elif msg["data"]["command"] == "macro2": + if not self.macro(2): + self.data.ui_queue1.put("Alert", "Alert", "Error with performing macro") + elif msg["data"]["command"] == "clearLogs": + if not self.clearLogs(): + self.data.ui_queue1.put("Alert", "Alert", "Error clearing log files.") + + elif self.data.uploadFlag > 1 or self.data.uploadFlag < 0 : + self.data.ui_queue1.put("Alert", "Alert", "Cannot issue command while paused sending gcode. You must press STOP before performing this action.") + # Commands not allowed while paused.. if you did these commands, something could screw up. + + elif msg["data"]["command"] == "startRun": + if not self.startRun(): + if len(self.data.gcode) > 0: + self.data.ui_queue1.put("Alert", "Alert", "Error with starting run.") + else: + self.data.ui_queue1.put("Alert", "Alert", "No GCode file loaded.") + elif msg["data"]["command"] == "update": + if not self.data.releaseManager.update(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with updating webcontrol.") + return "Shutdown" + elif msg["data"]["command"] == "cutTriangularCalibrationPattern": + if not self.data.triangularCalibration.cutTriangularCalibrationPattern(): + self.data.ui_queue1.put("Alert", "Alert", "Error with cutting triangular calibration pattern.") + elif msg["data"]["command"] == "acceptTriangularCalibrationResults": + if not self.data.triangularCalibration.acceptTriangularCalibrationResults(): + self.data.ui_queue1.put("Alert", "Alert", "Error with accepting triangular calibration results.") + elif msg["data"]["command"] == "cutHoleyCalibrationPattern": + if not self.data.holeyCalibration.CutTestPattern(): + self.data.ui_queue1.put("Alert", "Alert", "Error with cutting holey calibration pattern.") + elif msg["data"]["command"] == "acceptHoleyCalibrationResults": + if not self.data.holeyCalibration.acceptCalibrationResults(): + self.data.ui_queue1.put("Alert", "Alert", "Error with accepting holey calibration results.") + elif msg["data"]["command"] == "resetChainLengths": + if not self.resetChainLengths(): + self.data.ui_queue1.put("Alert", "Alert", "Error with resetting chain lengths.") + elif msg["data"]["command"] == "defineHome": + print(self.data.uploadFlag) + posX= msg["data"]["arg"] + posY= msg["data"]["arg1"] + if self.defineHome(posX, posY): + ## the gcode file might change the active units so we need to inform the UI of the change. + self.data.ui_queue1.put("Action", "unitsUpdate", "") + self.data.ui_queue1.put("Action", "gcodeUpdate", "") + else: + self.data.ui_queue1.put("Alert", "Alert", "Error with defining home.") + elif msg["data"]["command"] == "testMotors": + if not self.testMotors(): + self.data.ui_queue1.put("Alert", "Alert", "Error with testing motors") + elif msg["data"]["command"] == "wipeEEPROM": + if not self.wipeEEPROM(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with wiping EEPROM") + elif msg["data"]["command"] == "clearGCode": + if self.clearGCode(): + # send blank gcode to UI + self.data.ui_queue1.put("Action", "gcodeUpdate", "") + else: + self.data.ui_queue1.put("Alert", "Alert", "Error with clearing gcode") + elif msg["data"]["command"] == "setSprockets": + if not self.setSprockets(msg["data"]["arg"], msg["data"]["arg1"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with setting sprocket") + elif msg["data"]["command"] == "rotateSprocket": + if not self.rotateSprocket(msg["data"]["arg"], msg["data"]["arg1"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with setting sprocket") + elif msg["data"]["command"] == "setSprocketAutomatic": + if not self.setSprocketAutomatic(): + self.data.ui_queue1.put("Alert", "Alert", "Error with setting sprockets automatically") + elif msg["data"]["command"] == "setSprocketsZero": + if not self.setSprocketsZero(): + self.data.ui_queue1.put("Alert", "Alert", "Error with setting sprockets zero value") + elif msg["data"]["command"] == "setSprocketsDefault": + if not self.setSprocketsDefault(): + self.data.ui_queue1.put("Alert", "Alert", "Error with setting sprockets as default") + elif msg["data"]["command"] == "updatePorts": + if not self.updatePorts(): + self.data.ui_queue1.put("Alert", "Alert", "Error with updating list of ports") + elif msg["data"]["command"] == "optical_onStart": + pass + #if not self.data.opticalCalibration.on_Start(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with starting optical calibration") + elif msg["data"]["command"] == "optical_Calibrate": + pass + #if not self.data.opticalCalibration.on_Calibrate(msg["data"]["arg"]): + # self.data.ui_queue1.put("Alert", "Alert", "Error with starting optical calibration") + elif msg["data"]["command"] == "saveOpticalCalibrationConfiguration": + pass + #if not self.data.opticalCalibration.saveOpticalCalibrationConfiguration(msg["data"]["arg"]): + # self.data.ui_queue1.put("Alert", "Alert", "Error with saving optical calibration configuration") + elif msg["data"]["command"] == "stopOpticalCalibration": + pass + #if not self.data.opticalCalibration.stopOpticalCalibration(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with stopping optical calibration.") + elif msg["data"]["command"] == "testOpticalCalibration": + pass + #if not self.data.opticalCalibration.testImage(msg["data"]["arg"]): + # self.data.ui_queue1.put("Alert", "Alert", "Error with test image.") + elif msg["data"]["command"] == "findCenterOpticalCalibration": + pass + #if not self.data.opticalCalibration.findCenter(msg["data"]["arg"]): + # self.data.ui_queue1.put("Alert", "Alert", "Error with find Center.") + elif msg["data"]["command"] == "saveAndSendOpticalCalibration": + pass + #if not self.data.opticalCalibration.saveAndSend(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with saving and sending calibration matrix.") + elif msg["data"]["command"] == "reloadCalibration": + pass + ''' + errorX, errorY, curveX, curveY = self.data.opticalCalibration.reloadCalibration() + if errorX is None or errorY is None or curveX is None or curveY is None: + self.data.ui_queue1.put("Alert", "Alert", "Error with (re)loading calibration.") + else: + data = {"errorX": errorX, "errorY": errorY} + self.data.ui_queue1.put("Action", "updateOpticalCalibrationError", data) + ''' + elif msg["data"]["command"] == "saveCalibrationToCSV": + pass + #if not self.data.opticalCalibration.saveCalibrationToCSV(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with saving calibration to CSV.") + elif msg["data"]["command"] == "clearCalibration": + pass + #if not self.data.opticalCalibration.clearCalibration(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with clearing calibration.") + elif msg["data"]["command"] == "curveFitOpticalCalibration": + pass + ''' + curveX, curveY = self.data.opticalCalibration.surfaceFit() + #curveX, curveY = self.data.opticalCalibration.polySurfaceFit() + if curveX is None or curveY is None: + self.data.ui_queue1.put("Alert", "Alert", "Error with curve fitting calibration data.") + else: + data = {"curveX": curveX, "curveY": curveY} + self.data.ui_queue1.put("Action", "updateOpticalCalibrationCurve", data) + ''' + elif msg["data"]["command"] == "adjustCenter": + if not self.adjustCenter(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with adjusting center.") + elif msg["data"]["command"] == "upgradeCustomFirmware": + if not self.upgradeFirmware(0): + self.data.ui_queue1.put("Alert", "Alert", "Error with upgrading custom firmware.") + else: + self.data.ui_queue1.put("Alert", "Alert", "Custom firmware update complete.") + elif msg["data"]["command"] == "upgradeStockFirmware": + if not self.upgradeFirmware(1): + self.data.ui_queue1.put("Alert", "Alert", "Error with upgrading stock firmware.") + else: + self.data.ui_queue1.put("Alert", "Alert", "Stock firmware update complete.") + elif msg["data"]["command"] == "upgradeHoleyFirmware": + if not self.upgradeFirmware(2): + self.data.ui_queue1.put("Alert", "Alert", "Error with upgrading holey firmware.") + else: + self.data.ui_queue1.put("Alert", "Alert", "Custom firmware update complete.") + elif msg["data"]["command"] == "adjustChain": + if not self.adjustChain(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with adjusting chain.") + elif msg["data"]["command"] == "executeVelocityPIDTest": + if not self.velocityPIDTest(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with executing velocity PID test.") + elif msg["data"]["command"] == "executePositionPIDTest": + if not self.positionPIDTest(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with executing velocity PID test.") + elif msg["data"]["command"] == "boardProcessGCode": + if not self.data.boardManager.processGCode(): + self.data.ui_queue1.put("Alert", "Alert", "Error with processing gcode") + elif msg["data"]["command"] == "boardClearBoard": + if not self.data.boardManager.clearBoard(): + self.data.ui_queue1.put("Alert", "Alert", "Error with clearing board") + elif msg["data"]["command"] == "updatePyInstaller": + if not self.data.releaseManager.updatePyInstaller(): + self.data.ui_queue1.put("Alert", "Alert", "Error with updating WebControl") + return "Shutdown" + elif msg["data"]["command"] == "setFakeServo": + if not self.setFakeServo(msg["data"]["arg"]): + self.data.ui_queue1.put("Alert", "Alert", "Error with changing Fake Servo") + elif msg["data"]["command"] == "resetHomeToCenter": + if self.defineHome(0, 0): + ## notify UI of home change to request gcode update + self.data.ui_queue1.put("Action", "gcodeUpdate", "") + else: + self.data.ui_queue1.put("Alert", "Alert", "Error with resetting home to center") + #elif msg["data"]["command"] == "optimizeGCode": + # if not self.data.gcodeOptimizer.optimize(): + # self.data.ui_queue1.put("Alert", "Alert", "Error with optimizing gcode") else: - self.data.ui_queue.put("Message: Error with clearing gcode") - elif msg["data"]["command"] == "moveGcodeZ": - if not self.moveGcodeZ(int(msg["data"]["arg"])): - self.data.ui_queue.put("Message: Error with moving to Z move") - elif ( - msg["data"]["command"] == "moveGcodeIndex" - or msg["data"]["command"] == "moveGcodeZ" - ): - if not self.moveGcodeIndex(int(msg["data"]["arg"])): - self.data.ui_queue.put("Message: Error with moving to index") - elif msg["data"]["command"] == "setSprockets": - if not self.setSprockets(msg["data"]["arg"], msg["data"]["arg1"]): - self.data.ui_queue.put("Message: Error with setting sprocket") - elif msg["data"]["command"] == "setSprocketsAutomatic": - if not self.setSprocketsAutomatic(): - self.data.ui_queue.put( - "Message: Error with setting sprockets automatically" - ) - elif msg["data"]["command"] == "setSprocketsZero": - if not self.setSprocketsZero(): - self.data.ui_queue.put( - "Message: Error with setting sprockets zero value" - ) - elif msg["data"]["command"] == "updatePorts": - if not self.updatePorts(): - self.data.ui_queue.put("Message: Error with updating list of ports") - elif msg["data"]["command"] == "macro1": - if not self.macro(1): - self.data.ui_queue.put("Message: Error with performing macro") - elif msg["data"]["command"] == "macro2": - if not self.macro(2): - self.data.ui_queue.put("Message: Error with performing macro") - elif msg["data"]["command"] == "optical_onStart": - if not self.data.opticalCalibration.on_Start(): - self.data.ui_queue.put( - "Message: Error with starting optical calibration" - ) - elif msg["data"]["command"] == "optical_Calibrate": - print("here") - if not self.data.opticalCalibration.on_Calibrate(msg["data"]["arg"]): - self.data.ui_queue.put( - "Message: Error with starting optical calibration" - ) + response = "Function not currently implemented.. Sorry." + response = response + "["+msg["data"]["command"]+"]" + self.data.ui_queue1.put("Alert", "Alert", response) + print(msg["data"]) + except Exception as e: + print(str(e)) + - def defineHome(self): + def shutdown(self): + ''' + If running docker, sends message to WebMCP to shutdown webcontrol. + If running as pyinstaller, calls an exit function. + TODO: Add option to shutdown RPI completely. + :return: + ''' + try: + print(self.data.platform) + if self.data.platform == "PYINSTALLER": + os._exit(0) + else: + self.data.ui_queue1.put("WebMCP", "shutdown", "") + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def turnOff(self): + ''' this is the rpi soft power button function''' + try: + oname = os.name + print ("OS is ", oname) + if oname == "posix": + print(os.system("uname -a")) + # insert other possible OSes here + # ... NEED ARE YOU SURE YOU WANT POWER DOWN? + call("sudo --non-interactive shutdown -h now", shell=True) + return(True) #this should never run + else: + print("unknown OS") + print(self.data.platform) + #print(os.uname) + return(False) + except: + print("shutdown failure") + return(False) + + def defineHome(self, posX="", posY=""): + ''' + Redefines the home location and sends message to update the UI client. In a break from ground control, this + does not alter the gcode. Gcode is altered by the home location only when sent to the controller. + posX and posY define the coordinates of home. The are populated via the right-click context menu if the user + uses the mouse to define home. They are empty if the user presses the define home button on the frontpage + menu, in which case the coordinates of the sled are used. + :param posX: + :param posY: + :return: + ''' try: if self.data.units == "MM": scaleFactor = 25.4 else: scaleFactor = 1.0 - self.data.gcodeShift = [ - self.data.xval / scaleFactor, - self.data.yval / scaleFactor, - ] - self.data.config.setValue("Advanced Settings", "homeX", str(self.data.xval)) - self.data.config.setValue("Advanced Settings", "homeY", str(self.data.yval)) - self.data.gcodeFile.loadUpdateFile() + # if posX and posY have values, use them, else use the sled's position. + if posX != "" and posY != "": + homeX = round(posX * scaleFactor, 4) + homeY = round(posY * scaleFactor, 4) + else: + homeX = round(self.data.xval, 4) + homeY = round(self.data.yval, 4) + + self.data.config.setValue("Advanced Settings", "homeX", str(homeX)) + self.data.config.setValue("Advanced Settings", "homeY", str(homeY)) + + # the moveLine function of gcodeFile is still used (though its called directly by serialThread) so I still + # track the home coordinates in gcodeShift. + self.data.gcodeShift = [ homeX, homeY ] + # send update to UI client with new home position + position = {"xval": homeX, "yval": homeY} + self.data.ui_queue1.put("Action", "homePositionMessage", position) return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def home(self): + ''' + Directs controller to move to home location + :return: + ''' try: + self.data.sledMoving = True self.data.gcode_queue.put("G90 ") - # todo:self.gcodeVel = "[MAN]" safeHeightMM = float( self.data.config.getValue("Maslow Settings", "zAxisSafeHeight") ) - safeHeightInches = safeHeightMM / 25.5 + safeHeightInches = safeHeightMM / 25.4 if self.data.units == "INCHES": self.data.gcode_queue.put("G00 Z" + "%.3f" % (safeHeightInches)) else: self.data.gcode_queue.put("G00 Z" + str(safeHeightMM)) + homeX = self.data.config.getValue("Advanced Settings", "homeX") + homeY = self.data.config.getValue("Advanced Settings", "homeY") self.data.gcode_queue.put( "G00 X" - + str(self.data.gcodeShift[0]) + + str(homeX) + " Y" - + str(self.data.gcodeShift[1]) + + str(homeY) + " " ) self.data.gcode_queue.put("G00 Z0 ") + self.data.sledMoving = False return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) + self.data.sledMoving = False return False def resetChainLengths(self): + ''' + Sends B08 command to controller to tell controller chains are at their 'extendChainLengths' distance. + (equivalent to ground control's manualCalibrateChainLengths function) + :return: + ''' try: self.data.gcode_queue.put("B08 ") + # not sure why this was added here. Probably not needed. Todo: cleanup if not needed. + if self.data.units == "INCHES": + self.data.gcode_queue.put("G20 ") + else: + self.data.gcode_queue.put("G21 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def defineZ0(self): + ''' + Sends G10 Z0 to controller to set current Z height as zero. + :return: + ''' try: self.data.gcode_queue.put("G10 Z0 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def stopZ(self): + ''' + Sends a stop signal to the controller and clears the gcode queue. This is called from the Z-Axis popup, not + the frontpage. This is equivalent to ground control's stopZMove. + :return: + ''' try: + print("z!") self.data.quick_queue.put("!") with self.data.gcode_queue.mutex: self.data.gcode_queue.queue.clear() + self.data.Zmoving = False return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def startRun(self): + ''' + Starts the process of sending the gcode to the controller. + :return: + ''' try: - self.data.uploadFlag = 1 - self.data.gcode_queue.put(self.data.gcode[self.data.gcodeIndex]) - self.data.gcodeIndex += 1 - return True + if len(self.data.gcode) > 0: + # set current Z target to the current z height in case gcode doesn't include a z move before an xy move. + # if it doesn't and the user pauses during an xy move, then the target Z is set to 0. This sets it to + # what it currently is when the user started the gcode send. + self.data.currentZTarget = self.data.zval + # if the gcode index is not 0, then make sure the machine is in the proper state before starting to send + # the gcode. + if self.data.gcodeIndex > 0: + # get machine into proper state by sending appropriate commands + self.processGCode() + # update the gcode position on the UI client.. Have to recalculate it from the gcode because + # starting at some place other than 0 + self.sendGCodePositionUpdate(recalculate=True) + self.data.uploadFlag = 1 + else: + self.data.uploadFlag = 1 + if (self.data.GPIOButtonService == False): + self.data.gpioActions.causeAction("PlayLED", "on") + return True + else: + return False except Exception as e: - print(e) + # something goes wrong, stop uploading. + self.data.console_queue.put(str(e)) self.data.uploadFlag = 0 self.data.gcodeIndex = 0 return False def stopRun(self): + ''' + Stops the uploading of gcode. + :return: + ''' + print("stopping run") try: - print("trying to stop run") + self.data.console_queue.put("stopping run") + # this is necessary because of the way PID data is being processed. Otherwise could potentially get stuck + # in PID test + self.data.inPIDPositionTest = False + self.data.inPIDVelocityTest = False self.data.uploadFlag = 0 self.data.gcodeIndex = 0 + self.data.manualZAxisAdjust = False self.data.quick_queue.put("!") with self.data.gcode_queue.mutex: self.data.gcode_queue.queue.clear() - # TODO: app.onUploadFlagChange(self.stopRun, 0) - print("Gcode Stopped") + # TODO: app.onUploadFlagChange(self.stopRun, 0) edit: not sure this is needed anymore + self.data.console_queue.put("Gcode stopped") + self.sendGCodePositionUpdate(self.data.gcodeIndex) + # notify UI client to clear any alarm that's active because a stop has been process. + self.data.ui_queue1.put("Action", "clearAlarm", "") + if (self.data.GPIOButtonService == False): + self.data.gpioActions.causeAction("StopLED", "on") + # reset pause + self.data.ui_queue1.put("Action", "setAsPause", "") + if (self.data.GPIOButtonService == False): + self.data.gpioActions.causeAction("PauseLED", "off") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def moveToDefault(self): + ''' + Moves the sled to the spot where the chains are extended to the extendChainLength. Not regularly used, but + more for testing. Uses B09 commands (single axis moves). + :return: + ''' try: chainLength = self.data.config.getValue( "Advanced Settings", "chainExtendLength" @@ -213,51 +540,161 @@ def moveToDefault(self): "B09 R" + str(chainLength) + " L" + str(chainLength) + " " ) self.data.gcode_queue.put("G91 ") + + # the firmware doesn't update sys.xPosition or sys.yPosition during a singleAxisMove + # therefore, the machine doesn't know where it is. + # so tell the machine where it's at by sending a reset chain length + self.data.gcode_queue.put("B08 ") + return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def testMotors(self): + ''' + Sends the test motors/encoder command, B04 + :return: + ''' try: self.data.gcode_queue.put("B04 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False - def wipeEEPROM(self): + def wipeEEPROM(self, extent): + ''' + Sends the wipe EEPROM commands + :param extent: + :return: + ''' try: - self.data.gcode_queue.put("$RST=* ") - timer = threading.Timer(6.0, self.data.gcode_queue.put("$$")) - timer.start() + if extent == "All": + self.data.gcode_queue.put("$RST=* ") + elif extent == "Settings": + self.data.gcode_queue.put("$RST=$ ") + elif extent == "Maslow": + self.data.gcode_queue.put("$RST=# ") + else: + return False + + #sync settings after 2 seconds (give time form controller to reset) + time.sleep(2) + self.data.gcode_queue.put("$$") + + #reset chain lengths so they aren't zero + if extent == "All" or extent == "Maslow": + self.resetChainLengths() + + # these two lines were commented out and aren't used (and may not even work). The thought was that after + # the EEPROM got wiped, you need to sync settings. + #self.timer = threading.Timer(6.0, self.data.gcode_queue.put("$$")) + #self.timer.start() return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def pauseRun(self): + ''' + Pause the current uploading of gcode. Notify UI client to change the Pause button to say Resume + :return: + ''' + print("pause run selected") try: - self.data.uploadFlag = 0 - print("Run Paused") - self.data.ui_queue.put("Action: setAsResume") + if self.data.uploadFlag == 1: + self.data.uploadFlag = 2 + self.data.console_queue.put("Run Paused") + self.data.ui_queue1.put("Action", "setAsResume", "") + # The idea was to be able to make sure the machine returns to + # the correct z-height after a pause in the event the user raised/lowered the bit. + #self.data.pausedzval = self.data.zval + #self.data.pausedUnits = self.data.units + self.data.pausedzval = self.data.currentZTarget + self.data.pausedUnits = self.data.units + self.data.pausedPositioningMode = self.data.positioningMode + #print("Saving paused positioning mode: " + str(self.data.pausedPositioningMode)) + if (self.data.GPIOButtonService == False): + self.data.gpioActions.causeAction("PauseLED", "on") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def resumeRun(self): + ''' + Resumes sending the gcode. If a tool change command was received, then the manualZAxisAdjust is enabled and + the machine will be returned to the z-axis height it was when the tool change command was processed. It also + makes sure the units are what they were prior to the tool change (user might have switched them up while + setting z-axis to zero). + :return: + ''' try: - self.data.uploadFlag = 1 - # send cycle resume command to unpause the machine + # Restore self.data.upladFlag properly + if self.data.manualZAxisAdjust: + # Z-axis is disabled and requires manual adjustment. + print("Resume run with manual z-axis adjust.") + # Clear the flag. + self.data.manualZAxisAdjust = False + # Reenable the uploadFlag if it was previous set. + if self.data.previousUploadStatus == -1: + # If this was M command pause, then set to 1. + self.data.uploadFlag = 1 + else: + self.data.uploadFlag = self.data.previousUploadStatus ### just moved this here from after if statement + else: + # User has paused and is now resuming. + # User could have used UI to move z-axis so restore paused values. + print("Resume run without manual z-axis adjust.") + # Restore units. + if self.data.pausedUnits is not None and self.data.pausedUnits != self.data.units: + print("Restoring units to:" + str(self.data.pausedUnits)) + if self.data.pausedUnits == "INCHES": + self.data.gcode_queue.put("G20 ") + elif self.data.pausedUnits == "MM": + self.data.gcode_queue.put("G21 ") + self.data.pausedUnits = None + + # Move the z-axis back to where it was. + if self.data.pausedzval is not None and self.data.pausedzval != self.data.zval: + # Put in absolute mode to make z axis move. + self.data.gcode_queue.put("G90 ") + # THE ABOVE COMMAND IS NOT EXECUTED IN LINE AND REQUIRES THE FOLLOWING TO TRACK POSITIONING MODE + self.data.positioningMode = 0 + print("Restoring paused Z value: " + str(self.data.pausedzval)) + self.data.gcode_queue.put("G0 Z" + str(self.data.pausedzval) + " ") + self.data.pausedzval = None + + # Restore the last gcode positioning mode in use before pauseRun executed. + if self.data.pausedPositioningMode is not None and self.data.positioningMode != self.data.pausedPositioningMode: + print("Restoring positioning mode: " + str(self.data.pausedPositioningMode)) + if self.data.pausedPositioningMode == 0: + # this line technically should be unreachable + self.data.gcode_queue.put("G90 ") + elif self.data.pausedPositioningMode == 1: + self.data.gcode_queue.put("G91 ") + self.data.pausedPositioningMode = None + + self.sendGCodePositionUpdate(self.data.gcodeIndex, recalculate=True) + self.data.uploadFlag = 1 + + # Send cycle resume command to unpause the machine. + # Only needed if user initiated pause; but doesn't actually cause harm to controller. self.data.quick_queue.put("~") - self.data.ui_queue.put("Action: setAsPause") + self.data.ui_queue1.put("Action", "setAsPause", "") + if (self.data.GPIOButtonService == False): + self.data.gpioActions.causeAction("PauseLED", "off") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def returnToCenter(self): + ''' + Instructs controller to move sled to 0,0 at safe height. + :return: + ''' try: self.data.gcode_queue.put("G90 ") safeHeightMM = float( @@ -271,20 +708,30 @@ def returnToCenter(self): self.data.gcode_queue.put("G00 X0.0 Y0.0 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def clearGCode(self): + ''' + Calls function to clear the gcode file. Probably could have just been called directly from processAction + :return: + ''' try: - self.data.gcodeFile = "" + self.data.gcodeFile.clearGcodeFile() return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def moveGcodeZ(self, moves): + ''' + Moves the gcode index to the next z-axis move. + :param moves: + :return: + ''' try: dist = 0 + #determine the number of lines to move to reach the next Z-axis move. for index, zMove in enumerate(self.data.zMoves): if moves > 0 and zMove > self.data.gcodeIndex: dist = self.data.zMoves[index + moves - 1] - self.data.gcodeIndex @@ -297,15 +744,23 @@ def moveGcodeZ(self, moves): else: return False except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False - def moveGcodeIndex(self, dist): + def moveGcodeIndex(self, dist, index=False): + ''' + Moves the gcodeIndex by either the distance or, in index is True, uses the dist as the index. + :param dist: + :param index: + :return: + ''' try: maxIndex = len(self.data.gcode) - 1 - targetIndex = self.data.gcodeIndex + dist + if index is True: + targetIndex = dist + else: + targetIndex = self.data.gcodeIndex + dist - # print "targetIndex="+str(targetIndex) # check to see if we are still within the length of the file if maxIndex < 0: # break if there is no data to read return @@ -317,159 +772,277 @@ def moveGcodeIndex(self, dist): self.data.gcodeIndex = maxIndex else: self.data.gcodeIndex = targetIndex - gCodeLine = self.data.gcode[self.data.gcodeIndex] - # print self.data.gcode - # print "gcodeIndex="+str(self.data.gcodeIndex)+", gCodeLine:"+gCodeLine - xTarget = 0 - yTarget = 0 try: - x = re.search("X(?=.)([+-]?([0-9]*)(\.([0-9]+))?)", gCodeLine) - if x: - xTarget = float(x.groups()[0]) - self.data.previousPosX = xTarget - else: - xTarget = self.data.previousPosX - - y = re.search("Y(?=.)([+-]?([0-9]*)(\.([0-9]+))?)", gCodeLine) - - if y: - yTarget = float(y.groups()[0]) - self.data.previousPosY = yTarget - else: - yTarget = self.data.previousPosY - # self.gcodecanvas.positionIndicator.setPos(xTarget,yTarget,self.data.units) - # print "xTarget:"+str(xTarget)+", yTarget:"+str(yTarget) - position = {"xval": xTarget, "yval": yTarget, "zval": self.data.zval} - self.data.ui_queue.put( - "Action: positionMessage:_" + json.dumps(position) - ) # the "_" facilitates the parse + # send gcode position update to UI client + retval = self.sendGCodePositionUpdate(recalculate=True) + return retval except Exception as e: - print(e) - print("Unable to update position for new gcode line") + self.data.console_queue.put(str(e)) + self.data.console_queue.put("Unable to update position for new gcode line") return False return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def move(self, direction, distToMove): + ''' + Issues commands to move the sled + :param direction: direction to move + :param distToMove: distance to move + :return: + ''' + # if enabled, for diagonal movements, the x and y move distance will be calculated such that the total distance + # moved equals what was specified. For example, if enabled and command is issued to move one foot to top right, + # then the x and y coordinates will be calculated as the 0.707 feet so that a total of 1 foot is moved. If + # disabled, then the sled will move 1 foot left and 1 foot up, for a total distance of 1.414 feet. + if self.data.config.getValue("WebControl Settings","diagonalMove") == 1: + diagMove = round(math.sqrt(distToMove*distToMove/2.0), 4) + else: + diagMove = distToMove try: + self.data.sledMoving = True + self.data.gcode_queue.put("G91 ") if direction == "upLeft": - self.data.gcode_queue.put( - "G91 G00 X" - + str(-1.0 * distToMove) - + " Y" - + str(distToMove) - + " G90 " - ) + self.data.gcode_queue.put("G00 X"+ str(-1.0 * diagMove)+ " Y"+ str(diagMove)+ " ") elif direction == "up": - self.data.gcode_queue.put("G91 G00 Y" + str(distToMove) + " G90 ") + self.data.gcode_queue.put("G00 Y" + str(distToMove) + " ") elif direction == "upRight": - self.data.gcode_queue.put( - "G91 G00 X" + str(distToMove) + " Y" + str(distToMove) + " G90 " - ) + self.data.gcode_queue.put("G00 X" + str(diagMove) + " Y" + str(diagMove) + " ") elif direction == "left": - self.data.gcode_queue.put( - "G91 G00 X" + str(-1.0 * distToMove) + " G90 " - ) + self.data.gcode_queue.put("G00 X" + str(-1.0 * distToMove) + " ") elif direction == "right": - self.data.gcode_queue.put("G91 G00 X" + str(distToMove) + " G90 ") + self.data.gcode_queue.put("G00 X" + str(distToMove) + " ") elif direction == "downLeft": - self.data.gcode_queue.put( - "G91 G00 X" - + str(-1.0 * distToMove) - + " Y" - + str(-1.0 * distToMove) - + " G90 " - ) + self.data.gcode_queue.put("G00 X" + str(-1.0 * diagMove) + " Y" + str(-1.0 * diagMove) + " ") elif direction == "down": - self.data.gcode_queue.put( - "G91 G00 Y" + str(-1.0 * distToMove) + " G90 " - ) + self.data.gcode_queue.put("G00 Y" + str(-1.0 * distToMove) + " ") elif direction == "downRight": - self.data.gcode_queue.put( - "G91 G00 X" - + str(distToMove) - + " Y" - + str(-1.0 * distToMove) - + " G90 " - ) + self.data.gcode_queue.put("G00 X" + str(diagMove) + " Y" + str(-1.0 * diagMove) + " ") + else: + return False + self.data.gcode_queue.put("G90 ") + # keep track of the distToMove value + self.data.config.setValue("Computed Settings", "distToMove", distToMove) + self.data.sledMoving = False return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) + self.data.sledMoving = False return False def moveZ(self, direction, distToMoveZ): + ''' + Moves the Z-Axis a distance and direction + :param direction: + :param distToMoveZ: + :return: + ''' + try: - # distToMoveZ = float(msg['data']['distToMoveZ']) + #keep track of distToMoveZ value self.data.config.setValue("Computed Settings", "distToMoveZ", distToMoveZ) + # It's possible the front page is set for one units and when you go to z-axis control, you might switch + # to a different unit. Webcontrol keeps these units separate, so we need to make sure the machine is in + # the correct units when the z-axis move is sent unitsZ = self.data.config.getValue("Computed Settings", "unitsZ") + previousUnits = self.data.config.getValue("Computed Settings", "units") + self.data.zMoving = True if unitsZ == "MM": self.data.gcode_queue.put("G21 ") else: self.data.gcode_queue.put("G20 ") if direction == "raise": - self.data.gcode_queue.put( - "G91 G00 Z" + str(float(distToMoveZ)) + " G90 " - ) + self.data.gcode_queue.put("G91 ") + self.data.gcode_queue.put("G00 Z" + str(float(distToMoveZ)) + " ") + self.data.gcode_queue.put("G90 ") elif direction == "lower": - self.data.gcode_queue.put( - "G91 G00 Z" + str(-1.0 * float(distToMoveZ)) + " G90 " - ) - units = self.data.config.getValue("Computed Settings", "units") - if units == "MM": - self.data.gcode_queue.put("G21 ") - else: - self.data.gcode_queue.put("G20 ") + self.data.gcode_queue.put("G91 ") + self.data.gcode_queue.put("G00 Z" + str(-1.0 * float(distToMoveZ)) + " " ) + self.data.gcode_queue.put("G90 ") + # now, since we might have changed the units of the machine, make sure they are set back to what it was + # originally. + #units = self.data.config.getValue("Computed Settings", "units") + if previousUnits != unitsZ: + if previousUnits == "MM": + self.data.gcode_queue.put("G21 ") + else: + self.data.gcode_queue.put("G20 ") + self.data.zMoving = False return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) + self.data.zMoving = False return False - def updateSetting(self, setting, value): + + def touchZ(self): + ''' + Sends a gcode line to set z axis depth using touch plate. I've not personally tested this. + :return: + ''' try: - if setting == "toInches": - self.data.units = "INCHES" - self.data.config.setValue("Computed Settings", "units", self.data.units) - scaleFactor = 1.0 - self.data.gcodeShift = [ - self.data.gcodeShift[0] / scaleFactor, - self.data.gcodeShift[1] / scaleFactor, - ] - self.data.tolerance = 0.020 - self.data.gcode_queue.put("G20 ") - self.data.config.setValue("Computed Settings", "distToMove", value) - elif setting == "toMM": - self.data.units = "MM" - self.data.config.setValue("Computed Settings", "units", self.data.units) - scaleFactor = 25.4 + plungeDepth = self.data.config.getValue("Advanced Settings", "maxTouchProbePlungeDistance") + revertToInches = False + self.data.zMoving = True + if self.data.units == "INCHES": + revertToInches = True + self.data.gcode_queue.put("G21") + self.data.gcode_queue.put("G90 G38.2 Z-" + plungeDepth + " F1 M02") + if revertToInches: + self.data.gcode_queue.put("G20") + # don't think this line is needed + # todo: remove if not needed. + self.data.measureRequest = self.defineZ0() + self.data.zMoving = False + return True + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.zMoving = False + return False + + def updateSetting(self, setting, value, fromGcode = False): + ''' + update settings that come from frontpage or from the gcode file. + :param setting: + :param value: + :param fromGcode: + :return: + ''' + try: + self.data.console_queue.put("at update setting from gcode("+str(fromGcode)+"): "+setting+" with value: "+str(value)) + # if front page button has been pressed or serialPortThread is going to send gcode with a G21 or G20.. + if setting == "toInches" or setting == "toMM": + # this shouldn't be reached any more after I reordered the processActions function + # todo: remove this if if not needed. + if self.data.uploadFlag == 1 and fromGcode == False: + self.data.ui_queue1.put("Alert", "Alert", "Cannot change units while sending gcode.") + return True + scaleFactor = 0 + if fromGcode: + # if from a gcode line, then update the get the distToMove value from storage and figure out the + # scaleFactor + value = float(self.data.config.getValue("Computed Settings", "distToMove")) + if self.data.units == "INCHES": + if setting == "toMM": + value = value * 25.4 + scaleFactor = 25.4 + else: + scaleFactor = 1.0 + if self.data.units == "MM": + if setting == "toInches": + value = value / 25.4 + scaleFactor = 1.0 / 25.4 + else: + scaleFactor = 1.0 + + if setting == "toInches": + # was metric, now going to imperial + self.data.units = "INCHES" + self.data.config.setValue("Computed Settings", "units", self.data.units) + # if scaleFactor == 0, then it wasn't previously set by the if fromGcode. + if scaleFactor == 0: + scaleFactor = 1.0/25.4 + self.data.tolerance = 0.020 + self.data.config.setValue("Computed Settings", "tolerance", self.data.tolerance) + # only send G20 to the controller if not already sending G20 + if not fromGcode: + self.data.gcode_queue.put("G20 ") + else: + # was imperial, now going to metric + self.data.units = "MM" + self.data.config.setValue("Computed Settings", "units", self.data.units) + # if scaleFactor == 0, then it wasn't previously set by the if fromGcode. + if scaleFactor == 0: + scaleFactor = 25.4 + self.data.tolerance = 0.5 + self.data.config.setValue("Computed Settings", "tolerance", self.data.tolerance) + # only send G21 to the controller if not already sending G21 + if not fromGcode: + self.data.gcode_queue.put("G21 ") + # update the gcodeShift coordinates to match units self.data.gcodeShift = [ - self.data.gcodeShift[0] / scaleFactor, - self.data.gcodeShift[1] / scaleFactor, + self.data.gcodeShift[0] * scaleFactor, + self.data.gcodeShift[1] * scaleFactor, ] - self.data.tolerance = 0.5 - self.data.gcode_queue.put("G21") + # The UI client sending this has already converted the distToMove to the correct value for the units self.data.config.setValue("Computed Settings", "distToMove", value) + # get the current homeX and homeY coordinates + oldHomeX = float(self.data.config.getValue("Advanced Settings", "homeX")) + oldHomeY = float(self.data.config.getValue("Advanced Settings", "homeY")) + # convert homeX and homeY to the correct units and save + self.data.config.setValue("Advanced Settings", "homeX", oldHomeX * scaleFactor) + self.data.config.setValue("Advanced Settings", "homeY", oldHomeY * scaleFactor) + # notify all UI clients that units have been changed and distToMove has been changed + self.data.ui_queue1.put("Action", "unitsUpdate", "") + self.data.ui_queue1.put("Action", "distToMoveUpdate", "") + # update the home position.. + position = {"xval": oldHomeX * scaleFactor, "yval": oldHomeY * scaleFactor} + self.data.ui_queue1.put("Action", "homePositionMessage", position) + # update the gcode position + self.sendGCodePositionUpdate(recalculate=True) elif setting == "toInchesZ": - self.data.units = "INCHES" + # this shouldn't be reached any more after I reordered the processActions function + # todo: remove this if if not needed. + if self.data.uploadFlag == 1: + self.data.ui_queue1.put("Alert", "Alert", "Cannot change units while sending gcode.") + return True + + #self.data.units = "INCHES" ### commented this out and added the unitZ self.data.config.setValue( - "Computed Settings", "unitsZ", self.data.units + "Computed Settings", "unitsZ", "INCHES" ) self.data.config.setValue("Computed Settings", "distToMoveZ", value) + self.data.ui_queue1.put("Action", "unitsUpdateZ", "") + self.data.ui_queue1.put("Action", "distToMoveUpdateZ", "") + elif setting == "toMMZ": - self.data.units = "MM" + # this shouldn't be reached any more after I reordered the processActions function + # todo: remove this if if not needed. + if self.data.uploadFlag == 1: + self.data.ui_queue1.put("Alert", "Alert", "Cannot change units while sending gcode.") + return True + #self.data.units = "MM" ### commented this out self.data.config.setValue( - "Computed Settings", "unitsZ", self.data.units + "Computed Settings", "unitsZ", "MM" ) self.data.config.setValue("Computed Settings", "distToMoveZ", value) + self.data.ui_queue1.put("Action", "unitsUpdateZ", "") + self.data.ui_queue1.put("Action", "distToMoveUpdateZ", "") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False + def rotateSprocket(self, sprocket, time): + ''' + Turns sprocket at 100 speed for given amount of time using B11 command + This command is used in PID tuning to try to get sprockets in a position without using encoder. + :param sprocket: + :param time: + :return: + ''' + try: + if time > 0: + self.data.gcode_queue.put("B11 "+sprocket+" S100 T"+str(time)) + else: + self.data.gcode_queue.put("B11 "+sprocket+" S-100 T"+str(abs(time))) + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def setSprockets(self, sprocket, degrees): + ''' + Moves sprocket a specified number of degrees. + :param sprocket: + :param degrees: + :return: + ''' try: + # calculate the amoount of chain to be fed out/in based upon sprocket circumference and degree degValue = round( float(self.data.config.getValue("Advanced Settings", "gearTeeth")) * float(self.data.config.getValue("Advanced Settings", "chainPitch")) @@ -477,32 +1050,47 @@ def setSprockets(self, sprocket, degrees): * degrees, 4, ) + # Adjust distance based upon chainOverSprocket value self.data.gcode_queue.put("G91 ") if ( self.data.config.getValue("Advanced Settings", "chainOverSprocket") - == "Top" + == "Bottom" ): - self.data.gcode_queue.put("B09 " + sprocket + str(degValue) + " ") - else: - self.data.gcode_queue.put("B09 " + sprocket + "-" + str(degValue) + " ") + degValue *= -1.0 + if self.data.config.getValue("Advanced Settings", "chainOverSprocket") == "Bottom": + if self.data.config.getValue("Computed Settings", "chainOverSprocketComputed") != 2: + self.data.ui_queue1.put("Alert", "Alert", "mismatch between setting and computed setting. set for bottom feed, but computed !=2. report as issue on github please") + if self.data.config.getValue("Advanced Settings", "chainOverSprocket") == "Top": + if self.data.config.getValue("Computed Settings", "chainOverSprocketComputed") != 1: + self.data.ui_queue1.put("Alert", "Alert", "mismatch between setting and computed setting. set for top feed, but computed != 1. report as issue on github please") + # send command + self.data.gcode_queue.put("B09 " + sprocket + str(degValue) + " ") self.data.gcode_queue.put("G90 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False - def setVerticalAutomatic(self): - # set the call back for the measurement + def setSprocketAutomatic(self): + ''' + Sets the call back for the automatic sprocket alignment and requests the measurement. + ''' try: self.data.measureRequest = self.getLeftChainLength # request a measurement self.data.gcode_queue.put("B10 L") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False def getLeftChainLength(self, dist): + ''' + Part of setSprocketAutomatic. Is called after the measurement is received. Left chain measurement is logged + and then callback is set to get the right chain measurement. + :param dist: + :return: + ''' self.leftChainLength = dist # set the call back for the measurement self.data.measureRequest = self.getRightChainLength @@ -510,33 +1098,67 @@ def getLeftChainLength(self, dist): self.data.gcode_queue.put("B10 R") def getRightChainLength(self, dist): + ''' + Part of setSprocketAutomatic. Is called after the measurement is received. Right chain measurement is logged + and then moveToVertical is called. + :param dist: + :return: + ''' self.rightChainLength = dist self.moveToVertical() def moveToVertical(self): - chainPitch = float(self.data.config.get("Advanced Settings", "chainPitch")) - gearTeeth = float(self.data.config.get("Advanced Settings", "gearTeeth")) + ''' + Using distances logged from previous calls, calculates how much to move the sprockets so that one tooth is + vertical. + :return: + ''' + chainPitch = float(self.data.config.getValue("Advanced Settings", "chainPitch")) + gearTeeth = float(self.data.config.getValue("Advanced Settings", "gearTeeth")) distPerRotation = chainPitch * gearTeeth distL = -1 * (self.leftChainLength % distPerRotation) distR = -1 * (self.rightChainLength % distPerRotation) + # Issue required move commands.. self.data.gcode_queue.put("G91 ") self.data.gcode_queue.put("B09 L" + str(distL) + " ") self.data.gcode_queue.put("B09 R" + str(distR) + " ") self.data.gcode_queue.put("G90 ") def setSprocketsZero(self): - # mark that the sprockets are straight up + ''' + Notify controller that the chain length is zero. This is called by the user after the sprockets are set to + vertical. + :return: + ''' try: self.data.gcode_queue.put("B06 L0 R0 ") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) + return False + + def setSprocketsDefault(self): + ''' + Notify controller that the chain length is equal to the extendChainLength distance. This is called by the user + after setting both sprockets to vertical and reapplying the appropriate chain links to the top teeth. + https://forums.maslowcnc.com/t/how-to-manually-reset-chains-the-easy-way/ + :return: + ''' + try: + self.data.gcode_queue.put("B08 ") + return True + except Exception as e: + self.data.console_queue.put(str(e)) return False def updatePorts(self): - # refresh the list of available comports + ''' + Updates the list of ports found on the computer. + :return: + ''' + self.data.console_queue.put("at Update Ports") portsList = [] try: if sys.platform.startswith("linux") or sys.platform.startswith("cygwin"): @@ -555,21 +1177,40 @@ def updatePorts(self): if len(portsList) == 0: portsList.append("None") self.data.comPorts = portsList - self.data.ui_queue.put("Action: updatePorts") + # send updated list to UI client + self.data.ui_queue1.put("Action", "updatePorts", "") return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False + def acceptTriangularKinematicsResults(self): + ''' + Calls function to accept the results of the triangular calibration. + Todo: call directly from processActions. + :return: + ''' + try: + self.data.triangularCalibration.acceptTriangularKinematicsResults() + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def calibrate(self, result): + ''' + Sends form data for triangular calibration to the triangular calibration routine to calculate and then return + results to the UI client + :param result: + :return: + ''' try: motorYoffsetEst, rotationRadiusEst, chainSagCorrectionEst, cut34YoffsetEst = self.data.triangularCalibration.calculate( result ) - print(motorYoffsetEst) - print(rotationRadiusEst) - print(chainSagCorrectionEst) - if motorYoffsetEst == False: + if not motorYoffsetEst: + # if something didn't go correctly, motorYoffsetEst would have been returned as none return False return ( motorYoffsetEst, @@ -577,24 +1218,76 @@ def calibrate(self, result): chainSagCorrectionEst, cut34YoffsetEst, ) - """ - self.data.config.setValue('Maslow Settings', 'motorOffsetY', str(motorYoffsetEst)) - self.data.config.setValue('Advanced Settings', 'rotationRadius', str(rotationRadiusEst)) - self.data.config.setValue('Advanced Settings', 'chainSagCorrection', str(chainSagCorrectionEst)) + except Exception as e: + self.data.console_queue.put(str(e)) + return False - # With new calibration parameters return sled to workspace center + def holeyCalibrate(self, result): + ''' + Sends form data for holey calibration to the triangular calibration routine to calculate and then return + results to the UI client + :param result: + :return: + ''' + try: + motorYoffsetEst, distanceBetweenMotors, leftChainTolerance, rightChainTolerance, calibrationError = self.data.holeyCalibration.Calibrate( + result + ) + if not motorYoffsetEst: + # if something didn't go correctly, motorYoffsetEst would have been returned as none + return False + return ( + motorYoffsetEst, + distanceBetweenMotors, + leftChainTolerance, + rightChainTolerance, + calibrationError, + ) + except Exception as e: + self.data.console_queue.put(str(e)) + return False - self.data.gcode_queue.put("G21 ") - self.data.gcode_queue.put("G90 ") - self.data.gcode_queue.put("G40 ") - self.data.gcode_queue.put("G0 X0 Y0 ") - """ + + def sendGCode(self, gcode): + ''' + Sends gcode entered to controller. Comes from the Gcode->Send GCode function + :param gcode: + :return: + ''' + try: + self.data.sentCustomGCode = gcode + gcodeLines = gcode.splitlines() + for line in gcodeLines: + self.data.gcode_queue.put(line) return True except Exception as e: - print(e) + self.data.console_queue.put(str(e)) return False - + def macro(self, number): + ''' + Sends gcode associated with macro buttons + :param number: + :return: + ''' + ''' + Ignore this stuff in this comment block.. used for testing gpio + try: + if number == 1: + print("here") + btn_pin = Device.pin_factory.pin(2) + btn_pin.drive_low() + return True + if number == 2: + print("here2") + self.data.gpioActions.setGPIOAction(3, "Stop") + return True + + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + ''' try: if number == 1: macro = self.data.config.getValue("Maslow Settings", "macro1") @@ -602,6 +1295,873 @@ def macro(self, number): macro = self.data.config.getValue("Maslow Settings", "macro2") self.data.gcode_queue.put(macro) return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + ''' + def testImage(self): + # + #Calls function to send the test image from optical calibration + #Todo: move to processAction + #:return: + # + try: + self.data.opticalCalibration.testImage() + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + ''' + def adjustCenter(self, dist): + ''' + Used in optical calibration to allow user to raise/lower the center point and then move there. + :param dist: + :return: + ''' + try: + motorOffsetY = float(self.data.config.getValue("Maslow Settings","motorOffsetY"))+dist + self.data.config.setValue('Maslow Settings', 'motorOffsetY', str(motorOffsetY)) + if not self.returnToCenter(): + return False + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def processSettingRequest(self, section, setting): + ''' + Returns requested settings to the UI clients. Needed when a client connects or reconnects to get in sync + :param section: + :param setting: + :return: + ''' + try: + if setting == "homePosition": + # send home position + homeX = self.data.config.getValue("Advanced Settings", "homeX") + homeY = self.data.config.getValue("Advanced Settings", "homeY") + position = {"xval": homeX, "yval": homeY} + self.data.ui_queue1.put("Action", "homePositionMessage", position) + return None, None + elif setting == "calibrationCurve": + # send optical calibration curve values + try: + xCurve = [0,0,0,0,0,0] + yCurve = [0,0,0,0,0,0] + xCurve[0] = float(self.data.config.getValue('Optical Calibration Settings', 'calX0')) + xCurve[1] = float(self.data.config.getValue('Optical Calibration Settings', 'calX1')) + xCurve[2] = float(self.data.config.getValue('Optical Calibration Settings', 'calX2')) + xCurve[3] = float(self.data.config.getValue('Optical Calibration Settings', 'calX3')) + xCurve[4] = float(self.data.config.getValue('Optical Calibration Settings', 'calX4')) + xCurve[5] = float(self.data.config.getValue('Optical Calibration Settings', 'calX5')) + yCurve[0] = float(self.data.config.getValue('Optical Calibration Settings', 'calY0')) + yCurve[1] = float(self.data.config.getValue('Optical Calibration Settings', 'calY1')) + yCurve[2] = float(self.data.config.getValue('Optical Calibration Settings', 'calY2')) + yCurve[3] = float(self.data.config.getValue('Optical Calibration Settings', 'calY3')) + yCurve[4] = float(self.data.config.getValue('Optical Calibration Settings', 'calY4')) + yCurve[5] = float(self.data.config.getValue('Optical Calibration Settings', 'calY5')) + data = {"curveX": xCurve, "curveY": yCurve} + self.data.ui_queue1.put("Action", "updateOpticalCalibrationCurve", data) + except Exception as e: + self.data.console_queue.put(str(e)) + elif setting == "calibrationError": + # send calibration error matrix + try: + xyErrorArray = self.data.config.getValue("Optical Calibration Settings","xyErrorArray") + # parse the error array because its stored as a long string + errorX, errorY = self.data.config.parseErrorArray(xyErrorArray, True) + data = {"errorX": errorX, "errorY": errorY} + self.data.ui_queue1.put("Action", "updateOpticalCalibrationError", data) + except Exception as e: + self.data.console_queue.put(str(e)) + elif setting == "pauseButtonSetting": + # send current pause button state + try: + if self.data.uploadFlag == 0 or self.data.uploadFlag == 1: + return setting, "Pause" + else: + return setting, "Resume" + except Exception as e: + self.data.console_queue.put(str(e)) + else: + # send whatever is being request + if setting == "units": + # sled position messages from controller aren't sent unless there's a change, so we set the + # xval_prev to some silly number and when the next position message comes in from the controller, + # it will be processed and passed on to the UI clients. + self.data.xval_prev = -99999.0 + retval = self.data.config.getValue(section, setting) + # return the setting requested and its value. + return setting, retval + except Exception as e: + pass + return None, None + + def upgradeFirmware(self, version): + ''' + Upgrades the firmware in the controller + :param version: + :return: + ''' + try: + # if this is a pyinstaller release, find out where the root is... it could be a temp directory if single + # file + if self.data.platform == "PYINSTALLER": + home = os.path.join(self.data.platformHome) + else: + home = "." + if version == 0: + self.data.ui_queue1.put("SpinnerMessage", "", "Custom Firmware Update in Progress, Please Wait.") + path = home+"/firmware/madgrizzle/*.hex" + if version == 1: + self.data.ui_queue1.put("SpinnerMessage", "", "Stock Firmware Update in Progress, Please Wait.") + path = home+"/firmware/maslowcnc/*.hex" + if version == 2: + self.data.ui_queue1.put("SpinnerMessage", "", "Holey Firmware Update in Progress, Please Wait.") + path = home+"/firmware/holey/*.hex" + # wait half second.. not sure why.. + time.sleep(.5) + t0 = time.time()*1000 + portClosed = False + # request the the serial port is closed + self.data.serialPort.closeConnection() + # give it five seconds to close + while (time.time()*1000 - t0) < 5000: + if self.data.serialPort.getConnectionStatus(): + portClosed = True + break + # wait 1.5 seconds.. not sure why... + time.sleep(1.5) + # if port is closed, then upgrade firmware.. + if portClosed: + # works if there is only only valid hex file in the directory + for filename in glob.glob(path): + port = self.data.comport + # this was commented out below. probably not needed. Todo: cleanup + #if home != "": + # cmd = "\"C:\\Program Files (x86)\\Arduino\\hardware\\tools\\avr\\bin\\avrdude\" -Cavr/avrdude.conf -v -patmega2560 -cwiring -P" + port + " -b115200 -D -Uflash:w:" + filename + ":i" + #else: + cmd = home+"/tools/avrdude -C"+home+"/tools/avrdude.conf -v -patmega2560 -cwiring -P"+port+" -b115200 -D -Uflash:w:"+filename+":i" + #print(cmd) + # I think this is blocking.. + x = os.system(cmd) + self.data.connectionStatus = 0 + #print(x) + #print("closing modals") + # close off the modals and put away the spinner. + self.data.ui_queue1.put("Action", "closeModals", "Notification:") + return True + else: + self.data.ui_queue1.put("Action", "closeModals", "Notification:") + print("Port not closed") + return False + except Exception as e: + self.data.console_log.put(str(e)) + return False + + + def createDirectory(self, _directory): + ''' + Called to create a directory when saving gcode + :param _directory: + :return: + ''' + try: + home = self.data.config.getHome() + directory = home + "/.WebControl/gcode/" + _directory + if not os.path.isdir(directory): + os.mkdir(directory) + data = {"directory": _directory} + self.data.ui_queue1.put("Action", "updateDirectories", data) except Exception as e: print(e) + return True + + def sendGCodePositionUpdate(self, gCodeLineIndex=None, recalculate=False): + ''' + Send the updated gcode position. + :param gCodeLineIndex: Specific gcode index to send position based upon + :param recalculate: recalculate position by parsing through the gcode file. + :return: + ''' + if self.data.gcode: + if gCodeLineIndex is None: + gCodeLineIndex = self.data.gcodeIndex + gCodeLine = self.data.gcode[gCodeLineIndex] + + if not recalculate and gCodeLine.find("(") == -1 and gCodeLine.find(";") == -1: + # parse the position from the gcode line or use previous x, y, or z position if not present. Works only + # if in absolute mode + x = re.search("X(?=.)([+-]?([0-9]*)(\.([0-9]+))?)", gCodeLine) + if x: + if self.data.positioningMode == 0: + xTarget = float(x.groups()[0]) + else: + xTarget = float(x.groups()[0]) + self.data.previousPosX + self.data.previousPosX = xTarget + else: + xTarget = self.data.previousPosX + + y = re.search("Y(?=.)([+-]?([0-9]*)(\.([0-9]+))?)", gCodeLine) + + if y: + if self.data.positioningMode == 0: + yTarget = float(y.groups()[0]) + else: + yTarget = float(y.groups()[0]) + self.data.previousPosY + self.data.previousPosY = yTarget + else: + yTarget = self.data.previousPosY + + z = re.search("Z(?=.)([+-]?([0-9]*)(\.([0-9]+))?)", gCodeLine) + + if z: + if self.data.positioningMode == 0: + zTarget = float(z.groups()[0]) + else: + zTarget = float(z.groups()[0]) + self.data.previousPosZ + self.data.previousPosZ = zTarget + else: + zTarget = self.data.previousPosZ + else: + # if directed to recalculate or there's a comment in the line, then use more time consuming method. + xTarget, yTarget, zTarget = self.findPositionAt(self.data.gcodeIndex) + + # correct for scaling. If the gcode line is in one units but machine is in different units, then need + # to make adjustment. This might occur if a gcode is loaded that's metric and then the user switches + # the front page to imperial and then changes the gcode index. + scaleFactor = 1.0 + if self.data.gcodeFileUnits == "MM" and self.data.units == "INCHES": + scaleFactor = 1/25.4 + if self.data.gcodeFileUnits == "INCHES" and self.data.units == "MM": + scaleFactor = 25.4 + + # send the position to the UI client + position = {"xval": xTarget * scaleFactor, "yval": yTarget * scaleFactor,"zval": zTarget * scaleFactor, "gcodeLine": gCodeLine, "gcodeLineIndex": gCodeLineIndex} + self.data.ui_queue1.put("Action", "gcodePositionMessage", position) + return True + + def moveTo(self, posX, posY): + ''' + Commands controller to move sled to specified coordinates from the contextmenu moveto command(always in inches) + :param posX: + :param posY: + :return: + ''' + # make sure not out of range. + bedHeight = float(self.data.config.getValue("Maslow Settings","bedHeight"))/25.4 + bedWidth = float(self.data.config.getValue("Maslow Settings", "bedWidth"))/25.4 + try: + if posX<=bedWidth/2 and posX>=bedWidth/-2 and posY<=bedHeight/2 and posY>=bedHeight/-2: + self.data.sledMoving = True + if self.data.units == "INCHES": + posX=round(posX,4) + posY=round(posY,4) + else: + # convert to mm + posX=round(posX*25.4,4) + posY=round(posY*25.4,4) + self.data.gcode_queue.put( + "G90 G00 X" + + str(posX) + + " Y" + + str(posY) + + " " + ) + self.data.sledMoving = False + return True + return False + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.sledMoving = False + return False + + def processGCode(self): + ''' + This function processes the gcode up to the current gcode index to develop a set of + gcodes to send to the controller upon the user starting a run. This function is intended + to ensure that the sled is in the correct location and the controller is in the correct + state prior to starting the current gcode move. Currently processed are relative/absolute + positioning (G90/G91), imperial/metric units (G20/G21) and x, y and z positions + ''' + positioning = "G90 " + zAxisSafeHeight = float(self.data.config.getValue("Maslow Settings", "zAxisSafeHeight")) + zAxisFeedRate = 12.8 # currently hardcoded, but Todo: Add setting + xyAxisFeedRate = float(self.data.config.getValue("Advanced Settings", "maxFeedrate")) + positioning = "G90 " + units = "G20 " + homeX = float(self.data.config.getValue("Advanced Settings", "homeX")) + homeY = float(self.data.config.getValue("Advanced Settings", "homeY")) + previousUnits = self.data.config.getValue("Computed Settings", "units") + if previousUnits == "INCHES": + previousUnits = "G20 " + else: + previousUnits = "G21 " + xpos = homeX + ypos = homeY + zpos = 0 + tool = None + spindle = None + laser = None + dwell = None + # start parsing through gcode file up to the index + for x in range(self.data.gcodeIndex): + filtersparsed = re.sub(r'\(([^)]*)\)', '', self.data.gcode[x]) # replace mach3 style gcode comments with newline + filtersparsed = re.sub(r';([^\n]*)?', '\n', filtersparsed) # replace standard ; initiated gcode comments with nothing + if not filtersparsed.isspace(): # if all spaces, don't send. likely a comment. #self.data.gcode[x][0] != "(": + #lines = self.data.gcode[x].split(" ") + lines = filtersparsed.split(" ") + if lines: + finalLines = [] + # I think this splits everything up and reassembles them so that each line starts with a M, G or T + # I must of had a reason to do so + for line in lines: + if len(line) > 0: + if line[0] == "M" or line[0] == "G" or line[0] == "T" or len(finalLines) == 0: + finalLines.append(line) + else: + finalLines[-1] = finalLines[-1] + " " + line + # start processing the lines, keeping track of the state variables. + for line in finalLines: + if line[0]=='G': + if line.find("G90") != -1: + positioning = "G90 " + if line.find("G91") != -1: + positioning = "G91 " + if line.find("G20") != -1: + units = "G20 " + if previousUnits != units: #previous metrics now imperial + homeX = xpos / 25.4 + homeY = ypos / 25.4 + xpos = xpos / 25.4 + ypos = ypos / 25.4 + previousUnits = units + if line.find("G21") != -1: + units = "G21 " + if previousUnits != units: #previous imperial now metrics + homeX = xpos * 25.4 + homeY = ypos * 25.4 + xpos = xpos * 25.4 + ypos = ypos * 25.4 + previousUnits = units + if line.find("X") != -1: + _xpos = re.search("X(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + xpos = xpos+float(_xpos.groups()[0]) + else: + xpos = float(_xpos.groups()[0])+homeX + if line.find("Y") != -1: + _ypos = re.search("Y(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + ypos = ypos+float(_ypos.groups()[0]) + else: + ypos = float(_ypos.groups()[0])+homeY + if line.find("Z") != -1: + _zpos = re.search("Z(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + zpos = zpos+float(_zpos.groups()[0]) + else: + zpos = float(_zpos.groups()[0]) + if line.find("G4") != -1: + dwell = line[3:] + if line.find("G04") != -1: + dwell = line[4:] + if line.find("F") != -1: + _feedRate = re.search("F(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + _feedRateFloat = float(_feedRate.groups()[0]) + if line.find("X") != -1 or line.find("Y") == -1: + xyAxisFeedRate = _feedRateFloat + if line.find("Z") != -1: + zAxisFeedRate = _feedRateFloat + if line[0] == 'M': + if line.find("M3") != -1 or line.find("M03") != -1: + spindle = "M3 " + if line.find("M4") != -1 or line.find("M05") != -1: + spindle = "M4 " + if line.find("M5") != -1 or line.find("M05") != -1: + spindle = "M5 " + if line.find("M106") != -1: + laser = "M106 " + if line.find("M107") != -1: + laser = "M107 " + if line.find("M16") != -1: + laser = "M107 " + if line[0] == 'T': + tool = line[1:] #slice off the T + # now, after all that processing has been done, put machine in the correct state + # first, send the units command. + self.data.gcode_queue.put(units) + # if units is imperial, then change the zAxisSafeHeight + if units == "G20 ": + zAxisSafeHeight = zAxisSafeHeight / 25.4 + ''' + I had commented this out.. don't think it should be done since the units above will do it. + if units == "G20 ": + print("is G20") + self.data.actions.updateSetting("toInches", 0, True) # value = doesn't matter + + else: + print("is G21") + self.data.actions.updateSetting("toMM", 0, True) # value = doesn't matter + ''' + # move the Z-axis to the safe height + print("moving to safe height as part of processgcode") + #force into absolute mode + self.data.gcode_queue.put("G90 ") + self.data.gcode_queue.put("G0 Z"+str(round(zAxisSafeHeight, 4))+" F"+str(round(zAxisFeedRate, 4))) + # move the sled to the x, y coordinate it is supposed to be. + self.data.gcode_queue.put("G0 X"+str(round(xpos, 4))+" Y"+str(round(ypos, 4))+" F"+str(round(xyAxisFeedRate, 4))) + # if there is a tool, then send tool change command. + if tool is not None: + self.data.gcode_queue.put("T"+tool+" M6 ") + # if there is a spindle command, then send it. + if spindle is not None: + self.data.gcode_queue.put(spindle) + # if there is a laser command, then send it. + if laser is not None: + self.data.gcode_queue.put(laser) + # if there is a dwell command, then send it. + if dwell is not None: + self.data.gcode_queue.put("G4 "+dwell) + # move the z-axis to where it is supposed to be. + print("moving to where it should be as part of processgcode") + self.data.gcode_queue.put("G0 Z" + str(round(zpos, 4)) + " ") + # finally, put the machine in the appropriate positioning + # I have no idea if this really works for G91 gcode files.. + self.data.gcode_queue.put(positioning) + + def findPositionAt(self, index): + ''' + Find the x,y,z gcode position at a given index when simple calcs don't work. Parse through the file. + :param index: + :return: + ''' + xpos = 0 + ypos = 0 + zpos = 0 + positioning = "G90 " + for x in range(index): + filtersparsed = re.sub(r'\(([^)]*)\)', '', self.data.gcode[x]) # replace mach3 style gcode comments with newline + filtersparsed = re.sub(r';([^\n]*)?', '\n', filtersparsed) # replace standard ; initiated gcode comments with "" + if not filtersparsed.isspace(): # if all spaces, don't send. likely a comment. #self.data.gcode[x][0] != "(": + listOfLines = filter(None, re.split("(G)", filtersparsed)) # self.data.gcode[x])) # self.data.gcode[x].split("G") + # it is necessary to split the lines along G commands so that commands concatenated on one line + # are processed correctly + for line in listOfLines: + line = 'G'+line + if line[0] == 'G': + if line.find("G90") != -1: + positioning = "G90 " + if line.find("G91") != -1: + positioning = "G91 " + if line.find("X") != -1: + _xpos = re.search("X(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + xpos = xpos + float(_xpos.groups()[0]) + else: + xpos = float(_xpos.groups()[0]) + if line.find("Y") != -1: + _ypos = re.search("Y(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + ypos = ypos + float(_ypos.groups()[0]) + else: + ypos = float(_ypos.groups()[0]) + if line.find("Z") != -1: + _zpos = re.search("Z(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if positioning == "G91 ": + zpos = zpos + float(_zpos.groups()[0]) + else: + zpos = float(_zpos.groups()[0]) + #print("xpos="+str(xpos)+", ypos="+str(ypos)+", zpoz="+str(zpos)+" for index="+str(index)) + return xpos, ypos, zpos + + def adjustChain(self, chain): + ''' + Called during the chain extend routine + :param chain: + :return: + ''' + try: + for x in range(6): + self.data.ui_queue1.put("Action", "updateTimer", chain+":"+str(5-x)) + self.data.console_queue.put("Action:updateTimer_" + chain + ":" + str(5 - x)) + time.sleep(1) + if chain == "left": + self.data.gcode_queue.put("B02 L1 R0 ") + self.data.measureRequest = self.issueStopCommand + self.data.gcode_queue.put("B10 L") + if chain == "right": + self.data.gcode_queue.put("B02 L0 R1 ") + self.data.measureRequest = self.issueStopCommand + self.data.gcode_queue.put("B10 R") + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def issueStopCommand(self, distance): + try: + self.data.quick_queue.put("!") + with self.data.gcode_queue.mutex: + self.data.gcode_queue.queue.clear() + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def toggleCamera(self): + ''' + Turns camera on or off. If its suspended, it's read (though I can't explain why at this moment). + :return: + ''' + try: + status = self.data.camera.status() + if status == "stopped": + self.data.camera.start() + if status == "suspended": + self.data.camera.read() + if status == "running": + self.data.camera.stop() + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def cameraStatus(self): + ''' + Sends the status of the camera to the UI client. Not sure why its not called requestCameraStatus + Todo: update name to request cameraStatus + :return: + ''' + try: + status = self.data.camera.status() + if status == "stopped": + self.data.ui_queue1.put("Action", "updateCamera", "off") + if status == "suspended": + self.data.ui_queue1.put("Action", "updateCamera", "off") + if status == "running": + self.data.ui_queue1.put("Action", "updateCamera", "on") + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def queryCamera(self): + ''' + Query the camera's settings. Probably could be called directly by processAction + Todo: move to processAction + :return: + ''' + try: + self.data.camera.getSettings() + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def velocityPIDTest(self, parameters): + ''' + Send commands to start the veloctiy pid test + Todo: further explain this + :param parameters: + :return: + ''' + try: + print(parameters) + print(parameters["KpV"]) + self.data.config.setValue("Advanced Settings", "KpV", parameters["KpV"]) + self.data.config.setValue("Advanced Settings", "KiV", parameters["KiV"]) + self.data.config.setValue("Advanced Settings", "KdV", parameters["KdV"]) + gcodeString = "B13 "+parameters["vMotor"]+"1 S"+parameters["vStart"]+" F"+parameters["vStop"]+" I"+parameters["vSteps"]+" V"+parameters["vVersion"] + print(gcodeString) + self.data.PIDVelocityTestVersion = parameters["vVersion"] + self.data.gcode_queue.put(gcodeString) + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def positionPIDTest(self, parameters): + ''' + Send commands to start the position pid test + Todo: further explain this + :param parameters: + :return: + ''' + + try: + print(parameters) + print(parameters["KpP"]) + self.data.config.setValue("Advanced Settings", "KpPos", parameters["KpP"]) + self.data.config.setValue("Advanced Settings", "KiPos", parameters["KiP"]) + self.data.config.setValue("Advanced Settings", "KdPos", parameters["KdP"]) + + gcodeString = "B14 "+parameters["pMotor"]+"1 S"+parameters["pStart"]+" F"+parameters["pStop"]+" I"+parameters["pSteps"]+" T"+parameters["pTime"]+" V"+parameters["pVersion"] + print(gcodeString) + self.data.PIDPositionTestVersion = parameters["pVersion"] + self.data.gcode_queue.put(gcodeString) + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def velocityPIDTestRun(self, command, msg): + ''' + + :param command: + :param msg: + :return: + ''' + try: + if command == 'stop': + self.data.inPIDVelocityTest = False + print("PID velocity test stopped") + print(self.data.PIDVelocityTestData) + data = json.dumps({"result": "velocity", "version": self.data.PIDVelocityTestVersion, "data": self.data.PIDVelocityTestData}) + self.data.ui_queue1.put("Action", "updatePIDData", data) + self.stopRun() + if command == 'running': + if msg.find("Kp=") == -1: + if self.data.PIDVelocityTestVersion == "2": + if msg.find("setpoint") == -1: + self.data.PIDVelocityTestData.append(msg) + else: + self.data.PIDVelocityTestData.append(float(msg)) + if command == 'start': + self.data.inPIDVelocityTest = True + self.data.PIDVelocityTestData = [] + print("PID velocity test started") + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def positionPIDTestRun(self, command, msg): + try: + if command == 'stop': + self.data.inPIDPositionTest = False + print("PID position test stopped") + print(self.data.PIDPositionTestData) + data = json.dumps({"result": "position", "version": self.data.PIDPositionTestVersion, "data": self.data.PIDPositionTestData}) + self.data.ui_queue1.put("Action", "updatePIDData", data) + self.stopRun() + if command == 'running': + if msg.find("Kp=") == -1: + if self.data.PIDPositionTestVersion == "2": + if msg.find("setpoint") == -1: + self.data.PIDPositionTestData.append(msg) + else: + self.data.PIDPositionTestData.append(float(msg)) + if command == 'start': + self.data.inPIDPositionTest = True + self.data.PIDPositionTestData = [] + print("PID position test started") + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def updateGCode(self, gcode): + try: + #print(gcode) + homeX = float(self.data.config.getValue("Advanced Settings", "homeX")) + homeY = float(self.data.config.getValue("Advanced Settings", "homeY")) + + if self.data.units == "MM": + scaleFactor = 25.4 + else: + scaleFactor = 1.0 + self.data.gcodeShift = [ + homeX, + homeY + ] + + self.data.gcodeFile.loadUpdateFile(gcode) + self.data.ui_queue1.put("Action", "gcodeUpdate", "") + return True + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def downloadDiagnostics(self): + try: + timestr = time.strftime("%Y%m%d-%H%M%S") + filename = self.data.config.home+"/.WebControl/"+"wc_diagnostics_"+timestr+".zip" + zipObj = zipfile.ZipFile(filename, 'w') + path1 = self.data.config.home+"/.WebControl/webcontrol.json" + zipObj.write(path1, os.path.basename(path1)) + path1 = self.data.config.home + "/.WebControl/alog.txt" + zipObj.write(path1, os.path.basename(path1)) + path1 = self.data.config.home + "/.WebControl/log.txt" + zipObj.write(path1, os.path.basename(path1)) + zipObj.close() + return filename + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def clearLogs(self): + try: + retval = self.data.logger.deleteLogFiles() + return retval + except Exception as e: + self.data.console_queue.put(str(e)) + return False + ''' + def checkForLatestPyRelease(self): + if True: #self.data.platform=="PYINSTALLER": + print("check for pyrelease") + g = Github() + repo = g.get_repo("madgrizzle/WebControl") + releases = repo.get_releases() + latest = 0 + latestRelease = None + type = self.data.pyInstallType + platform = self.data.pyInstallPlatform + for release in releases: + try: + tag_name = re.sub(r'[v]',r'',release.tag_name) + print(release.body) + #print(tag_name) + if float(tag_name) > latest: + latest = float(tag_name) + latestRelease = release + + except: + print("error parsing tagname") + print(latest) + if latest>self.data.pyInstallCurrentVersion: + if latestRelease is not None: + print(latestRelease.tag_name) + assets = latestRelease.get_assets() + for asset in assets: + if asset.name.find(type) != -1 and asset.name.find(platform) != -1: + print(asset.name) + print(asset.url) + self.data.ui_queue1.put("Action", "pyinstallUpdate", "on") + self.data.pyInstallUpdateAvailable = True + self.data.pyInstallUpdateBrowserUrl = asset.browser_download_url + self.data.pyInstallUpdateVersion = latest + + def processAbsolutePath(self, path): + index = path.find("main.py") + self.data.pyInstallInstalledPath = path[0:index-1] + print(self.data.pyInstallInstalledPath) + + def updatePyInstaller(self): + home = self.data.config.getHome() + if self.data.pyInstallUpdateAvailable == True: + if not os.path.exists(home+"/.WebControl/downloads"): + print("creating downloads directory") + os.mkdir(home+"/.WebControl/downloads") + fileList=glob.glob(home+"/.WebControl/downloads/*.gz") + for filePath in fileList: + try: + os.remove(filePath) + except: + print("error cleaning download directory: ",filePath) + print("---") + if self.data.pyInstallPlatform == "win32" or self.data.pyInstallPlatform == "win64": + filename = wget.download(self.data.pyInstallUpdateBrowserUrl, out=home+"\\.WebControl\\downloads") + else: + filename = wget.download(self.data.pyInstallUpdateBrowserUrl, out=home+"/.WebControl/downloads") + print(filename) + + if self.data.platform == "PYINSTALLER": + lhome = os.path.join(self.data.platformHome) + else: + lhome = "." + if self.data.pyInstallPlatform == "win32" or self.data.pyInstallPlatform == "win64": + path = lhome+"/tools/upgrade_webcontrol_win.bat" + copyfile(path, home+"/.WebControl/downloads/upgrade_webcontrol_win.bat") + path = lhome+"/tools/7za.exe" + copyfile(path, home+"/.WebControl/downloads/7za.exe") + self.data.pyInstallInstalledPath = self.data.pyInstallInstalledPath.replace('/','\\') + program_name = home+"\\.WebControl\\downloads\\upgrade_webcontrol_win.bat" + + else: + path = lhome+"/tools/upgrade_webcontrol.sh" + copyfile(path, home+"/.WebControl/downloads/upgrade_webcontrol.sh") + program_name = home+"/.WebControl/downloads/upgrade_webcontrol.sh" + self.make_executable(home+"/.WebControl/downloads/upgrade_webcontrol.sh") + tool_path = home+"\\.WebControl\\downloads\\7za.exe" + arguments = [filename, self.data.pyInstallInstalledPath, tool_path] + command = [program_name] + command.extend(arguments) + print("popening") + print(command) + subprocess.Popen(command) + return True + return False + + def make_executable(self, path): + print("1") + mode = os.stat(path).st_mode + print("2") + mode |= (mode & 0o444) >> 2 # copy R bits to X + print("3") + os.chmod(path, mode) + print("4") + ''' + + def addDirToZip(self, zipHandle, path, basePath=""): + basePath = basePath.rstrip("\\/") + "" + basePath = basePath.rstrip("\\/") + for root, dirs, files in os.walk(path): + # add dir itself (needed for empty dirs + zipHandle.write(os.path.join(root, ".")) + # add files + for file in files: + filePath = os.path.join(root, file) + inZipPath = filePath.replace(basePath, "", 1).lstrip("\\/") + # print filePath + " , " + inZipPath + zipHandle.write(filePath, inZipPath) + + def zipfolder(self, filename, target_dir): + zipobj = zipfile.ZipFile(filename, 'w', zipfile.ZIP_DEFLATED) + rootlen = len(target_dir) + 1 + for base, dirs, files in os.walk(target_dir): + for file in files: + fn = os.path.join(base, file) + zipobj.write(fn, fn[rootlen:]) + zipobj.close() + + + def backupWebControl(self): + try: + timestr = time.strftime("%Y%m%d-%H%M%S") + home = self.data.config.getHome() + filename = home+"/wc_backup_"+timestr+".zip" + print(filename) + folder = self.data.config.home+'/.WebControl' + print(folder) + self.zipfolder(filename, folder) + #self.addDirToZip(zipObj, self.data.config.home+'/.Webcontrol') + #zipObj.close() + return filename + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + + def restoreWebControl(self, fileName): + try: + with zipfile.ZipFile(fileName, 'r') as zipObj: + # Extract all the contents of zip file in different directory + zipObj.extractall(self.data.config.home+'/.Webcontrol') + retval = self.data.config.reloadWebControlJSON() + if retval is True: + self.data.gcode_queue.put("$$") + return retval + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def setFakeServo(self, value): + try: + if value: + self.data.gcode_queue.put("B99 ON") + else: + self.data.gcode_queue.put("B99 OFF") + return True + except Exception as e: + self.data.console_queue.put(str(e)) return False diff --git a/Actions/gpioActions.py b/Actions/gpioActions.py new file mode 100644 index 00000000..bc434d0e --- /dev/null +++ b/Actions/gpioActions.py @@ -0,0 +1,96 @@ +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs +from gpiozero.pins.mock import MockFactory +from gpiozero import Device, Button, LED +class GPIOActions(MakesmithInitFuncs): + def __init__(self): + pass + + ''' + get gpio settings + ''' + Buttons = [] + LEDs = [] + actionList = ["", "WebMCP Running", "Shutdown", "Stop", "Pause", "Play", "Home", "Return to Center","Pendant", "PlayLED", "PauseLED", "StopLED"] + + def getActionList(self): + return self.actionList + + def setup(self): + #self.setGPIOAction(2,"Stop") + setValues = self.data.config.getJSONSettingSection("GPIO Settings") + self.data.GPIOButtonService = self.data.config.getValue("Maslow Settings","MaslowButtonService") + if (self.data.GPIOButtonService): + self.data.wiiPendantPresent = self.data.config.getValue("Maslow Settings","wiiPendantPresent") + self.clidisplay = self.data.config.getValue("Maslow Settings", "clidisplay") + #print(setValues) + for setting in setValues: + if setting["value"] != "": + pinNumber = int(setting["key"][4:]) + if (self.data.GPIOButtonService == False): + self.setGPIOAction(pinNumber, setting["value"]) + + def setGPIOAction(self,pin, action): + # first remove pin assignments if already made + foundButton = None + for button in self.Buttons: + if button.pin.number == pin: + button.pin.close() + foundButton = button + break + if foundButton is not None: + self.Buttons.remove(foundButton) + + foundLED = None + for led in self.LEDs: + if led[1].pin.number == pin: + led[1].pin.close() + foundLED = led + break + if foundLED is not None: + self.LEDs.remove(foundLED) + + type, pinAction = self.getAction(action) + if type == "button": + button = Button(pin) + button.when_pressed = pinAction + self.Buttons.append(button) + print("set Button ", pin, " with action: ", action) + if type == "led": + _led = LED(pin) + led = (action,_led) + self.LEDs.append(led) + print("set LED with action: " + action) + #pause() + def getAction(self, action): + if action == "Stop": + return "button", self.data.actions.stopRun + elif action == "Pause": + return "button", self.data.actions.pauseRun + elif action == "Play": + return "button", self.data.actions.startRun + else: + return "led", None + + def runrun(self): + print("gpio button press detected") + self.data.actions.startRun() + + def causeAction(self, action, onoff): + for led in self.LEDs: + if led[0] == action: + print(led[1]) + if onoff == "on": + led[1].on() + else: + led[1].off() + print(led[1]) + if action == "PlayLED" and onoff == "on": + self.causeAction("PauseLED", "off") + self.causeAction("StopLED", "off") + if action == "PauseLED" and onoff == "on": + self.causeAction("PlayLED", "off") + self.causeAction("StopLED", "off") + if action == "StopLED" and onoff == "on": + self.causeAction("PauseLED", "off") + self.causeAction("PlayLED", "off") + diff --git a/Actions/holeyCalibration.py b/Actions/holeyCalibration.py new file mode 100644 index 00000000..9a2efd8c --- /dev/null +++ b/Actions/holeyCalibration.py @@ -0,0 +1,351 @@ +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs +import Actions.HoleySimulationKinematics as kinematics +import math +from scipy.optimize import least_squares +import numpy + +import itertools +import time +import re +import sys +import math +import base64 +import time +import os +import datetime + + +class HoleyCalibration(MakesmithInitFuncs): + + def __init__(self): + # can't do much because data hasn't been initialized yet + pass + + SP_D = 3629.025 + SP_motorOffsetY = 503.4 + SP_rotationDiskRadius = 138.1 + SP_leftChainTolerance = 0 + SP_rightChainTolerance = 0 + SP_sledWeight = 109.47 # N + SP_chainOverSprocket = False + + Opt_D = 3601.2 + Opt_motorOffsetY = 468.4 + Opt_rotationDiskRadius = 139.1 + Opt_leftChainTolerance = 0 + Opt_rightChainTolerance = 0 + Opt_sledWeight = 97.9 # N + + # Chain lengths @ each hole + ChainLengths = [] + MeasurementMap = [ + (1, 2), + (2, 3), + (4, 5), + (5, 6), + (1, 4), + (2, 5), + (3, 6), + (2, 4), + (1, 5), + (3, 5), + (2, 6)] + CutOrder = [1, 0, 3, 4, 5, 2] # Counter Clockwise, starting at top + IdealLengthArray = 0 + MeasuredLengthArray = 0 + DesiredLengthDeltaArray = 0 + + OptimizationOutput = 0 + + kin = kinematics.Kinematics() + kin.isQuadKinematics = False + + # Define function with input of (ideal lengths and) machine parameters (delta) and output of length error + def LengthDeltaFromIdeal(self, + DeltaArray): # Del_D,Del_motorOffsetY,Del_rotationDiskRadius,Del_chainSagCorrection): + self.kin.D = self.SP_D + DeltaArray[0] + self.kin.motorOffsetY = self.SP_motorOffsetY + DeltaArray[1] + self.kin.leftChainTolerance = self.SP_leftChainTolerance + DeltaArray[2] + self.kin.rightChainTolerance = self.SP_rightChainTolerance + DeltaArray[3] + self.kin.recomputeGeometry() + CalculatedPositions = [] + for LeftChainLength, RightChainLength in self.ChainLengths: + CalculatedPositions.append(self.kin.forward(LeftChainLength, RightChainLength)) + + return self.MeasuredLengthArray - self.CalculateMeasurements(CalculatedPositions) + + def InitialMeasurementError(self, Meas, idx): # For validating in GUI + Ideal = self.IdealLengthArray[idx] + return Ideal - Meas + + def ValidateMeasurement(self, Meas, idx): + if idx < 11: + Ideal = self.IdealLengthArray[idx] + return 0.1 > abs((Ideal - Meas) / Ideal) + else: + return Meas > 0.0 + + def CutTestPattern(self): + oldUnits = self.data.units + self.data.console_queue.put('Cutting Holey Calibration Test Pattern') + self.InitializeIdealXyCoordinates() + self.data.gcode_queue.put("G21") + self.data.gcode_queue.put("G90") # Switch to absolute mode + self.data.gcode_queue.put("G40") + self.data.gcode_queue.put("G17") + self.data.gcode_queue.put("M3") + for idx in self.CutOrder: + x, y = self.IdealCoordinates[idx] + self.data.console_queue.put('cutting index: ' + str(idx + 1)) + self.data.gcode_queue.put("G0 X" + str(x) + " Y" + str(y)) + self.data.gcode_queue.put("G0 Z-5") + self.data.gcode_queue.put("G0 Z5") + + self.data.gcode_queue.put("G0 X0 Y0") + self.data.gcode_queue.put("M5") + if oldUnits == "INCHES": + self.data.gcode_queue.put("G20") + return True + + def CalculateMeasurements(self, HolePositions): + # aH1x,aH1y,aH2x,aH2y,aH3x,aH3y,aH4x,aH4y,aH5x,aH5y,aH6x,aH6y + Measurements = [] + for StartHoleIdx, EndHoleIdx in self.MeasurementMap: + x1, y1 = HolePositions[StartHoleIdx - 1] + x2, y2 = HolePositions[EndHoleIdx - 1] + Measurements.append(self.GeometricLength(x1, y1, x2, y2)) + ToTopHole = 1 + Measurements.append( + self.GeometricLength(HolePositions[ToTopHole][0], HolePositions[ToTopHole][1], HolePositions[ToTopHole][0], + self.kin.machineHeight / 2)) + + return numpy.array(Measurements) + + def InitializeIdealXyCoordinates(self): + self.SP_D = float(self.data.config.getValue("Maslow Settings", "motorSpacingX")) + self.SP_motorOffsetY = float(self.data.config.getValue("Maslow Settings", "motorOffsetY")) + self.SP_rotationDiskRadius = float(self.data.config.getValue("Advanced Settings", "rotationRadius")) + self.SP_leftChainTolerance = float(self.data.config.getValue("Advanced Settings", "leftChainTolerance")) + self.SP_rightChainTolerance = float(self.data.config.getValue("Advanced Settings", "rightChainTolerance")) + self.SP_sledWeight = float(self.data.config.getValue("Maslow Settings", "sledWeight")) + if self.data.config.getValue("Advanced Settings", "chainOverSprocket") == "Top": + self.SP_chainOverSprocket = 1 + else: + self.SP_chainOverSprocket = 2 + self.kin.machineHeight = float(self.data.config.getValue("Maslow Settings","bedHeight")) + self.kin.machineWidth = float(self.data.config.getValue("Maslow Settings","bedWidth")) + workspaceHeight = self.kin.machineHeight + workspaceWidth = self.kin.machineWidth + aH1x = -(workspaceWidth / 2.0 - 254.0) + aH1y = (workspaceHeight / 2.0 - 254.0) + aH2x = 0 + IdealCoordinates = [ + (aH1x, aH1y), + (aH2x, aH1y), + (-aH1x, aH1y), + (aH1x, -aH1y), + (aH2x, -aH1y), + (-aH1x, -aH1y)] + self.IdealCoordinates = IdealCoordinates + self.kin.D = self.SP_D + self.kin.motorOffsetY = self.SP_motorOffsetY + self.kin.rotationDiskRadius = self.SP_rotationDiskRadius + self.kin.sledWeight = self.SP_sledWeight + self.kin.leftChainTolerance = self.SP_leftChainTolerance + self.kin.rightChainTolerance = self.SP_rightChainTolerance + self.kin.chainOverSprocket = self.SP_chainOverSprocket + self.kin.recomputeGeometry() + self.IdealLengthArray = self.CalculateMeasurements(IdealCoordinates) + + self.ChainLengths = [] + for x, y in IdealCoordinates: + self.ChainLengths.append(self.kin.inverse(x, y)) + return self.IdealLengthArray + + def SetMeasurements(self, Measurements): + self.MeasuredLengthArray = numpy.array(Measurements) + + def Calibrate(self, result): + self.InitializeIdealXyCoordinates() + measurements = self.processMeasurements(result) + self.SetMeasurements(measurements) + self.OptimizationOutput = least_squares(self.LengthDeltaFromIdeal, numpy.array([0, 0, 0, 0]), jac='2-point', + diff_step=.1, ftol=1e-11) + Deltas = self.OptimizationOutput.x + self.Opt_D = round(Deltas[0] + self.SP_D,5) + self.Opt_motorOffsetY = round(Deltas[1] + self.SP_motorOffsetY,5) + self.Opt_leftChainTolerance = round(Deltas[2] + self.SP_leftChainTolerance,5) + self.Opt_rightChainTolerance = round(Deltas[3] + self.SP_rightChainTolerance,5) + self.kin.D = self.Opt_D + self.kin.motorOffsetY = self.Opt_motorOffsetY + self.kin.leftChainTolerance = self.Opt_leftChainTolerance + self.kin.rightChainTolerance = self.Opt_rightChainTolerance + self.kin.recomputeGeometry() + totalError = self.ReportCalibration() + return self.Opt_motorOffsetY, self.Opt_D, self.Opt_leftChainTolerance, self.Opt_rightChainTolerance, totalError + + + def ReportCalibration(self): + self.data.console_queue.put('Optimized Errors') + totalError = 0 + for idx, pts, ms, cal, er in zip( + range(self.MeasuredLengthArray.size), + self.MeasurementMap, + self.MeasuredLengthArray, + self.CalibratedLengths(), + self.CalibratedLengthError()): + self.data.console_queue.put(('\tIndex : {}' + + '\n\t\tPoints Span : {} to {}' + + '\n\t\tMeasured Distance : {}' + + '\n\t\tCalibrated Distance: {}' + + '\n\t\tDistance Error : {}').format( + idx, pts[0], pts[1], ms, cal, er)) + totalError = totalError + er*er + + totalError = math.sqrt(totalError) + self.data.console_queue.put("") + self.data.console_queue.put("Distance Between Motors:") + self.data.console_queue.put(self.Opt_D) + self.data.console_queue.put("") + self.data.console_queue.put("Motor Y Offset:") + self.data.console_queue.put(self.Opt_motorOffsetY) + self.data.console_queue.put("") + self.data.console_queue.put("Left Chain Tolerance:") + self.data.console_queue.put(self.Opt_leftChainTolerance) + self.data.console_queue.put("") + self.data.console_queue.put("Right Chain Tolerance:") + self.data.console_queue.put(self.Opt_rightChainTolerance) + return totalError + + def CalibratedLengths(self): + return self.MeasuredLengthArray - self.OptimizationOutput.fun + + def CalibratedLengthError(self): + return self.OptimizationOutput.fun + + def HolePositionsFromChainLengths(self): + HolePositions = [] + for LeftChainLength, RightChainLength in self.ChainLengths: + HolePositions.append(self.kin.forward(LeftChainLength, RightChainLength)) + return HolePositions + + def SimulateMeasurement(self, D, motorOffsetY, leftChainTolerance, rightChainTolerance): + # Simulate Measurement. Modify machine parameters, use kin.forward to determine x,y coordinates. Return machine parameters to original + self.kin.D = D + self.kin.motorOffsetY = motorOffsetY + self.kin.leftChainTolerance = leftChainTolerance + self.kin.rightChainTolerance = rightChainTolerance + self.kin.recomputeGeometry() + + HolePositions = self.HolePositionsFromChainLengths() + + self.kin.D = self.SP_D + self.kin.motorOffsetY = self.SP_motorOffsetY + self.kin.rotationDiskRadius = self.SP_rotationDiskRadius + self.kin.sledWeight = self.SP_sledWeight + self.kin.leftChainTolerance = self.SP_leftChainTolerance + self.kin.rightChainTolerance = self.SP_rightChainTolerance + self.kin.recomputeGeometry() + + Measurements = self.CalculateMeasurements(HolePositions) + + self.SetMeasurements(Measurements) + + def processMeasurements(self, result): + measurements = [] + try: + M1 = float(result["M1"]) + self.data.console_queue.put(M1) + measurements.append(M1) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M1." ) + return False + try: + M2 = float(result["M2"]) + self.data.console_queue.put(M2) + measurements.append(M2) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M2." ) + return False + try: + M3 = float(result["M3"]) + self.data.console_queue.put(M3) + measurements.append(M3) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M3." ) + return False + try: + M4 = float(result["M4"]) + self.data.console_queue.put(M4) + measurements.append(M4) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M4." ) + return False + try: + M5 = float(result["M5"]) + self.data.console_queue.put(M5) + measurements.append(M5) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M5." ) + return False + try: + M6 = float(result["M6"]) + self.data.console_queue.put(M6) + measurements.append(M6) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M6." ) + return False + try: + M7 = float(result["M7"]) + self.data.console_queue.put(M7) + measurements.append(M7) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M7." ) + return False + try: + M8 = float(result["M8"]) + self.data.console_queue.put(M8) + measurements.append(M8) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M8.") + return False + try: + M9 = float(result["M9"]) + self.data.console_queue.put(M9) + measurements.append(M9) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M9." ) + return False + try: + M10 = float(result["M10"]) + self.data.console_queue.put(M10) + measurements.append(M10) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M10." ) + return False + try: + M11 = float(result["M11"]) + self.data.console_queue.put(M11) + measurements.append(M11) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M11." ) + return False + try: + M12 = float(result["M12"]) + self.data.console_queue.put(M12) + measurements.append(M12) + except: + self.data.message_queue.put("Message: Please enter a number for the distance M12." ) + return False + return measurements + + def GeometricLength(self,x1,y1,x2,y2): + return math.sqrt(math.pow(x1-x2,2) + math.pow(y1-y2,2)) + + def acceptCalibrationResults(self): + self.data.config.setValue('Maslow Settings', 'motorOffsetY', str(self.Opt_motorOffsetY)) + self.data.config.setValue('Maslow Settings', 'motorSpacingX', str(self.Opt_D)) + self.data.config.setValue('Advanced Settings', 'leftChainTolerance', str(self.Opt_leftChainTolerance)) + self.data.config.setValue('Advanced Settings', 'rightChainTolerance', str(self.Opt_rightChainTolerance)) + return True + diff --git a/Actions/opticalCalibration.py b/Actions/opticalCalibration.py deleted file mode 100644 index fa499957..00000000 --- a/Actions/opticalCalibration.py +++ /dev/null @@ -1,348 +0,0 @@ -from DataStructures.makesmithInitFuncs import MakesmithInitFuncs -from scipy.spatial import distance as dist -from imutils import perspective, contours - -# from imutils.video import VideoStream -# from Background.webcamVideoStream import WebcamVideoStream -# from imutils.video import WebcamVideoStream -import numpy as np -import imutils -import cv2 -import time -import re -import sys -import math -import base64 - - -class OpticalCalibration(MakesmithInitFuncs): - - camera = None - time.sleep(2.0) - gaussianBlurValue = 5 - cannyLowValue = 50 - cannyHighValue = 100 - opticalCenter = (None, None) - HomingPosX = 0 - HomingPosY = 0 - HomingX = 0 - HomingY = 0 - matrixSize = (31, 15) - calErrorsX = np.zeros(matrixSize) - calErrorsY = np.zeros(matrixSize) - inAutoMode = False - inMeasureOnlyMode = False - autoScanDirection = 0 - - def stopCut(self): - self.data.quick_queue.put("!") - with self.data.gcode_queue.mutex: - self.data.gcode_queue.queue.clear() - self.inAutoMode = False - self.inMeasureOnlyMode = False - - def midpoint(self, ptA, ptB): - return ((ptA[0] + ptB[0]) * 0.5, (ptA[1] + ptB[1]) * 0.5) - - def removeOutliersAndAverage(self, data): - mean = np.mean(data) - sd = np.std(data) - tArray = [ - x for x in data if ((x >= mean - 2.0 * sd) and (x <= mean + 2.0 * sd)) - ] - return np.average(tArray), np.std(tArray) - - def simplifyContour(self, c, sides=4): - tolerance = 0.01 - while True: - _c = cv2.approxPolyDP(c, tolerance * cv2.arcLength(c, True), True) - if len(_c) <= sides or tolerance > 0.5: - break - tolerance += 0.01 - if ( - len(_c) < sides - ): # went too small.. now lower the tolerance until four points or more are reached - while True: - tolerance -= 0.01 - _c = cv2.approxPolyDP(c, tolerance * cv2.arcLength(c, True), True) - if len(_c) >= sides or tolerance <= 0.1: - break - return _c - - def setCalibrationSettings(self, args): - self.markerX = float(args["markerX"]) * 25.4 - self.markerY = float(args["markerY"]) * 25.4 - self.centerX = float(args["centerX"]) - self.centerY = float(args["centerY"]) - self.scaleX = float(args["scaleX"]) - self.scaleY = float(args["scaleY"]) - self.tlX = int(args["tlX"]) - self.tlY = int(args["tlY"]) - self.brX = int(args["brX"]) - self.brY = int(args["brY"]) - - def translatePoint(self, xB, yB, xA, yA, angle): - cosa = math.cos((angle) * 3.141592 / 180.0) - sina = math.sin((angle) * 3.141592 / 180.0) - xB -= xA - yB -= yA - _xB = xB * cosa - yB * sina - _yB = xB * sina + yB * cosa - xB = _xB + xA - yB = _yB + yA - return xB, yB - - def HomeIn(self): - _posX = round(self.HomingPosX * 3.0 + self.HomingX / 25.4, 4) - _posY = round(self.HomingPosY * 3.0 + self.HomingY / 25.4, 4) - # self.updateTargetIndicator(_posX,_posY,"INCHES") - print( - "Moving to ({},{}) by trying [{}, {}]".format( - self.HomingPosX * 3.0, self.HomingPosY * 3.0, _posX, _posY - ) - ) - self.data.units = "INCHES" - self.data.gcode_queue.put("G20 ") - self.data.gcode_queue.put("G90 ") - self.data.gcode_queue.put("G0 X" + str(_posX) + " Y" + str(_posY) + " ") - self.data.gcode_queue.put("G91 ") - self.data.measureRequest = self.on_CenterOnSquare - # request a measurement - self.data.gcode_queue.put("B10 L") - - def on_CenterOnSquare(self, _dist, findCenter=False): - - dxList = np.zeros(shape=(10)) - dyList = np.zeros(shape=(10)) - diList = np.zeros(shape=(10)) - xBList = np.zeros(shape=(10)) - yBList = np.zeros(shape=(10)) - x = 0 - falseCounter = 0 - - while True: - (grabbed, image) = self.camera.read() - gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - gray = cv2.GaussianBlur( - gray, (self.gaussianBlurValue, self.gaussianBlurValue), 0 - ) - edged = cv2.Canny(gray, self.cannyLowValue, self.cannyHighValue) - edged = cv2.dilate(edged, None, iterations=1) - edged = cv2.erode(edged, None, iterations=1) - cnts = cv2.findContours( - edged.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE - ) - cnts = cnts[0] if imutils.is_cv2() else cnts[1] - (cnts, _) = contours.sort_contours(cnts) - colors = ( - (0, 0, 255), - (240, 0, 159), - (0, 165, 255), - (255, 255, 0), - (255, 0, 255), - ) - refObj = None - height, width, channels = image.shape - if self.opticalCenter[0] is None or self.opticalCenter[1] is None: - xA = int(width / 2) - yA = int(height / 2) - else: - xA, yA = self.opticalCenter - - maxArea = 0 - for cTest in cnts: - if cv2.contourArea(cTest) > maxArea: - maxArea = cv2.contourArea(cTest) - c = cTest - if cv2.contourArea(c) > 1000: - orig = image.copy() - # approximate to a square (i.e., four contour segments) - cv2.drawContours(orig, [c.astype("int")], -1, (255, 255, 0), 2) - # simplify the contour to get it as square as possible (i.e., remove the noise from the edges) - if findCenter: - sides = 20 - else: - sides = 4 - c = self.simplifyContour(c, sides) - cv2.drawContours(orig, [c.astype("int")], -1, (255, 0, 0), 2) - box = cv2.minAreaRect(c) - angle = box[-1] - if abs(angle + 90) < 30: - _angle = angle + 90 - else: - _angle = angle - box = cv2.cv.BoxPoints(box) if imutils.is_cv2() else cv2.boxPoints(box) - box = np.array(box, dtype="int") - box = perspective.order_points(box) - cv2.drawContours(orig, [box.astype("int")], -1, (0, 255, 0), 2) - # cv2.imwrite('testing/image-out'+str(x)+".png",orig) - if findCenter == False: - M = cv2.getRotationMatrix2D((xA, yA), _angle, 1) - orig = cv2.warpAffine(orig, M, (width, height)) - xB = np.average(box[:, 0]) - yB = np.average(box[:, 1]) - (tl, tr, br, bl) = box - (tlblX, tlblY) = self.midpoint(tl, bl) - (trbrX, trbrY) = self.midpoint(tr, br) - (tltrX, tltrY) = self.midpoint(tl, tr) - (blbrX, blbrY) = self.midpoint(bl, br) - xD = dist.euclidean((tlblX, tlblY), (trbrX, trbrY)) / self.markerX - yD = dist.euclidean((tltrX, tltrY), (blbrX, blbrY)) / self.markerY - if xD == 0: # doing this to catch bad calibrations and stop crashing - xD = 1.0 - if yD == 0: - yD = 1.0 - cos = math.cos(angle * 3.141592 / 180.0) - sin = math.sin(angle * 3.141592 / 180.0) - if _angle < 30: - _angle = _angle * -1.0 - if findCenter == False: - xB, yB = self.translatePoint(xB, yB, xA, yA, _angle) - Dx = dist.euclidean((xA, 0), (xB, 0)) / xD - if xA > xB: - Dx *= -1 - Dy = dist.euclidean((0, yA), (0, yB)) / yD - if yA < yB: - Dy *= -1 - Dist = math.sqrt(Dx ** 2.0 + Dy ** 2.0) - dxList[x] = Dx - dyList[x] = Dy - diList[x] = Dist - xBList[x] = xB - yBList[x] = yB - x += 1 - print("Processed Image " + str(x)) - if x == 10: - break - else: - falseCounter += 1 - if falseCounter == 10: - break - print("Got 10 images processed, " + str(falseCounter) + " images were bad") - if dxList.ndim != 0: - avgDx, stdDx = self.removeOutliersAndAverage(dxList) - avgDy, stdDy = self.removeOutliersAndAverage(dyList) - avgDi, stdDi = self.removeOutliersAndAverage(diList) - avgxB, stdxB = self.removeOutliersAndAverage(xBList) - avgyB, stdyB = self.removeOutliersAndAverage(yBList) - print("avgDx, stdDx:" + str(avgDx) + ", " + str(stdDx)) - print("avgDy, stdDy:" + str(avgDy) + ", " + str(stdDy)) - print("avgDy, stdDy:" + str(avgDy) + ", " + str(stdDy)) - print("avgxB, stdxB:" + str(avgxB) + ", " + str(stdxB)) - print("avgyB, stdyB:" + str(avgyB) + ", " + str(stdyB)) - imgencode = cv2.imencode(".png", orig)[1] - stringData = base64.b64encode(imgencode).decode() - self.data.opticalCalibrationImage = stringData - self.data.opticalCalibrationImageUpdated = True - self.HomingX += avgDx - self.HomingY += avgDy - if ((abs(avgDx) >= 0.125) or (abs(avgDy) >= 0.125)) and ( - findCenter == False - ): - print("Adjusting Location") - self.HomeIn() - else: - xS = self.HomingX + self.HomingPosX * 3 * 25.4 * (1.0 - self.scaleX) - yS = self.HomingY + self.HomingPosY * 3 * 25.4 * (1.0 - self.scaleY) - if self.inMeasureOnlyMode: - self.measuredErrorsX[self.HomingPosX + 15][7 - self.HomingPosY] = xS - self.measuredErrorsY[self.HomingPosX + 15][7 - self.HomingPosY] = yS - elif findCenter == False: - self.calErrorsX[self.HomingPosX + 15][7 - self.HomingPosY] = xS - self.calErrorsY[self.HomingPosX + 15][7 - self.HomingPosY] = yS - # else: - # self.ids.centerX.text = "%.1f" % avgxB - # self.ids.centerY.text = "%.1f" % avgyB - # self.opticalCenter = (avgxB, avgyB) - # self.updateScreenValues(self.HomingPosX, self.HomingPosY) - if self.inAutoMode: - self.on_AutoHome() - else: - print("avgDx,Dy:" + str(avgDx) + ", " + str(avgDy)) - print("Releasing Camera") - self.camera.release() - print("Done") - return False - - def on_AutoHome(self, measureMode=False): - - minX = self.tlX - maxX = self.brX - minY = self.tlY - maxY = self.brY - if measureMode == True: - print("Measure Only") - self.inMeasureOnlyMode = True - if self.inAutoMode == False: - self.HomingX = 0.0 - self.HomingY = 0.0 - self.HomingPosX = minX - self.HomingPosY = minY - self.HomingScanDirection = 1 - self.inAutoMode = True - self.HomeIn() - else: - # note, the self.HomingX and self.HomingY are not reinitialzed here - # The rationale is that the offset for the previous registration point is - # probably a good starting point for this registration point.. - if self.autoScanDirection == 0: # horizontal - print("Horizontal Scan") - if self.inMeasureOnlyMode: - self.HomingX = 0.0 - self.HomingY = 0.0 - self.HomingPosX += self.HomingScanDirection - if (self.HomingPosX == maxX + 1) or (self.HomingPosX == minX - 1): - if self.HomingPosX == maxX + 1: - self.HomingPosX = maxX - else: - self.HomingPosX = minX - self.HomingScanDirection *= -1 - self.HomingPosY -= 1 - if self.HomingPosY > maxY - 1: - self.HomingY -= 7.0 # drop down 7 mm for next square's guess (only) - self.HomeIn() - else: - self.inAutoMode = False - print("Calibration Completed") - # self.printCalibrationErrorValue() - else: # vertical - print("Vertical Scan") - if self.inMeasureOnlyMode: - self.HomingX = 0.0 - self.HomingY = 0.0 - self.HomingPosY -= self.HomingScanDirection - if (self.HomingPosY == maxY - 1) or (self.HomingPosY == minY + 1): - if self.HomingPosY == minY + 1: - self.HomingPosY = minY - else: - self.HomingPosY = maxY - self.HomingScanDirection *= -1 - self.HomingPosX += 1 - if self.HomingPosX != maxX + 1: - self.HomingY -= 7.0 # drop down 7 mm for next square's guess (only) - self.HomeIn() - else: - self.inAutoMode = False - print("Releasing Camera") - self.camera.release() - print("Calibration Completed") - - def on_Calibrate(self, args): - print("Initializing") - self.setCalibrationSettings(args) - print( - "Extents:" - + str(self.tlX) - + ", " - + str(self.tlY) - + " to " - + str(self.brX) - + ", " - + str(self.brY) - ) - if self.camera is None: - print("Starting Camera") - self.camera = cv2.VideoCapture(0) - print("Analyzing Images") - self.on_AutoHome(False) - return True diff --git a/Actions/triangularCalibration.py b/Actions/triangularCalibration.py index 3a109044..97802dc4 100644 --- a/Actions/triangularCalibration.py +++ b/Actions/triangularCalibration.py @@ -3,14 +3,25 @@ class TriangularCalibration(MakesmithInitFuncs): - def cutTestPaternTriangular(self): + + motorYoffsetEst = 0 + chainSagCorrectionEst = 0 + rotationRadiusEst = 0 + + def cutTriangularCalibrationPattern(self): + ''' + Sends command to controller to cut the calibration pattern. + :return: + ''' workspaceHeight = float( self.data.config.getValue("Maslow Settings", "bedHeight") ) workspaceWidth = float(self.data.config.getValue("Maslow Settings", "bedWidth")) - self.data.units = "MM" + # keep track of the units the machine is currently using to revert back if needed. + oldUnits = self.data.units + self.data.gcode_queue.put("G21 ") self.data.gcode_queue.put("G90 ") # Switch to absolute mode self.data.gcode_queue.put("G40 ") @@ -62,6 +73,13 @@ def cutTestPaternTriangular(self): self.data.gcode_queue.put("G90 ") # Switch back to absolute mode self.data.gcode_queue.put("G0 X0 Y0 ") # Move to home location + if oldUnits == "INCHES": + # switch back to inches + self.data.gcode_queue.put("G20 ") # Switch back to inches + + return True + + def calculate(self, result): """ Takes the measured distance and uses it to iteratively calculate the rotationDiskRadius and yMotorOffset @@ -76,7 +94,7 @@ def calculate(self, result): try: distBetweenCuts12 = float(result["cut12"]) - print(distBetweenCuts12) + self.data.console_queue.put(distBetweenCuts12) except: self.data.message_queue.put( "Message: Please enter a number for the distance between cut 1 and cut 2." @@ -85,7 +103,7 @@ def calculate(self, result): try: distBetweenCuts34 = float(result["cut34"]) - print(distBetweenCuts34) + self.data.console_queue.put(distBetweenCuts34) except: self.data.message_queue.put( "Message: Please enter a number for the distance between cut 3 and cut 4." @@ -94,7 +112,7 @@ def calculate(self, result): try: distWorkareaTopToCut5 = float(result["cut5"]) - print(distWorkareaTopToCut5) + self.data.console_queue.put(distWorkareaTopToCut5) except: self.data.message_queue.put( "Message: Please enter a number for the distance between the top of the work area and cut 5." @@ -103,33 +121,13 @@ def calculate(self, result): try: bitDiameter = float(result["bitDiameter"]) - print(bitDiameter) + self.data.console_queue.put(bitDiameter) except: self.data.message_queue.put( "Message: Please enter a number for the bit diameter." ) return False - """ - if self.unitsBtnT.text == 'Units: inches': - if (((distBetweenCuts12*25.4) > workspaceWidth) or ((distBetweenCuts12*25.4) < (workspaceWidth / 2))): - self.data.message_queue.put('Message: The measurement between cut 1 and cut 2 of ' + str(distBetweenCuts12) + ' inches seems wrong.\n\nPlease check the number and enter it again.') - return - if (((distBetweenCuts34*25.4) > workspaceWidth) or ((distBetweenCuts34*25.4) < (workspaceWidth / 2))): - self.data.message_queue.put('Message: The measurement between cut 3 and cut 4 of ' + str(distBetweenCuts34) + ' inches seems wrong.\n\nPlease check the number and enter it again.') - return - if (((distWorkareaTopToCut5*25.4) > (workspaceHeight/2)) or (distWorkareaTopToCut5 < 0)): - self.data.message_queue.put('Message: The measurement between the top edge of the work area and cut 5 of ' + str(distWorkareaTopToCut5) + ' inches seems wrong.\n\nPlease check the number and enter it again.') - return - if ((bitDiameter > 1) or (bitDiameter < 0)): - self.data.message_queue.put('Message: The bit diameter value of ' + str(bitDiameter) + ' inches seems wrong.\n\nPlease check the number and enter it again.') - return - distBetweenCuts12 *= 25.4 - distBetweenCuts34 *= 25.4 - distWorkareaTopToCut5 *= 25.4 - bitDiameter *= 25.4 - else: - """ if True: if (distBetweenCuts12 > workspaceWidth) or ( distBetweenCuts12 < (workspaceWidth / 2) @@ -329,8 +327,8 @@ def calculate(self, result): # Set up the iterative algorithm - print("Previous machine parameters:") - print( + self.data.console_queue.put("Previous machine parameters:") + self.data.console_queue.put( "Motor Spacing: " + str(motorSpacing) + ", Motor Y Offset: " @@ -351,7 +349,7 @@ def calculate(self, result): ChainErrorCut4 = acceptableTolerance n = 0 - print("Iterating for new machine parameters") + self.data.console_queue.put("Iterating for new machine parameters") # Iterate until error tolerance is achieved or maximum number of iterations occurs @@ -517,7 +515,7 @@ def calculate(self, result): motorYcoordEst - distWorkareaTopToCut5 - (bitDiameter / 2) - 12.7 ) - print( + self.data.console_queue.put( "N: " + str(n) + ", Motor Spacing: " @@ -529,7 +527,7 @@ def calculate(self, result): + ", Chain Sag Correction Value: " + str(round(chainSagCorrectionEst, 6)) ) - print( + self.data.console_queue.put( " Chain Error Cut 1: " + str(round(ChainErrorCut1, 4)) + ", Chain Error Cut 2: " @@ -578,22 +576,17 @@ def calculate(self, result): chainSagCorrectionCorrectionScale / 2 ) cutYoffsetCorrectionScale = float(cutYoffsetCorrectionScale / 2) - print("Estimated values out of range, trying again with smaller steps") + self.data.console_queue.put("Estimated values out of range, trying again with smaller steps") if n == numberOfIterations: - self.data.message_queue.put( + self.data.ui_queue1.put("Alert", "Alert", "Message: The machine was not able to be calibrated. Please ensure the work area dimensions are correct and try again." ) - print("Machine parameters could not be determined") + self.data.console_queue.put("Machine parameters could not be determined") return False - # self.horzMeasureT1.disabled = True - # self.horzMeasureT2.disabled = True - # self.vertMeasureT1.disabled = True - # self.enterValuesT.disabled = True - - print("Machine parameters found:") + self.data.console_queue.put("Machine parameters found:") motorYoffsetEst = ( motorYcoordEst - distWorkareaTopToCut5 - (bitDiameter / 2) - 12.7 @@ -603,7 +596,7 @@ def calculate(self, result): rotationRadiusEst = round(rotationRadiusEst, 1) chainSagCorrectionEst = round(chainSagCorrectionEst, 6) - print( + self.data.console_queue.put( "Motor Spacing: " + str(motorSpacing) + ", Motor Y Offset: " @@ -614,19 +607,10 @@ def calculate(self, result): + str(chainSagCorrectionEst) ) - # Update machine parameters - """ - self.data.config.setValue('Maslow Settings', 'motorOffsetY', str(motorYoffsetEst)) - self.data.config.setValue('Advanced Settings', 'rotationRadius', str(rotationRadiusEst)) - self.data.config.setValue('Advanced Settings', 'chainSagCorrection', str(chainSagCorrectionEst)) - - # With new calibration parameters return sled to workspace center + self.motorYoffsetEst = motorYoffsetEst + self.rotationRadiusEst = rotationRadiusEst + self.chainSagCorrectionEst = chainSagCorrectionEst - self.data.gcode_queue.put("G21 ") - self.data.gcode_queue.put("G90 ") - self.data.gcode_queue.put("G40 ") - self.data.gcode_queue.put("G0 X0 Y0 ") - """ return ( motorYoffsetEst, rotationRadiusEst, @@ -634,11 +618,18 @@ def calculate(self, result): cut34YoffsetEst, ) + def acceptTriangularCalibrationResults(self): + ''' + Saves the values that were calculated. + :return: + ''' + self.data.config.setValue('Maslow Settings', 'motorOffsetY', str(self.motorYoffsetEst)) + self.data.config.setValue('Advanced Settings', 'rotationRadius', str(self.rotationRadiusEst)) + self.data.config.setValue('Advanced Settings', 'chainSagCorrection', str(self.chainSagCorrectionEst)) + + self.data.gcode_queue.put("G21 ") + self.data.gcode_queue.put("G90 ") + self.data.gcode_queue.put("G40 ") + self.data.gcode_queue.put("G0 X0 Y0 ") + return True -""" - def switchUnitsT(self): - if self.unitsBtnT.text == 'Units: mm': - self.unitsBtnT.text = 'Units: inches' - else: - self.unitsBtnT.text = 'Units: mm' -""" diff --git a/Background/LogStreamer.py b/Background/LogStreamer.py new file mode 100644 index 00000000..5830123f --- /dev/null +++ b/Background/LogStreamer.py @@ -0,0 +1,46 @@ +from __main__ import socketio + +import time + +''' +This class sends logs to a browser via sockets. It monitors two log queues (log and alog) and sends the messages +if data is available. + +This class is not MakesmithInitFuncs inherited, so it doesn't have direct access to the data. Therefore, it gets +passed the app. +''' +class LogStreamer: + app = None + + def start(self, _app): + self.app = _app + self.app.data.console_queue.put("Starting Log Streamer") + + timeSinceLastLoggerState = time.time() + with self.app.app_context(): + while True: + # this sleep function is needed to keep it non-blocking. + time.sleep(0.001) + # if there is new data to be read + loggerState = self.app.data.logger.getLoggerState() + while (not self.app.data.alog_streamer_queue.empty() or not self.app.data.log_streamer_queue.empty()): + # process a line from the alog queue + if not self.app.data.alog_streamer_queue.empty(): + message = self.app.data.alog_streamer_queue.get() + if message != "": + socketio.emit("message", {"log": "alog", "data": message, "state": loggerState, + "dataFormat": "text"}, namespace="/MaslowCNCLogs", ) + timeSinceLastLoggerState = time.time() + # process a line from the log queue + if not self.app.data.log_streamer_queue.empty(): + message = self.app.data.log_streamer_queue.get() + if message != "": + socketio.emit("message", {"log": "log", "data": message, "state": loggerState, + "dataFormat": "text"}, namespace="/MaslowCNCLogs", ) + timeSinceLastLoggerState = time.time() + currentTime = time.time() + if currentTime - timeSinceLastLoggerState > 5: + socketio.emit("message", {"log": "state", "state": loggerState, "dataFormat":"text"}, + namespace="/MaslowCNCLogs", ) + timeSinceLastLoggerState = currentTime + diff --git a/Background/UIProcessor.py b/Background/UIProcessor.py index 8184f86d..54147e75 100644 --- a/Background/UIProcessor.py +++ b/Background/UIProcessor.py @@ -3,120 +3,168 @@ import time import math import json +import psutil +import webbrowser +from flask import render_template +''' +This class sends messages the browser via sockets. It monitors the ui_controller_queue (where controller messages that +need to be sent to the UI are put) and the ui_queue1 (where webcontrol-initiated messages that need to be sent to the UI +are put). There is a legacy ui_queue (not the missing 1) that I reworked into ui_queue1 but I never refactored. -class UIProcessor: +This class is not MakesmithInitFuncs inherited, so it doesn't have direct access to the data. Therefore, it gets +passed the app. +''' +class UIProcessor: app = None + lastCameraTime = 0 + lastHealthCheck = 0 + previousUploadFlag = None + previousCurrentTool = None + previousPositioningMode = None def start(self, _app): self.app = _app + self.app.data.console_queue.put("starting UI") with self.app.app_context(): while True: time.sleep(0.001) - if self.app.data.opticalCalibrationImageUpdated is True: - self.sendCalibrationMessage( - "OpticalCalibrationImageUpdated", - self.app.data.opticalCalibrationImage, - ) - self.app.data.opticalCalibrationImageUpdated = False + # send health message + self.performHealthCheck() + # send status message + self.performStatusCheck() + # send message to UI client if this is the first time webcontrol is being run. + if self.app.data.config.firstRun: + self.app.data.config.firstRun = False + # I *think* this was added to give time for the browser to open up. + time.sleep(2) + self.activateModal("Notification:", + "New installation detected. If you have an existing groundcontrol.ini file you would like to import, please do so now by pressing Actions->Import groundcontrol.ini file before doing anything else.", + "notification") + # This sends an updated camera image if available (camera) + if self.app.data.cameraImageUpdated is True: + if time.time() - self.lastCameraTime > .25: + self.sendCameraMessage( + "cameraImageUpdated", + self.app.data.cameraImage, + ) + self.app.data.cameraImageUpdated = False + self.lastCameraTime = time.time() + # function is run while queues are not empty while ( - not self.app.data.ui_queue.empty() - ): # if there is new data to be read - message = self.app.data.ui_queue.get() - # send message to web for display in appropriate column - if message != "": - if message[0] != "[" and message[0] != "<": - self.sendControllerMessage(message) - if message[0] == "<": - # print message - self.setPosOnScreen(message) - elif message[0] == "[": - if message[1:4] == "PE:": - # todo: - oo = 1 - # app.setErrorOnScreen(message) - elif message[0:8] == "Message:": - if message.find("adjust Z-Axis") != -1: - print("found adjust Z-Axis in message") - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Resume"}, - namespace="/MaslowCNC", - ) - self.activateModal("Notification:", message[9:]) - elif message[0:7] == "Action:": - if message.find("unitsUpdate") != -1: - units = self.app.data.config.getValue( - "Computed Settings", "units" - ) - socketio.emit( - "requestedSetting", - {"setting": "units", "value": units}, - namespace="/MaslowCNC", - ) - if message.find("gcodeUpdate") != -1: - socketio.emit( - "gcodeUpdate", - { - "data": json.dumps( - [ - ob.__dict__ - for ob in self.app.data.gcodeFile.line - ] - ) - }, - namespace="/MaslowCNC", - ) - if message.find("setAsPause") != -1: - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Pause"}, - namespace="/MaslowCNC", - ) - if message.find("setAsResume") != -1: - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Resume"}, - namespace="/MaslowCNC", - ) - if message.find("positionUpdate") != -1: - msg = message.split( - "_" - ) # everything to the right of the "_" should be the position data already json.dumps'ed - socketio.emit( - "positionMessage", - {"data": msg[1]}, - namespace="/MaslowCNC", - ) - if message.find("updatePorts") != -1: - ports = json.dumps(self.app.data.comPorts) - socketio.emit( - "updatePorts", {"data": ports}, namespace="/MaslowCNC" - ) - elif message[0:6] == "ALARM:": - self.activateModal("Notification:", message[7:]) - elif message == "ok\r\n": - pass # displaying all the 'ok' messages clutters up the display - else: - print(message) + not self.app.data.ui_controller_queue.empty() or not self.app.data.ui_queue1.empty()): # if there is new data to be read + # ui_controller_queue are messages from the controller. + if not self.app.data.ui_controller_queue.empty(): + message = self.app.data.ui_controller_queue.get() + if message != "": + if message[0] == "<": + #call function to parse position message and update UI clients + self.setPosOnScreen(message) + elif message[0] == "[": + # call function to parse position error message and update UI clients + if message[1:4] == "PE:": + self.setErrorOnScreen(message) + elif message[0:13] == "Maslow Paused": + # Maslow has been paused. set uploadFlag to 0 and update the pause button on the + # UI clients. In reality, uploadFlag should already be set to 0 by serialPortThread + # that is, the controller shouldn't be pausing without webcontrol already know it's + # going to pause. + self.app.data.uploadFlag = 0 + # Send '~' upon receiveing the "Maslow Paused" notification. This + # operation is to issue this 'un-pause' command which then lets two ok's to come back + # (one for the tool change and one for the ~) Without this, serialThread won't see + # that the bufferSize = bufferSpace and therefore won't issue any commands. + ## new stuff + # self.app.data.quick_queue.put("~") + ## end new stuff + self.app.data.pausedUnits = self.app.data.units + data = json.dumps({"setting": "pauseButtonSetting", "value": "Resume"}) + socketio.emit("message", + {"command": "requestedSetting", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + elif message[0:12] == "Tool Change:": + # Tool change message detected. + # not sure what manualzaxisadjust is.. ### + self.app.data.manualZAxisAdjust = True + # keep track of whether upload flag was set... can't remember why. + self.app.data.previousUploadStatus = self.app.data.uploadFlag + # remember the current z value so it can be returned to. + self.app.data.pausedzval = self.app.data.zval + # remember the current units because user might switch them to zero the zaxis. + self.app.data.pausedUnits = self.app.data.units + # Remember current positioning mode. + self.app.data.pausedPositioningMode = self.app.data.positioningMode + self.app.data.console_queue.put("found tool change in message") + # send unpause + self.app.data.quick_queue.put("~") + # notify user + self.activateModal("Notification:", message[13:], "notification", resume="resume") + elif message[0:8] == "Message:": + # message received from controller + if message.find("adjust Z-Axis") != -1: + # manual z-axis adjustment requested. + self.app.data.console_queue.put("found adjust Z-Axis in message") + self.app.data.pausedUnits = self.app.data.units + self.activateModal("Notification:", message[9:], "notification", resume="resume") + elif message.find("Unable to find valid") != -1: + # position alarm detected.. chain lengths do not allow for forward kinematic. + # This could occur during the set sprockets to zero process.. so ignore alarm + # if either sprocket distance is zero. + if not self.isChainLengthZero(message): + self.sendAlarm(message); + else: + # something other than above.. + self.activateModal("Notification:", message[9:], "notification") + elif message[0:6] == "ALARM:": + # alarm received from controller + if message.find("The sled is not keeping up") != -1: + self.sendAlarm("Alarm: Sled Not Keeping Up") + elif message.find("Position Lost") != -1: + self.sendAlarm("Alarm: Position Lost. Reset Chains.") + else: + self.sendAlarm(message); + elif message[0:6] == "Unable to": + # not sure what this is for.. probably legacy or something.. TODO: cleanup + if message.find("The sled is not keeping up") != -1: + pass + self.sendAlarm("Alarm: Sled Not Keeping Up") + elif message == "ok\r\n": + pass # displaying all the 'ok' messages clutters up the display + else: + #if something else, send it to the UI to figure out. + self.sendControllerMessage(message) + if not self.app.data.ui_queue1.empty(): + # webcontrol generated messages to be sent to UI client + message = self.app.data.ui_queue1.get() + self.processMessage(message) def setPosOnScreen(self, message): + ''' + Parses the controller message and determines x,y,z coordinates of sled. + Based on ground control's implementation. Added a few hooks for the state. + :param message: + :return: + ''' try: with self.app.app_context(): startpt = message.find("MPos:") + 5 endpt = message.find("WPos:") numz = message[startpt:endpt] - units = "mm" # message[endpt+1:endpt+3] valz = numz.split(",") + state = "" + if message.find("Stop") != -1: + state = "Stopped" + elif message.find("Pause") != -1: + state = "Paused" + elif message.find("Idle") != -1: + state = "Idle" self.app.data.xval = float(valz[0]) self.app.data.yval = float(valz[1]) self.app.data.zval = float(valz[2]) - # print "x:"+str(app.data.xval)+", y:"+str(app.data.yval)+", z:"+str(app.data.zval) - if math.isnan(self.app.data.xval): self.sendControllerMessage("Unable to resolve x Kinematics.") self.app.data.xval = 0 @@ -126,33 +174,450 @@ def setPosOnScreen(self, message): if math.isnan(self.app.data.zval): self.sendControllerMessage("Unable to resolve z Kinematics.") self.app.data.zval = 0 + except: - print("One Machine Position Report Command Misread") + self.app.data.console_queue.put("One Machine Position Report Command Misread") return - position = { - "xval": self.app.data.xval, - "yval": self.app.data.yval, - "zval": self.app.data.zval, - } - self.sendPositionMessage(position) + # compute just how much the sled has moved. + xdiff = abs(self.app.data.xval - self.app.data.xval_prev) + ydiff = abs(self.app.data.yval - self.app.data.yval_prev) + zdiff = abs(self.app.data.zval - self.app.data.zval_prev) - def activateModal(self, title, message): - socketio.emit( - "activateModal", - {"title": title, "message": message}, - namespace="/MaslowCNC", - ) + # compute percentage of gcode that's been completed. + percentComplete = '%.1f' % math.fabs(100 * (self.app.data.gcodeIndex / (len(self.app.data.gcode) - 1))) + "%" + + # if the sled has moved more than a very, very small amount, then go ahead and send. Cuts down on network + # traffic. + if (xdiff + ydiff + zdiff) > 0.01: + # package as json + position = { + "xval": self.app.data.xval, + "yval": self.app.data.yval, + "zval": self.app.data.zval, + "pcom": percentComplete, + "state": state + } + # call command to send the position data + self.sendPositionMessage(position) + # keep track of position + self.app.data.xval_prev = self.app.data.xval + self.app.data.yval_prev = self.app.data.yval + self.app.data.zval_prev = self.app.data.zval + + def setErrorOnScreen(self, message): + ''' + Parses the error message and sends to UI clients. Allows for option to perform forward kinematics to see + where the router actually is based upon chain length errors. Not sure if particularly useful, but its an option + :param message: + :return: + ''' + limit = float(self.app.data.config.getValue("Advanced Settings", "positionErrorLimit")) + computedEnabled = int(self.app.data.config.getValue("WebControl Settings", "computedPosition")) + if limit != 0: + try: + with self.app.app_context(): + startpt = message.find(':') + 1 + endpt = message.find(',', startpt) + leftErrorValueAsString = message[startpt:endpt] + self.app.data.leftError = float(leftErrorValueAsString) / limit + + startpt = endpt + 1 + endpt = message.find(',', startpt) + rightErrorValueAsString = message[startpt:endpt] + self.app.data.rightError = float(rightErrorValueAsString) / limit + + # the two custom firmwares were modified to send the chain lengths as well as the error. This + # allows for forward kinematics to be performed to get sled location. + if self.app.data.controllerFirmwareVersion > 50 and self.app.data.controllerFirmwareVersion < 150: + + startpt = endpt + 1 + endpt = message.find(',', startpt) + bufferSizeValueAsString = message[startpt:endpt] + self.app.data.bufferSize = int(bufferSizeValueAsString) + + startpt = endpt + 1 + endpt = message.find(',', startpt) + leftChainLengthAsString = message[startpt:endpt] + self.app.data.leftChain = float(leftChainLengthAsString) + + startpt = endpt + 1 + endpt = message.find(']', startpt) + rightChainLengthAsString = message[startpt:endpt] + self.app.data.rightChain = float(rightChainLengthAsString) + + else: + # if not custom firmware, then just buffer size remains. + startpt = endpt + 1 + endpt = message.find(']', startpt) + bufferSizeValueAsString = message[startpt:endpt] + self.app.data.bufferSize = int(bufferSizeValueAsString) + # regardless of setting, if not custom firmware there cannot be a forward kinematic compute + computedEnabled = 0 + + if computedEnabled > 0: + self.app.data.computedX, self.app.data.computedY = self.app.data.holeyKinematics.forward( + self.app.data.leftChain, self.app.data.rightChain, self.app.data.xval, self.app.data.yval) + else: + # the -999999 value tells the UI client to ignore the data. + self.app.data.computedX = -999999 + self.app.data.computedY = -999999 + + # sets bad error values to zero. Not sure if needed anymore. + if math.isnan(self.app.data.leftError): + self.app.data.leftErrorValue = 0 + if math.isnan(self.app.data.rightError): + self.app.data.rightErrorValue = 0 + + except: + self.app.data.console_queue.put("One Error Report Command Misread") + return + + # only send the error information if it has changed at least slightly. + leftDiff = abs(self.app.data.leftError - self.app.data.leftError_prev) + rightDiff = abs(self.app.data.rightError - self.app.data.rightError_prev) + + if (leftDiff + rightDiff) >= 0.001: + errorValues = { + "leftError": abs(self.app.data.leftError), + "rightError": abs(self.app.data.rightError), + "computedX": self.app.data.computedX, + "computedY": self.app.data.computedY, + "computedEnabled": computedEnabled + } + self.sendErrorValueMessage(errorValues) + self.app.data.leftError_prev = self.app.data.leftError + self.app.data.rightError_prev = self.app.data.rightError + + def activateModal(self, title, message, modalType, resume="false", progress="false"): + ''' + Sends socket message to UI client to activate a modal + :param title: Title to be used for the modal + :param message: The message that should appear in the modal body + :param modalType: The type of modal (content, notification, alarm).. I don't think content modals come via this + fucntion because modalSize is hardcoded to small. + :param resume: Whether to have a resume button on the modal.. used while gcode is running + :param progress: Whether to show the progress bar/spinner + :return: + ''' + data = json.dumps( + {"title": title, "message": message, "resume": resume, "progress": progress, "modalSize": "small", + "modalType": modalType}) + socketio.emit("message", {"command": "activateModal", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", + ) + + def sendAlarm(self, message): + ''' + Sends an alarm to the UI client via socket. Alarms display as a scrolling marque on right side of screen. + :param message: + :return: + ''' + data = json.dumps({"message": message}) + socketio.emit("message", {"command": "alarm", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", + ) def sendControllerMessage(self, message): - socketio.emit("controllerMessage", {"data": message}, namespace="/MaslowCNC") + ''' + Sends a message from the controller to the UI client. This occurs for messages that aren't processed by + webcontrol in the main UI Processor loop. + :param message: + :return: + ''' + socketio.emit("message", {"command": "controllerMessage", "data": json.dumps(message), "dataFormat": "json"}, + namespace="/MaslowCNC") + + def sendWebMCPMessage(self, message): + ''' + Seems to just send a shutdown message to webmcp.. everything else appears to be commented out. + :param message: + :return: + ''' + # print(message) + # socketio.emit("message", {"command": json.dumps(message), "dataFormat": "json"},namespace="/WebMCP") + socketio.emit("shutdown", namespace="/WebMCP") def sendPositionMessage(self, position): + ''' + Sends the position message to UI client. Not sure why I separated this from the only function that calls it. + :param position: + :return: + ''' + socketio.emit("message", {"command": "positionMessage", "data": json.dumps(position), "dataFormat": "json"}, + namespace="/MaslowCNC") + + def sendErrorValueMessage(self, position): + ''' + Sends the error value message to UI client. Not sure why I separated this from the only function that calls it. + :param position: + :return: + ''' + socketio.emit("message", {"command": "errorValueMessage", "data": json.dumps(position), "dataFormat": "json"}, + namespace="/MaslowCNC") + + def sendCameraMessage(self, message, _data=""): + ''' + Sends message to the UI client regarding camera.. message could be to turn camera display on or off, or to + update the camera display. + :param message: + :param _data: + :return: + ''' + data = json.dumps({"command": message, "data": _data}) socketio.emit( - "positionMessage", {"data": json.dumps(position)}, namespace="/MaslowCNC" + "message", {"command": "cameraMessage", "data": data, "dataFormat": "json"}, namespace="/MaslowCNC" ) - def sendCalibrationMessage(self, message, data): + def updatePIDData(self, message, _data=""): + ''' + Sends PID test data to UI client. + :param message: + :param _data: + :return: + ''' + data = json.dumps({"command": message, "data": _data}) socketio.emit( - "calibrationMessage", {"msg": message, "data": data}, namespace="/MaslowCNC" + "message", {"command": "updatePIDData", "data": data, "dataFormat": "json"}, namespace="/MaslowCNC" + ) + + def sendGcodeUpdate(self): + ''' + Sends the gcode display data to the UI client. Originally this was sent uncompressed but later I added + compression to speed up the transmisison process. + :return: + ''' + if self.app.data.compressedGCode3D is not None: + self.app.data.console_queue.put("Sending Gcode compressed") + # turn on spinner on UI clients + socketio.emit("message", {"command": "showFPSpinner", + "data": len(self.app.data.compressedGCode3D), "dataFormat": "int"}, + namespace="/MaslowCNC", ) + # pause to let the spinner get turned on. + time.sleep(0.25) + # send the data. Once processed by the UI client, the client will turn off the spinner. + socketio.emit("message", {"command": "gcodeUpdateCompressed", + "data": self.app.data.compressedGCode3D, "dataFormat": "base64"}, + namespace="/MaslowCNC", ) + self.app.data.console_queue.put("Sent Gcode compressed") + else: + #send "" if there is no compressed data (i.e., because there's no gcode to compress) + socketio.emit("message", {"command": "gcodeUpdateCompressed", + "data": "", "dataFormat": "base64"}, + namespace="/MaslowCNC", ) + + def sendBoardUpdate(self): + ''' + Sends board information including cutdata. boardData is dimensions, material, ID. cutData is an array that + defines the area that has been cut. + :return: + ''' + boardData = self.app.data.boardManager.getCurrentBoard().getBoardInfoJSON() + if boardData is not None: + self.app.data.console_queue.put("Sending Board Data") + socketio.emit("message", {"command": "boardDataUpdate", + "data": boardData, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + self.app.data.console_queue.put("Sent Board Data") + + cutData = self.app.data.boardManager.getCurrentBoard().getCompressedCutData() + if True: # cutData is not None: + self.app.data.console_queue.put("Sending Board Cut Data compressed") + socketio.emit("message", {"command": "showFPSpinner", + "data": 1, "dataFormat": "int"}, + namespace="/MaslowCNC", ) + time.sleep(0.25) + socketio.emit("message", {"command": "boardCutDataUpdateCompressed", + "data": cutData, "dataFormat": "base64"}, + namespace="/MaslowCNC", ) + self.app.data.console_queue.put("Sent Board Cut Data compressed") + + def unitsUpdate(self): + ''' + Sends the units in use to the UI Client. + :return: + ''' + units = self.app.data.config.getValue( + "Computed Settings", "units" + ) + data = json.dumps({"setting": "units", "value": units}) + socketio.emit("message", {"command": "requestedSetting", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + + def distToMoveUpdate(self): + ''' + Sends the value to populate the distance to move on the frontpage. + :return: + ''' + distToMove = self.app.data.config.getValue( + "Computed Settings", "distToMove" + ) + data = json.dumps({"setting": "distToMove", "value": distToMove}) + socketio.emit("message", {"command": "requestedSetting", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + + def unitsUpdateZ(self): + ''' + Sends the z-units in use to the UI client + :return: + ''' + unitsZ = self.app.data.config.getValue( + "Computed Settings", "unitsZ" + ) + data = json.dumps({"setting": "unitsZ", "value": unitsZ}) + socketio.emit("message", {"command": "requestedSetting", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + + def distToMoveUpdateZ(self): + ''' + Sends the value to populate the distance to move on the z-axis popup. + :return: + ''' + distToMoveZ = self.app.data.config.getValue( + "Computed Settings", "distToMoveZ" ) + data = json.dumps({"setting": "distToMoveZ", "value": distToMoveZ}) + socketio.emit("message", {"command": "requestedSetting", "data": data, "dataFormat": "json"}, + namespace="/MaslowCNC", ) + + def processMessage(self, _message): + ''' + This function process all the webcontrol initiated ui_queue1 messages. I separated this because the main queue + function was getting annoyingly big. it basically just calls the function that has been requested. + :param _message: a json containing the message + :return: + ''' + # parse the message to get to its components + msg = json.loads(_message) + if msg["command"] == "WebMCP": + self.sendWebMCPMessage(msg["message"]) + if msg["command"] == "SendAlarm": + self.sendAlarm(msg["message"]) + if msg["command"] == "Action": + if msg["message"] == "gcodeUpdate": + self.sendGcodeUpdate() + elif msg["message"] == "boardUpdate": + self.sendBoardUpdate() + elif msg["message"] == "unitsUpdate": + self.unitsUpdate() + elif msg["message"] == "distToMoveUpdate": + self.distToMoveUpdate() + elif msg["message"] == "unitsUpdateZ": + self.unitsUpdateZ() + elif msg["message"] == "distToMoveUpdateZ": + self.distToMoveUpdateZ() + elif msg["message"] == "updateTimer": + # Todo: clean this up .. edit: sendCalibrationMessage got deleted somewhere. + #self.sendCalibrationMessage("updateTimer", json.loads(msg["data"])) + pass + elif msg["message"] == "updateCamera": + self.sendCameraMessage("updateCamera", json.loads(msg["data"])) + elif msg["message"] == "updatePIDData": + self.updatePIDData("updatePIDData", json.loads(msg["data"])) + elif msg["message"] == "clearAlarm": + msg["data"] = json.dumps({"data": ""}) + socketio.emit("message", {"command": msg["message"], "data": msg["data"], "dataFormat": "json"}, + namespace="/MaslowCNC") + # I'm pretty sure this can be cleaned up into just a continuation of elif's + else: + if msg["message"] == "setAsPause": + msg["message"] = "requestedSetting" + msg["data"] = json.dumps({"setting": "pauseButtonSetting", "value": "Pause"}) + elif msg["message"] == "setAsResume": + msg["message"] = "requestedSetting" + msg["data"] = json.dumps({"setting": "pauseButtonSetting", "value": "Resume"}) + elif msg["message"] == "updatePorts": + msg["data"] = json.dumps(self.app.data.comPorts) + elif msg["message"] == "closeModals": + title = json.loads(msg["data"]) + msg["data"] = json.dumps({"title": title}) # msg["data"]}) + socketio.emit("message", {"command": msg["message"], "data": msg["data"], "dataFormat": "json"}, + namespace="/MaslowCNC") + # I think I was working on clearing on an issue with the formatting of messages so I added this. I think the + # only function that calls it is the serialPortThread when webcontrol connects to the arduino. + elif msg["command"] == "TextMessage": + socketio.emit("message", {"command": "controllerMessage", "data": msg["data"], "dataFormat": "json"}, + namespace="/MaslowCNC") + # Alerts activate the modal. If alerts are sent on top of each other, it can mess up the UI client display. + elif msg["command"] == "Alert": + self.activateModal(msg["message"], msg["data"], "alert") + elif msg["command"] == "SpinnerMessage": + self.activateModal("Notification:", msg["data"], "notification", progress="spinner") + + def sendCalibrationImage(self, message, _data): + ''' + Sends the calibration image (either an image or the test image) + :param message: + :param _data: + :return: + ''' + + data = json.dumps({"command": message, "data": _data}) + + socketio.emit( + "message", {"command": "updateCalibrationImage", "data": data, "dataFormat": "json"}, namespace="/MaslowCNC" + ) + + def performHealthCheck(self): + ''' + This function sends a message to the UI clients every 5 seconds. It contains cpuUsage, current bufferSize as + reported by the controller, and indication if the uploadFlag is enabled or disabled. The client prevents the + user from performing certain functions when the gcode is being sent (i.e., uploadFlag enabled). + :return: + ''' + currentTime = time.time() + if currentTime - self.lastHealthCheck > 5: + self.lastHealthCheck = currentTime + load = max(psutil.cpu_percent(interval=None, percpu=True)) + weAreBufferingLines = bool(int(self.app.data.config.getValue("Maslow Settings", "bufferOn"))) + if weAreBufferingLines: + bufferSize = self.app.data.bufferSize + else: + bufferSize = -1 + healthData = { + "cpuUsage": load, + "bufferSize": bufferSize, + } + socketio.emit("message", {"command": "healthMessage", "data": json.dumps(healthData), "dataFormat": "json"}, + namespace="/MaslowCNC") + self.performStatusCheck(True) + + def performStatusCheck(self, healthCheckCalled=False): + ''' + This function sends a message to the client if it detects a change in the following parameters: + uploadFlag, positioningMode, currentTool + Also sends on every health check to get new connected clients in sync. + :return: + ''' + update = healthCheckCalled + if self.previousUploadFlag != self.app.data.uploadFlag: + update = True + self.previousUploadFlag = self.app.data.uploadFlag + if self.previousPositioningMode != self.app.data.positioningMode: + update = True + self.previousPositioningMode = self.app.data.positioningMode + if self.previousCurrentTool != self.app.data.currentTool: + update = True + self.previousCurrentTool = self.app.data.currentTool + + #print("positioning mode = "+str(self.app.data.positioningMode)) + + if update: + statusData = { + "uploadFlag": self.app.data.uploadFlag, + "positioningMode": self.app.data.positioningMode, + "currentTool": self.app.data.currentTool, + } + socketio.emit("message", + {"command": "statusMessage", "data": json.dumps(statusData), "dataFormat": "json"}, + namespace="/MaslowCNC") + + def isChainLengthZero(self, msg): + #Message: Unable to find valid machine position for chain lengths 0.00, 0.00 Left Chain Length + # If one chain length is zero, it will report as above. + if msg.find("lengths 0.00") != -1: + return True + if msg.find(", 0.00") != -1: + return True + return False + diff --git a/Background/WebMCPProcessor.py b/Background/WebMCPProcessor.py new file mode 100644 index 00000000..b82f511d --- /dev/null +++ b/Background/WebMCPProcessor.py @@ -0,0 +1,42 @@ +from __main__ import socketio +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs + +import time + +class WebMCPProcessor(MakesmithInitFuncs): + ''' + This class is deprecated as shifting away from WebMCP. + ''' + + app = None + + def start(self, _app): + print("Starting WebMCP Queue Processor") + self.app = _app + self.data.webMCPActive = True + while True: + time.sleep(0.001) + while (not self.data.mcp_queue.empty()): # if there is new data to be read + message = self.data.mcp_queue.get() + #print("MCP Queue:"+message) + if self.app is not None: + with self.app.app_context(): + #print("Emitting:"+message) + socketio.emit("webcontrolMessage", {"data": message}, namespace="/WebMCP") + + def connect(self, _app): + self.app = _app + +class ConsoleProcessor(MakesmithInitFuncs): + + def start(self): + print("Starting Console Queue Processor") + while True: + time.sleep(0.001) + while (not self.data.console_queue.empty()): # if there is new data to be read + message = self.data.console_queue.get() + print(message) + if self.data.webMCPActive: + #print("putting message in mcp_queue") + self.data.mcp_queue.put(message) + diff --git a/Background/controllerTask.py b/Background/controllerTask.py deleted file mode 100644 index 395c64bb..00000000 --- a/Background/controllerTask.py +++ /dev/null @@ -1,185 +0,0 @@ -import time -import math -import json - - -def background_stuff(app): - with app.app_context(): - while True: - time.sleep(0.001) - if app.data.opticalCalibrationImageUpdated is True: - sendCalibrationMessage( - "OpticalCalibrationImageUpdated", app.data.opticalCalibrationImage - ) - app.data.opticalCalibrationImageUpdated = False - while not app.data.message_queue.empty(): # if there is new data to be read - message = app.data.message_queue.get() - # send message to web for display in appropriate column - if message != "": - if message[0] != "[" and message[0] != "<": - sendControllerMessage(message) - if message[0] == "<": - # print message - setPosOnScreen(app, message) - elif message[0] == "$": - app.data.config.receivedSetting(message) - elif message[0] == "[": - if message[1:4] == "PE:": - # todo: - oo = 1 - # app.setErrorOnScreen(message) - elif message[1:8] == "Measure": - measuredDist = float(message[9 : len(message) - 3]) - try: - app.data.measureRequest(measuredDist) - except Exception as e: - print(e) - print("No function has requested a measurement") - elif message[0:13] == "Maslow Paused": - app.data.uploadFlag = 0 - print(message) - elif message[0:8] == "Message:": - if ( - app.data.calibrationInProcess - and message[0:15] == "Message: Unable" - ): # this suppresses the annoying messages about invalid chain lengths during the calibration process - break - app.previousUploadStatus = app.data.uploadFlag - app.data.uploadFlag = 0 - if message.find("adjust Z-Axis") != -1: - print("found adjust Z-Axis in message") - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Resume"}, - namespace="/MaslowCNC", - ) - activateModal("Notification:", message[9:]) - elif message[0:7] == "Action:": - if message.find("unitsUpdate") != -1: - units = app.data.config.getValue("Computed Settings", "units") - socketio.emit( - "requestedSetting", - {"setting": "units", "value": units}, - namespace="/MaslowCNC", - ) - if message.find("gcodeUpdate") != -1: - socketio.emit( - "gcodeUpdate", - { - "data": json.dumps( - [ob.__dict__ for ob in app.data.gcodeFile.line] - ) - }, - namespace="/MaslowCNC", - ) - if message.find("setAsPause") != -1: - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Pause"}, - namespace="/MaslowCNC", - ) - if message.find("setAsResume") != -1: - socketio.emit( - "requestedSetting", - {"setting": "pauseButtonSetting", "value": "Resume"}, - namespace="/MaslowCNC", - ) - if message.find("positionUpdate") != -1: - msg = message.split( - "_" - ) # everything to the right of the "_" should be the position data already json.dumps'ed - socketio.emit( - "positionMessage", {"data": msg[1]}, namespace="/MaslowCNC" - ) - elif message[0:6] == "ALARM:": - app.previousUploadStatus = app.data.uploadFlag - app.data.uploadFlag = 0 - activateModal("Notification:", message[7:]) - elif message[0:8] == "Firmware": - app.data.logger.writeToLog( - "Ground Control Version " + str(app.data.version) + "\n" - ) - print( - "Ground Control " - + str(app.data.version) - + "\r\n" - + message - + "\r\n" - ) - # Check that version numbers match - if float(message[-7:]) < float(app.data.version): - app.data.message_queue.put( - "Message: Warning, your firmware is out of date and may not work correctly with this version of Ground Control\n\n" - + "Ground Control Version " - + str(app.data.version) - + "\r\n" - + message - ) - if float(message[-7:]) > float(app.data.version): - app.data.message_queue.put( - "Message: Warning, your version of Ground Control is out of date and may not work with this firmware version\n\n" - + "Ground Control Version " - + str(app.data.version) - + "\r\n" - + message - ) - elif message == "ok\r\n": - pass # displaying all the 'ok' messages clutters up the display - else: - print(message) - - -def setPosOnScreen(app, message): - try: - with app.app_context(): - startpt = message.find("MPos:") + 5 - endpt = message.find("WPos:") - numz = message[startpt:endpt] - units = "mm" # message[endpt+1:endpt+3] - valz = numz.split(",") - - app.data.xval = float(valz[0]) - app.data.yval = float(valz[1]) - app.data.zval = float(valz[2]) - - # print "x:"+str(app.data.xval)+", y:"+str(app.data.yval)+", z:"+str(app.data.zval) - - if math.isnan(app.data.xval): - sendControllerMessage("Unable to resolve x Kinematics.") - app.data.xval = 0 - if math.isnan(app.data.yval): - sendControllerMessage("Unable to resolve y Kinematics.") - app.data.yval = 0 - if math.isnan(app.data.zval): - sendControllerMessage("Unable to resolve z Kinematics.") - app.data.zval = 0 - except: - print("One Machine Position Report Command Misread") - return - - position = {"xval": app.data.xval, "yval": app.data.yval, "zval": app.data.zval} - sendPositionMessage(position) - - -def activateModal(title, message): - socketio.emit( - "activateModal", {"title": title, "message": message}, namespace="/MaslowCNC" - ) - - -def sendControllerMessage(message): - socketio.emit("controllerMessage", {"data": message}, namespace="/MaslowCNC") - - -def sendPositionMessage(position): - socketio.emit( - "positionMessage", {"data": json.dumps(position)}, namespace="/MaslowCNC" - ) - - -def sendCalibrationMessage(message, data): - print("sending updated image") - # print(len(data)) - socketio.emit( - "calibrationMessage", {"msg": message, "data": data}, namespace="/MaslowCNC" - ) diff --git a/Background/messageProcessor.py b/Background/messageProcessor.py index a988bfb2..6f30bec8 100644 --- a/Background/messageProcessor.py +++ b/Background/messageProcessor.py @@ -11,78 +11,163 @@ class MessageProcessor(MakesmithInitFuncs): """ def start(self): + while True: + # give other threads opportunity to run time.sleep(0.001) - while ( - not self.data.message_queue.empty() - ): # if there is new data to be read + # check for available update file every hour. + if time.time()-self.data.lastChecked > 60*60: + self.data.lastChecked = time.time() + self.data.releaseManager.checkForLatestPyRelease() + self.data.helpManager.checkForUpdatedHelp() + # process messages while queue is not empty. Everything else is on hold until queue is cleared. + while ( not self.data.message_queue.empty() ): # if there is new data to be read message = self.data.message_queue.get() - # send message to web for display in appropriate column if message != "": - if message[0] != "[" and message[0] != "<": - self.data.ui_queue.put(message) - if message[0] == "<": - self.data.ui_queue.put(message) - elif message[0] == "$": - self.data.config.receivedSetting(message) - elif message[0] == "[": - if message[1:4] == "PE:": - # todo: - oo = 1 - # app.setErrorOnScreen(message) - elif message[1:8] == "Measure": - measuredDist = float(message[9 : len(message) - 3]) - try: - self.data.measureRequest(measuredDist) - except Exception as e: - print(e) - print("No function has requested a measurement") - elif message[0:13] == "Maslow Paused": - self.data.uploadFlag = 0 - print(message) - elif message[0:8] == "Message:": - if ( - self.data.calibrationInProcess - and message[0:15] == "Message: Unable" - ): # this suppresses the annoying messages about invalid chain lengths during the calibration process - break - self.data.previousUploadStatus = self.data.uploadFlag - self.data.uploadFlag = 0 - if message.find("adjust Z-Axis") != -1: - self.data.ui_queue.put(message) - elif message[0:6] == "ALARM:": - self.data.previousUploadStatus = self.data.uploadFlag - self.data.uploadFlag = 0 - self.data.ui_queue.put(message) - elif message[0:8] == "Firmware": - self.data.logger.writeToLog( - "Ground Control Version " + str(self.data.version) + "\n" - ) - print( - "Ground Control " - + str(self.data.version) - + "\r\n" - + message - + "\r\n" - ) - # Check that version numbers match - if float(message[-7:]) < float(self.data.version): - self.data.ui_queue.put( - "Message: Warning, your firmware is out of date and may not work correctly with this version of Ground Control\n\n" - + "Ground Control Version " - + str(self.data.version) - + "\r\n" - + message - ) - if float(message[-7:]) > float(self.data.version): - self.data.ui_queue.put( - "Message: Warning, your version of Ground Control is out of date and may not work with this firmware version\n\n" - + "Ground Control Version " - + str(self.data.version) - + "\r\n" - + message - ) - elif message == "ok\r\n": - pass # displaying all the 'ok' messages clutters up the display - else: - print(message) + if message[0] == "<": + # message to be processed by UIProcessor + self.data.ui_controller_queue.put(message) + elif message[0] == "$": + # setting received from controller + self.data.config.receivedSetting(message) + elif message[0] == "[": + # positional error or measurement message + if message[1:4] == "PE:": + # send to UIProcessor for processing + self.data.ui_controller_queue.put(message) + elif message[1:8] == "Measure": + # get distance and call the callback. + measuredDist = float(message[9 : len(message) - 3]) + try: + self.data.measureRequest(measuredDist) + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.console_queue.put("No function has requested a measurement") + elif message[0:14] == "FAKE_SERVO off": + self.data.fakeServoStatus = False + self.data.console_queue.put(message) + elif message[0:13] == "FAKE_SERVO on": + self.data.fakeServoStatus = True + self.data.console_queue.put(message) + elif message[0:13] == "Maslow Paused": + # received controller-initiated pause message. Free controller to accept moves and send + # message to UIProcessor to process. + self.data.uploadFlag = 0 + self.data.ui_controller_queue.put(message) + elif message[0:8] == "Message:": + # suppress the annoying messages about invalid chain lengths during the calibration process + if (self.data.calibrationInProcess and message[0:15] == "Message: Unable"): + break + # track the previous uploadFlag and stop uploading. Likely am going to receive a message + # that requires a pause. + self.data.previousUploadStatus = self.data.uploadFlag + self.data.uploadFlag = 0 + if message.find("adjust Z-Axis") != -1: + # z-axis must not be enabled. Send message to UIProcessor for processing. + # set manualZAxisAdjust, but not sure its needed if manual adjustment is being used. + # Todo: cleanup if needed. + self.data.manualZAxisAdjust = True + self.data.ui_controller_queue.put(message) + if message[0:15] == "Message: Unable": + # received an error message from forward.kinematics that chain lengths don't resolve. + # send to UIProcessor for processing. + self.data.ui_controller_queue.put(message) + elif message[0:6] == "ALARM:": + # something bad happened, likley sled not keeping up. + # keep track of upload status, pause run, and send to UIProcessor for processing. + self.data.previousUploadStatus = self.data.uploadFlag + self.data.uploadFlag = 0 + self.data.ui_controller_queue.put(message) + elif message[0:8] == "Firmware": + ''' + send this alarm clear if Firmware is received. The thought is that if there is an alarm and + this message appears, then the connection to the arduino has been reset. + ''' + self.data.ui_queue1.put("Action", "clearAlarm", "") + + self.data.logger.writeToLog("Ground Control Version " + str(self.data.version) + "\n") + self.data.console_queue.put("WebControl " + str(self.data.version) + "\r\n" + message + "\r\n") + # Check that version numbers match + self.data.controllerFirmwareVersion = float(message[16-len(message):]) + tmpVersion = self.data.controllerFirmwareVersion + # <50 implies a stock firmware + if tmpVersion <50: + if self.data.stockFirmwareVersion is not None: + if tmpVersion < float(self.data.stockFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "
Warning, you are running a stock firmware that is not up to date. This version may not work correctly with this version of WebControl.
" + + "Please, click Actions->Upgrade Stock Firmware to update the controller to the latest WebControl-compatible code.
" + +"WebControl:"+str(float(self.data.stockFirmwareVersion))+", Controller:"+str(tmpVersion)+"
" + ) + elif tmpVersion > float(self.data.stockFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "Warning, you are running a stock firmware that is newer than what is included in WebControl. This version may not work correctly with this version of WebControl.
" + +"WebControl:"+str(float(self.data.stockFirmwareVersion))+", Controller:"+str(tmpVersion)+"
" + ) + # 50 <= x < 100 implies holey calibration firmware + if tmpVersion >= 50 and tmpVersion < 100: + if self.data.holeyFirmwareVersion is not None: + if tmpVersion < float(self.data.holeyFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "Warning, you are running a Holey Calibration firmware that is not up to date. This version may not work correctly with this version of WebControl.
" + + "Please, click Actions->Upgrade Holey Firmware to update the controller to the latest WebControl-compatible code.
" + +"WebControl:"+str(float(self.data.holeyFirmwareVersion))+", Controller:"+str(tmpVersion)+"
" + ) + elif tmpVersion > float(self.data.holeyFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "Warning, you are running a Holey Calibration firmware that is newer than what is included in WebControl. This version may not work correctly with this version of WebControl.
" + "WebControl:"+str(float(self.data.holeyFirmwareVersion))+", Controller:"+str(tmpVersion)+"
" + ) + # x >= 100 implies optical calibration firmware + if tmpVersion >= 100: + if self.data.customFirmwareVersion is not None: + if tmpVersion < float(self.data.customFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "Warning, you are running a custom firmware that is not" + + " up to date. This version may not work correctly with" + + " this version of WebControl.
" + + "Please, click Actions->Upgrade Custom Firmware to" + + " update the controller to the latest" + + " WebControl-compatible code.
" + + "WebControl:" + + str(float(self.data.customFirmwareVersion)) + + ", Controller:"+str(tmpVersion)+"
" + ) + elif tmpVersion > float(self.data.customFirmwareVersion): + self.data.ui_queue1.put("Alert", "Alert", + "Warning, you are running a custom firmware that is " + + "newer than what is included in WebControl. This version" + + " may not work correctly with this version of WebControl." + + "
WebControl:" + + str(float(self.data.customFirmwareVersion)) + + ", Controller:" + str(tmpVersion)+"
" + ) + + elif message == "ok\r\n": + pass # displaying all the 'ok' messages clutters up the display + + ### + ### Velocity PID Testing Processing + ### Watch for messages and log data + ### + elif message[0:26] == "--PID Velocity Test Stop--": + self.data.actions.velocityPIDTestRun("stop", "") + elif self.data.inPIDVelocityTest: + self.data.actions.velocityPIDTestRun("running", message) + elif message[0:27] == "--PID Velocity Test Start--": + self.data.actions.velocityPIDTestRun("start", "") + ### + ### Positional PID Testing Processing + ### Watch for messages and log data + ### + elif message[0:26] == "--PID Position Test Stop--": + self.data.actions.positionPIDTestRun("stop", "") + elif self.data.inPIDPositionTest: + self.data.actions.positionPIDTestRun("running", message) + elif message[0:27] == "--PID Position Test Start--": + self.data.actions.positionPIDTestRun("start", "") + + else: + # Just send it to the UIProcessor for processing. + self.data.ui_controller_queue.put(message) + diff --git a/Background/webcamVideoStream.py b/Background/webcamVideoStream.py index d0101739..dff3157f 100644 --- a/Background/webcamVideoStream.py +++ b/Background/webcamVideoStream.py @@ -1,41 +1,189 @@ -from threading import Thread -import cv2 +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs +import threading +#import cv2 +import time +import base64 -class WebcamVideoStream: +class WebcamVideoStream(MakesmithInitFuncs): + th = None + lastCameraRead = 0 + stopped = True + suspended = False + stream = None + height = 640 + width = 480 + fps = 5 + videoSize = "640x480" + cameraSleep = 0.01 + debugCamera = False + def __init__(self, src=0): # initialize the video camera stream and read the first frame # from the stream - self.stream = cv2.VideoCapture(src) - (self.grabbed, self.frame) = self.stream.read() + #self.stream = cv2.VideoCapture(src) + + + #(self.grabbed, self.frame) = self.stream.read() # initialize the variable used to indicate if the thread should # be stopped - self.stopped = False + #self.stopped = True + #self.suspended = False print("Camera initialized") - def start(self): + def getSettings(self, src=0): + cameraOff = False + #if self.stream is None: + #self.stream = cv2.VideoCapture(src) + #self.setVideoSize() + #self.setFPS() + #cameraOff = True + # if debugging, print camera values + ''' + if self.debugCamera: + self.data.console_queue.put("CAP_PROP_POS_MSEC=" + str(self.stream.get(cv2.CAP_PROP_POS_MSEC))) + self.data.console_queue.put("CAP_PROP_POS_FRAMES=" + str(self.stream.get(cv2.CAP_PROP_POS_FRAMES))) + self.data.console_queue.put("CAP_PROP_POS_AVI_RATIO=" + str(self.stream.get(cv2.CAP_PROP_POS_AVI_RATIO))) + self.data.console_queue.put("CAP_PROP_FRAME_WIDTH=" + str(self.stream.get(cv2.CAP_PROP_FRAME_WIDTH))) + self.data.console_queue.put("CAP_PROP_FRAME_HEIGHT=" + str(self.stream.get(cv2.CAP_PROP_FRAME_HEIGHT))) + self.data.console_queue.put("CAP_PROP_FPS=" + str(self.stream.get(cv2.CAP_PROP_FPS))) + self.data.console_queue.put("CAP_PROP_FOURCC=" + str(self.stream.get(cv2.CAP_PROP_FOURCC))) + self.data.console_queue.put("CAP_PROP_FRAME_COUNT=" + str(self.stream.get(cv2.CAP_PROP_FRAME_COUNT))) + self.data.console_queue.put("CAP_PROP_FORMAT=" + str(self.stream.get(cv2.CAP_PROP_FORMAT))) + self.data.console_queue.put("CAP_PROP_MODE=" + str(self.stream.get(cv2.CAP_PROP_MODE))) + self.data.console_queue.put("CAP_PROP_BRIGHTNESS=" + str(self.stream.get(cv2.CAP_PROP_BRIGHTNESS))) + self.data.console_queue.put("CAP_PROP_CONTRAST=" + str(self.stream.get(cv2.CAP_PROP_CONTRAST))) + self.data.console_queue.put("CAP_PROP_SATURATION=" + str(self.stream.get(cv2.CAP_PROP_SATURATION))) + self.data.console_queue.put("CAP_PROP_HUE=" + str(self.stream.get(cv2.CAP_PROP_HUE))) + self.data.console_queue.put("CAP_PROP_GAIN=" + str(self.stream.get(cv2.CAP_PROP_GAIN))) + self.data.console_queue.put("CAP_PROP_EXPOSURE=" + str(self.stream.get(cv2.CAP_PROP_EXPOSURE))) + self.data.console_queue.put("CAP_PROP_CONVERT_RGB=" + str(self.stream.get(cv2.CAP_PROP_CONVERT_RGB))) + self.data.console_queue.put("CAP_PROP_RECTIFICATION=" + str(self.stream.get(cv2.CAP_PROP_RECTIFICATION))) + self.data.console_queue.put("CAP_PROP_ISO_SPEED=" + str(self.stream.get(cv2.CAP_PROP_ISO_SPEED))) + self.data.console_queue.put("CAP_PROP_BUFFERSIZE=" + str(self.stream.get(cv2.CAP_PROP_BUFFERSIZE))) + + if cameraOff: + self.stream.release() + self.stream = None + ''' + + def start(self, src = 0): # start the thread to read frames from the video stream + return self + ''' + following is unreachable + print("Starting camera thread") - Thread(target=self.update, args=()).start() - print("Camera thread started") + if self.stream is None: + self.stream = cv2.VideoCapture(src) + (self.grabbed, self.frame) = self.stream.read() + + #self.setVideoSize() + #self.setFPS() + + if self.stopped == True: + self.stopped = False + self.th = threading.Thread(target=self.update) + self.th.daemon = True + self.th.start() + print("Camera thread started") + #self.data.ui_queue.put("Action:updateCamera_on") + self.data.ui_queue1.put("Action", "updateCamera", "on") + else: + print("Camera already started") return self + ''' def update(self): # keep looping infinitely until the thread is stopped + return + ''' while True: + time.sleep(self.cameraSleep) + if not self.data.continuousCamera or time.time()-self.lastCameraRead < 20: + (self.grabbed, self.frame) = self.stream.read() + self.suspended = False + else: + if self.suspended == False: + #self.data.ui_queue.put("Action:updateCamera_off") + self.data.ui_queue1.put("Action", "updateCamera", "off") + self.suspended=True # if the thread indicator variable is set, stop the thread if self.stopped: + self.stream.release() + self.stream = None + #self.data.ui_queue.put("Action:updateCamera_off") + self.data.ui_queue1.put("Action", "updateCamera", "off") return - # otherwise, read the next frame from the stream - (self.grabbed, self.frame) = self.stream.read() + #if self.frame is not None: + #small = cv2.resize(self.frame, (256,192)) + #imgencode = cv2.imencode(".png",small )[1] + #stringData = base64.b64encode(imgencode).decode() + #self.data.cameraImage = stringData + #self.data.cameraImageUpdated = True + ''' def read(self): # return the frame most recently read - print("Reading camera frame") + #print("Reading camera frame") + return None + ''' + if self.suspended: + (self.grabbed, self.frame) = self.stream.read() + self.suspended = False + #self.data.ui_queue.put("Action:updateCamera_on") + self.data.ui_queue1.put("Action", "updateCamera", "on") + self.lastCameraRead = time.time() + + if self.stopped: + self.start() return self.frame + ''' def stop(self): # indicate that the thread should be stopped print("Stopping camera") self.stopped = True + + def status(self): + if self.stopped: + return("stopped") + if self.suspended: + return("suspended") + return("running") + + def changeSetting(self, key, value): + return + ''' + if key == 'fps' and value != self.fps: + self.fps = value + if self.stream is not None: + self.setFPS() + if key == 'videoSize' and value != self.videoSize: + self.videoSize = value + if self.stream is not None: + self.setVideoSize() + if key == 'cameraSleep' and value != self.cameraSleep: + if value<1: + print("changing sleep interval") + self.cameraSleep = value + if self.stream is not None: + self.stopped = True + self.stream.join() + self.start() + ''' + + def setFPS(self): + return + #self.stream.set(cv2.CAP_PROP_FPS, self.fps) + + def setVideoSize(self): + return + width = 640 + height = 480 + if self.videoSize == '1024x768': + width = 1024 + height = 768 + #self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, height) + #self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, width) diff --git a/Background/scheduler.py b/Boards/__init__.py similarity index 100% rename from Background/scheduler.py rename to Boards/__init__.py diff --git a/Boards/boardManager.py b/Boards/boardManager.py new file mode 100644 index 00000000..38aea373 --- /dev/null +++ b/Boards/boardManager.py @@ -0,0 +1,293 @@ +from DataStructures.makesmithInitFuncs import MakesmithInitFuncs +from Boards.boards import Board +import math +import base64 +import numpy as np +from scipy.spatial import ConvexHull, convex_hull_plot_2d + +class BoardManager(MakesmithInitFuncs): + ''' + This class implements the management of various boards (workpieces) that are tracked. + Each board is stored as a separate file containing both description, dimensions, makeup of board as well + as an array (1-inch resolution) of what parts of the board have been cut. + ''' + currentBoard = None + + def __init__(self): + # can't do much because data hasn't been initialized yet + pass + + def initializeNewBoard(self): + ''' + Create a new board and initialize it to the size of the bed. Default to 0.75 inches thick + :return: + ''' + self.currentBoard = Board() + bedWidth = round(float(self.data.config.getValue("Maslow Settings", "bedWidth"))/25.4, 2) + bedHeight = round(float(self.data.config.getValue("Maslow Settings", "bedHeight"))/25.4, 2) + + self.currentBoard.updateBoardInfo("-", "-", bedHeight, bedWidth, 0.75, 0, 0, "inches") + # this creates the necessary compressed data files to avoid None errors. + self.currentBoard.compressCutData() + + def getCurrentBoard(self): + ''' + Returns current board + :return: + ''' + return self.currentBoard + + def getCurrentBoardFilename(self): + ''' + Returns current board filename + :return: + ''' + return self.currentBoard.boardFilename + + def editBoard(self, result): + ''' + Updates the board information with data from the form. + :param result: + :return: + ''' + try: + if self.currentBoard.updateBoardInfo(result["boardID"], result["material"], result["height"], + result["width"], result["thickness"], result["centerX"], + result["centerY"], result["units"]): + # send update to the UI clients + self.data.ui_queue1.put("Action", "boardUpdate", "") + return True + else: + return False + except Exception as e: + self.data.console_queue.put(str(e)) + return False + + def saveBoard(self, fileName, directory): + ''' + Saves the board data. File format is one line of board data in json format and one line of cut data in + compressed and base64 encoded format. + :param fileName: filename to use + :param directory: directory to save file + :return: + ''' + if fileName is "": # Blank the g-code if we're loading "nothing" + return False + if directory is "": + return False + try: + fileToWrite = directory + "/" + fileName + file = open(fileToWrite, "w+") + print(fileToWrite) + self.currentBoard.setFilename(fileName) + boardData = self.currentBoard.getBoardInfoJSON() + file.write(boardData+'\n') + boardCutData = self.currentBoard.getCompressedCutData() + boardCutData = base64.b64encode(boardCutData) + boardCutData = boardCutData.decode('utf-8') + file.write(boardCutData) + file.write('\n') + print("Closing File") + file.close() + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.console_queue.put("Board Save File Error") + self.data.ui_queue1.put("Alert", "Alert", "Cannot save board.") + return False + return True + + def loadBoard(self, fileName): + ''' + Load the board data. File format is one line of board data in json format and one line of cut data in + compressed and base64 encoded format. + :param fileName: filename to load + :param directory: directory to load file from + :return: + ''' + if fileName is "": # Blank the g-code if we're loading "nothing" + return False + try: + file = open(fileName, "r") + print(fileName) + lines = file.readlines() + self.currentBoard.updateBoardInfoJSON(lines[0]) + # make sure got at least 2 lines... + if len(lines) > 1: + loadedCutData = base64.b64decode(lines[1].encode('utf-8')) + self.currentBoard.updateCompressedCutData(loadedCutData) + print("Closing File") + file.close() + self.data.ui_queue1.put("Action", "boardUpdate", "") + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.console_queue.put("Board Open File Error") + self.data.ui_queue1.put("Alert", "Alert", "Cannot open board.") + return False + return True + + def processGCode(self): + ''' + Using the gcode in memory, mark the areas in the array that the gcode passes through at a height less than 0. + :return: + ''' + boardWidth = self.currentBoard.width + boardHeight = self.currentBoard.height + boardLeftX = self.currentBoard.centerX - boardWidth/2 + boardRightX = self.currentBoard.centerX + boardWidth / 2 + boardTopY = self.currentBoard.centerY + boardWidth/2 + boardBottomY = self.currentBoard.centerY - boardWidth / 2 + + # calculate the extents of the array and its offset. + pointsX = math.ceil(boardWidth) + pointsY = math.ceil(boardHeight) + offsetX = pointsX / 2 - self.currentBoard.centerX + offsetY = pointsY / 2 - self.currentBoard.centerY + + if self.data.units == "MM": + scale = 1/25.4 + else: + scale = 1 + homeX = self.data.gcodeShift[0]*scale + homeY = self.data.gcodeShift[1]*scale + + + # initialize an array + cutPoints = [False for i in range( pointsX * pointsY )] + + # process gcode + for line in self.data.gcodeFile.line3D: + if line.type == "circle": + if line.points[0][0] + homeX >= boardLeftX and line.points[0][0] + homeX <= boardRightX and line.points[0][1] + homeY >= boardBottomY and line.points[0][1] + homeY <= boardTopY and line.points[0][2] < 0: + pointx = self.constrain(round(line.points[0][0] + offsetX + homeX), 0, pointsX) + pointy = self.constrain(round(line.points[0][1] + offsetY + homeY), 0, pointsY) + cutPoints[pointx + pointy * pointsX] = True + #cutPoints[int(line.points[0][0] + offsetX) + int(line.points[0][1] + offsetY) * pointsX] = True + else: + for x in range(len(line.points)): + if x != len(line.points)-1: + x0 = line.points[x][0]+homeX + y0 = line.points[x][1]+homeY + z0 = line.points[x][2] + + x1 = line.points[x+1][0]+homeX + y1 = line.points[x+1][1]+homeY + z1 = line.points[x+1][2] + + lineLength = math.sqrt( (x0-x1) ** 2 + (y0-y1) ** 2 + (z0-z1) ** 2) + if lineLength > 0.25: + for l in range(int(lineLength*4)+1): + xa = x0 + (x1 - x0) / (lineLength * 4) * l + ya = y0 + (y1 - y0) / (lineLength * 4) * l + za = z0 + (z1 - z0) / (lineLength * 4) * l + if za < 0: + if xa >= boardLeftX and xa <= boardRightX and ya >= boardBottomY and ya <= boardTopY: + pointx = self.constrain(round(xa + offsetX), 0, pointsX) + pointy = self.constrain(round(ya + offsetY), 0, pointsY) + cutPoints[pointx + pointy * pointsX] = True + #cutPoints[round(xa + offsetX) + round(ya + offsetY) * pointsX] = True + + else: + if line.points[x][2] < 0: + if line.points[x][0] + homeX >= boardLeftX and line.points[x][0] + homeX <= boardRightX and line.points[x][1] + homeY >= boardBottomY and line.points[x][1] + homeX <= boardTopY: + pointx = self.constrain(round(line.points[x][0] + offsetX + homeX), 0, pointsX) + pointy = self.constrain(round(line.points[x][1] + offsetY + homeY), 0, pointsY) + cutPoints[pointx + pointy * pointsX] = True + if line.points[x+1][2] < 0: + if line.points[x+1][0] + homeX >= boardLeftX and line.points[x+1][0] + homeX <= boardRightX and line.points[x+1][1] + homeY >= boardBottomY and line.points[x+1][1] + homeY <= boardTopY: + pointx = self.constrain(round(line.points[x+1][0] + offsetX + homeX), 0, pointsX) + pointy = self.constrain(round(line.points[x+1][1] + offsetY + homeY), 0, pointsY) + cutPoints[pointx + pointy * pointsX] = True + + + else: + if line.points[x][2] < 0: + if line.points[x][0] + homeX >= boardLeftX and line.points[x][0] + homeX <= boardRightX and line.points[x][1] + homeY >= boardBottomY and line.points[x][1] + homeY <= boardTopY: + pointx = self.constrain(round(line.points[x][0] + offsetX + homeX), 0, pointsX) + pointy = self.constrain(round(line.points[x][1] + offsetY + homeY), 0, pointsY) + cutPoints[pointx + pointy * pointsX] = True + + # send this array to the current board for updating + self.currentBoard.updateCutPoints(cutPoints) + # update the UI clients. + self.data.ui_queue1.put("Action", "boardUpdate", "") + return True + + def clearBoard(self): + ''' + Clear the current board cut data + :return: + ''' + self.currentBoard.clearCutPoints() + # update the UI clients. + self.data.ui_queue1.put("Action", "boardUpdate", "") + return True + + def constrain(self, value, lower, upper): + ''' + Helper routine to constrain values + :param value: value to be constrained + :param lower: lower limit + :param upper: upper limit + :return: + ''' + if value < lower: + return lower + if value > upper-1: + return upper-1 + return value + + def trimBoard(self, result): + ''' + Processes message from form to trim the board cut data. + :param result: + :return: + ''' + trimTop = round(float(result["trimTop"]), 3) + trimBottom = round(float(result["trimBottom"]), 3) + trimLeft = round(float(result["trimLeft"]), 3) + trimRight = round(float(result["trimRight"]), 3) + units = result["units"] + retval = self.currentBoard.trimBoard(trimTop, trimBottom, trimLeft, trimRight, units) + self.data.ui_queue1.put("Action", "boardUpdate", "") + return retval + + ''' + old stuff not used anymore.. keeping it around for a bit + # Todo: delete when done with + def processGCode(self): + #points = np.random.rand(30,2) + points = self.data.gcodeFile.getLinePoints() + #try: + index = 0 + + for line in points: + #print(line) + point = np.zeros(2) + #print(point) + point[0] = line[0] + point[1] = line[1] + #print(point) + if index == 0: + npoints = [point] + else: + npoints = np.append(npoints, [point], axis=0) + index += 1 + #print(npoints) + hull = ConvexHull(npoints) + print("here0") + cutPoints = [] + for vertex in hull.vertices: + cutPoints.append([hull.points[vertex][0],hull.points[vertex][1],0]) + print("here1") + self.currentBoard.updateCutPoints(cutPoints) + self.data.ui_queue1.put("Action", "boardUpdate", "") + #except Exception as e: + # self.data.console_queue.put(str(e)) + # self.data.console_queue.put("Gcode Processing Error") + # self.data.ui_queue1.put("Alert", "Alert", "Cannot process gcode.") + # return False + + return True + ''' + + diff --git a/Boards/boards.py b/Boards/boards.py new file mode 100644 index 00000000..a15a73fc --- /dev/null +++ b/Boards/boards.py @@ -0,0 +1,237 @@ +import json +import gzip +import io +import math + + +class Board(): + ''' + Class is the generalization of a board that tracks board information including areas that have been cut. Resolution + is 1-inch squares. + ''' + def __init__(self): + # can't do much because data hasn't been initialized yet + pass + + width = 0 + height = 0 + thickness = 0 + centerX = 0 + centerY = 0 + + boardID = "Undefined" + material = "Undefined" + boardFilename = "" + + ### + # old, not sure is used + # todo: delete boardPoints and cutPoints2 if not used + boardPoints = [] + cutPoints2 = [] + ### + + cutPoints = [] + pointsPerInch = 1 + compressedCutData = "" + + def updateBoardInfo(self, boardID, material, height, width, thickness, centerX, centerY, units): + ''' + Updates the cut data + :param boardID: Name give to board, as entered by user + :param material: Type of material of board, as entered by user + :param height: height of board, stored in inches but entered as defined by units + :param width: width of board, stored in inches but entered as defined by units + :param thickness: thickness of board, stored in inches but entered as defined by units + :param centerX: X-coordinate of center of board, stored in inches but entered as defined by units + :param centerY: Y-coordinate of center of board, stored in inches but entered as defined by units + :param units: units the measurements are provided in. + :return: + ''' + + try: + scale = 1 + if units == "mm": + scale = 25.4 + self.width = round(float(width)/scale, 2) + self.height = round(float(height)/scale, 2) + self.thickness = round(float(thickness)/scale, 2) + self.centerX = round(float(centerX)/scale, 2) + self.centerY = round(float(centerY)/scale, 2) + self.boardID = boardID + self.material = material + return True + except Exception as e: + print(e) + return False + + def updateCutPoints(self, data): + ''' + Using array received from board manager, updates cut data and recompresses. + :param data: + :return: + ''' + if len(self.cutPoints) == 0: + #if cutdata isn't an array yet, then use the cut data (i.e., no existing data) + self.cutPoints = data + else: + # existing cut data exists, so add this data + for x in range(len(self.cutPoints)): + # if the data in data is cut (True) then make the data in cutPoints true as well.. i.e., performs + # an OR operation + if data[x]: + self.cutPoints[x] = True + # call to compress the cut data. + self.compressCutData() + + def trimBoard(self, trimTop, trimBottom, trimLeft, trimRight, units): + ''' + Trims portion of the board and updates cutdata + :param trimTop: Amount to trim off top, units as defined in units + :param trimBottom: Amount to trim off bottom, units as defined in units + :param trimLeft: Amount to trim off left, units as defined in units + :param trimRight: Amount to trim off left, units as defined in units + :param units: units of trim measurements + :return: + ''' + # adjust for correct units (i.e., convert to inches and round to 2 decimal points) + scale = 1 + if units == "mm": + scale = 25.4 + trimTop = round(float(trimTop)/scale, 2) + trimBottom = round(float(trimBottom) / scale, 2) + trimLeft = round(float(trimLeft) / scale, 2) + trimRight = round(float(trimRight) / scale, 2) + + # determine the new array extents + aPointsX = math.ceil(self.width) + aPointsY = math.ceil(self.height) + bPointsX = math.ceil(self.width - trimLeft - trimRight) + bPointsY = math.ceil(self.height - trimTop - trimBottom) + + # create a new cut array + bCutPoints = [False for i in range( bPointsX * bPointsY )] + + ''' + print(aPointsX) + print(aPointsY) + print(bPointsX) + print(bPointsY) + print(trimTop) + print(trimBottom) + print(trimLeft) + print(trimRight) + ''' + # if cutPoints are found... + if (len(self.cutPoints)!=0): + # iterate through new array and translate to old cut array points and assign matrix values. + for x in range(bPointsX): + for y in range(bPointsY): + ax = x + int(trimLeft) + ay = y + int(trimBottom) + ib = x+y*bPointsX + ia = ax+ay*aPointsX + #print(str(ib) +", "+ str(ia)) + bCutPoints[ib] = self.cutPoints[ia] + # assign cutPoints to new array + self.cutPoints = bCutPoints + # update dimensions + self.height = self.height - trimTop - trimBottom + self.width = self.width - trimLeft - trimRight + # compress data + self.compressCutData() + return True + + def setFilename(self, data): + ''' + Sets the filename of the board + :param data: + :return: + ''' + self.boardFilename = data + + def getPoints(self): + ''' + Don't think this is used. + todo: remove if not needed + :return: + ''' + return self.boardPoints + + def getCutPoints(self): + ''' + Returns uncompressed cut point data + :return: + ''' + return self.cutPoints + + def getCompressedCutData(self): + ''' + returns compressed cut point data + :return: + ''' + return self.compressedCutData + + def getBoardInfoJSON(self): + ''' + Returns board information formatted as a json + :return: + ''' + tstr = json.dumps({"width": self.width, "height": self.height, "thickness": self.thickness, "centerX": self.centerX, "centerY": self.centerY, "boardID":self.boardID, "material":self.material, "fileName":self.boardFilename}) + return tstr + + def updateBoardInfoJSON(self, data): + ''' + updates board data from a json + :param data: + :return: + ''' + boardData = json.loads(data) + self.width = boardData["width"] + self.height = boardData["height"] + self.thickness = boardData["thickness"] + self.centerX = boardData["centerX"] + self.centerY = boardData["centerY"] + self.boardID = boardData["boardID"] + self.material = boardData["material"] + self.boardFilename = boardData["fileName"] + + def compressCutData(self): + ''' + Compresses cut data using gzip + :return: + ''' + tstr = json.dumps(self.cutPoints) + out = io.BytesIO() + with gzip.GzipFile(fileobj=out, mode="w") as f: + f.write(tstr.encode()) + self.compressedCutData = out.getvalue() + + def getUnCompressedCutDataJSON(self): + ''' + returns uncompressed cut data in json format. + Not sure is used anymore + todo: delete if not needed + :return: + ''' + tstr = json.dumps(self.cutPoints) + return tstr + + def updateCompressedCutData(self, data): + ''' + Updates the compressed cut data using data, uncompresses it and assigns it to cutPoints. Used in loading + a board + :param data: + :return: + ''' + self.compressedCutData = data + mem = io.BytesIO(data) + f = gzip.GzipFile(fileobj=mem, mode="rb") + self.cutPoints = json.loads(f.read().decode()) + + def clearCutPoints(self): + ''' + clears the cut points and compressed cut data + :return: + ''' + self.cutPoints = [] + self.compressedCutData = None \ No newline at end of file diff --git a/Connection/nonVisibleWidgets.py b/Connection/nonVisibleWidgets.py index b5b09bd4..95cbdc98 100644 --- a/Connection/nonVisibleWidgets.py +++ b/Connection/nonVisibleWidgets.py @@ -1,12 +1,24 @@ +import sys +import distro +import os +import platform from DataStructures.makesmithInitFuncs import MakesmithInitFuncs from Connection.serialPort import SerialPort from File.gcodeFile import GCodeFile from File.importFile import ImportFile from Actions.actions import Actions from Actions.triangularCalibration import TriangularCalibration -from Actions.opticalCalibration import OpticalCalibration +from Actions.holeyCalibration import HoleyCalibration +from Actions.HoleySimulationKinematics import Kinematics as HoleyKinematics +from Actions.gpioActions import GPIOActions from Background.messageProcessor import MessageProcessor - +from Background.WebMCPProcessor import WebMCPProcessor +from Background.WebMCPProcessor import ConsoleProcessor +from Background.webcamVideoStream import WebcamVideoStream +from Boards.boardManager import BoardManager +from ReleaseManager.releaseManager import ReleaseManager +from HelpManager.helpManager import HelpManager +#from GCodeOptimizer.gcodeOptimizer import GCodeOptimizer class NonVisibleWidgets(MakesmithInitFuncs): """ @@ -21,8 +33,17 @@ class NonVisibleWidgets(MakesmithInitFuncs): importFile = ImportFile() actions = Actions() triangularCalibration = TriangularCalibration() - opticalCalibration = OpticalCalibration() + holeyCalibration = HoleyCalibration() + holeyKinematics = HoleyKinematics() messageProcessor = MessageProcessor() + mcpProcessor = WebMCPProcessor() + consoleProcessor = ConsoleProcessor() + camera = WebcamVideoStream() + gpioActions = GPIOActions() + boardManager = BoardManager() + releaseManager = ReleaseManager() + helpManager = HelpManager() + #gcodeOptimizer = GCodeOptimizer() def setUpData(self, data): """ @@ -43,12 +64,75 @@ def setUpData(self, data): data.importFile = self.importFile data.actions = self.actions data.triangularCalibration = self.triangularCalibration - data.opticalCalibration = self.opticalCalibration + data.holeyCalibration = self.holeyCalibration + data.holeyKinematics = self.holeyKinematics data.messageProcessor = self.messageProcessor + data.mcpProcessor = self.mcpProcessor + data.consoleProcessor = self.consoleProcessor + data.camera = self.camera + data.gpioActions = self.gpioActions + data.boardManager = self.boardManager + data.releaseManager = self.releaseManager + data.helpManager = self.helpManager + #data.gcodeOptimizer = self.gcodeOptimizer + + if hasattr(sys, '_MEIPASS'): + data.platform = "PYINSTALLER" + data.platformHome = sys._MEIPASS + + data.pyInstallPlatform = platform.system().lower() + + if data.pyInstallPlatform == "windows": + if platform.machine().endswith('64'): + data.pyInstallPlatform = "win64" + if platform.machine().endswith('32'): + data.pyInstallPlatform = "win32" + + if data.pyInstallPlatform == "linux": + _platform = distro.linux_distribution()[0].lower() + print("##") + print(_platform) + print("##") + if _platform.find("raspbian") != -1: + data.pyInstallPlatform = 'rpi' + print("----") + print(data.pyInstallPlatform) + + + if getattr(sys, 'frozen', False): + if hasattr(sys, '_MEIPASS'): + if sys._MEIPASS.find("_MEI") == -1: + data.pyInstallType = "singledirectory" + else: + data.pyInstallType = "singlefile" + else: + data.pyInstallType = "singledirectory" + + print(data.pyInstallType) + print("----") + self.serialPort.setUpData(data) self.gcodeFile.setUpData(data) self.importFile.setUpData(data) self.actions.setUpData(data) self.triangularCalibration.setUpData(data) - self.opticalCalibration.setUpData(data) + self.holeyCalibration.setUpData(data) + self.holeyKinematics.setUpData(data) self.messageProcessor.setUpData(data) + self.mcpProcessor.setUpData(data) + self.consoleProcessor.setUpData(data) + self.camera.setUpData(data) + self.camera.getSettings() + self.gpioActions.setUpData(data) + self.gpioActions.setup() + self.boardManager.setUpData(data) + self.boardManager.initializeNewBoard() + self.releaseManager.setUpData(data) + self.helpManager.setUpData(data) + #self.gcodeOptimizer.setUpData(data) + + #set up kinematics with current settings + self.holeyKinematics.initializeSettings() + #self.camera.start() + + diff --git a/Connection/serialPort.py b/Connection/serialPort.py index d4473f59..17471475 100644 --- a/Connection/serialPort.py +++ b/Connection/serialPort.py @@ -7,7 +7,7 @@ import threading import schedule import time - +import json class SerialPort(MakesmithInitFuncs): """ @@ -17,6 +17,7 @@ class SerialPort(MakesmithInitFuncs): The actual connection is run in a separate thread by an instance of a SerialPortThread object. """ + serialPortRequest = "" # COMports = ListProperty(("Available Ports:", "None")) @@ -33,8 +34,7 @@ def setPort(self, port): Defines which port the machine is attached to """ - print("update ports") - print(port) + self.data.console_queue.put(port) self.data.comport = port def connect(self, *args): @@ -57,14 +57,29 @@ def connect(self, *args): def openConnection(self, *args): # This function opens the thread which handles the input from the serial port - # It only needs to be run once, it is run by connecting to the machine - # print("Attempting to open connection to controller") if not self.data.connectionStatus: - # self.data.message_queue is the queue which handles passing CAN messages between threads - # print "serialport.self.app.logger="+str(self.app.logger) + #self.data.ui_queue.put( + # "Action: connectionStatus:_" + json.dumps({'status': 'disconnected', 'port': 'none'}) + #) # the "_" facilitates the parse + self.data.ui_queue1.put("Action", "connectionStatus", + {'status': 'disconnected', 'port': 'none', 'fakeServoStatus': self.data.fakeServoStatus}) x = SerialPortThread() x.data = self.data self.th = threading.Thread(target=x.getmessage) self.th.daemon = True self.th.start() + else: + self.data.ui_queue1.put("Action", "connectionStatus", + {'status': 'connected', 'port': self.data.comport, 'fakeServoStatus': self.data.fakeServoStatus}) + #self.data.ui_queue.put( + # "Action: connectionStatus:_" + json.dumps({'status': 'connected', 'port': self.data.comport}) + #) # the "_" facilitates the parse + + def closeConnection(self): + self.serialPortRequest = "requestToClose" + + def getConnectionStatus(self): + return self.serialPortRequest + + diff --git a/Connection/serialPortThread.py b/Connection/serialPortThread.py index e0ed2756..441fc588 100644 --- a/Connection/serialPortThread.py +++ b/Connection/serialPortThread.py @@ -3,6 +3,8 @@ import serial import time from collections import deque +import json +import re class SerialPortThread(MakesmithInitFuncs): @@ -24,111 +26,234 @@ class SerialPortThread(MakesmithInitFuncs): bufferSize = 126 # The total size of the arduino buffer bufferSpace = bufferSize # The amount of space currently available in the buffer lengthOfLastLineStack = deque() - + weAreBufferingLines = 0 # Minimum time between lines sent to allow Arduino to cope # could be smaller (0.02) however larger number doesn't seem to impact performance MINTimePerLine = 0.05 - def _write(self, message, isQuickCommand=False): - # message = message + 'L' + str(len(message) + 1 + 2 + len(str(len(message))) ) + serialInstance = None + def _write(self, message, isQuickCommand=False): + ''' + Sends messages to controller. No change from what I can tell from ground control + :param message: + :param isQuickCommand: + :return: + ''' taken = time.time() - self.lastWriteTime if taken < self.MINTimePerLine: # wait between sends - # self.data.logger.writeToLog("Sleeping: " + str( taken ) + "\n") time.sleep(self.MINTimePerLine) # could use (taken - MINTimePerLine) message = message + "\n" - # message = message.encode() - print("Sending: " + str(message)) - - self.bufferSpace = self.bufferSpace - len( - message - ) # shrink the available buffer space by the length of the line - - self.machineIsReadyForData = False - - # if this is a quick message sent as soon as the button is pressed (like stop) then put it on the right side of the queue - # because it is the first message sent, otherwise put it at the end (left) because it is the last message sent - if isQuickCommand: - if message[0] == "!": - # if we've just sent a stop command, the buffer is now empty on the arduino side - self.lengthOfLastLineStack.clear() - self.bufferSpace = self.bufferSize - len(message) - self.lengthOfLastLineStack.append(len(message)) + + if len(message) > 1: + self.data.console_queue.put("Sending: " + str(message).rstrip('\n')) + + self.bufferSpace = self.bufferSpace - len( + message + ) # shrink the available buffer space by the length of the line + + self.machineIsReadyForData = False + + # if this is a quick message sent as soon as the button is pressed (like stop) then put it on the right side of the queue + # because it is the first message sent, otherwise put it at the end (left) because it is the last message sent + if isQuickCommand: + if message[0] == "!": + # if we've just sent a stop command, the buffer is now empty on the arduino side + self.lengthOfLastLineStack.clear() + self.bufferSpace = self.bufferSize - len(message) + self.lengthOfLastLineStack.append(len(message)) + else: + self.lengthOfLastLineStack.append(len(message)) else: - self.lengthOfLastLineStack.append(len(message)) + self.lengthOfLastLineStack.appendleft(len(message)) + + ''' + Monitor gcode for positioning mode change requests + ''' + positioningMode = None + findG90 = message.rfind("G90") + findG91 = message.rfind("G91") + + if findG90 != -1 and findG91 != -1: + if findG90 > findG91: + positioningMode = 0 + else: + positioningMode = 1 + else: + if findG90 != -1: + positioningMode = 0 + if findG91 != -1: + positioningMode = 1 + + message = message.encode() + + ''' + try sending message + ''' + try: + self.serialInstance.write(message) + # Update positioning mode after message has been sent. + # In 'try' block to maintain state integrity if message send fails. + if (positioningMode is not None): + self.data.positioningMode = positioningMode + #print("Set positioning mode: " + str(positioningMode)) + self.data.logger.writeToLog("Sent: " + str(message.decode())) + except: + self.data.console_queue.put("write issue") + self.data.logger.writeToLog("Send FAILED: " + str(message.decode())) + + self.lastWriteTime = time.time() else: - self.lengthOfLastLineStack.appendleft(len(message)) - - message = message.encode() - try: - self.serialInstance.write(message) - self.data.logger.writeToLog("Sent: " + str(message)) - except: - print("write issue") - self.data.logger.writeToLog("Send FAILED: " + str(message)) - - self.lastWriteTime = time.time() + self.data.console_queue.put("Skipping: " + str(message).rstrip('\n')) def _getFirmwareVersion(self): + ''' + Send command to have controller report details + :return: + ''' self.data.gcode_queue.put("B05 ") def _setupMachineUnits(self): + ''' + Send command to put controller in correct units state. + :return: + ''' if self.data.units == "INCHES": self.data.gcode_queue.put("G20 ") else: self.data.gcode_queue.put("G21 ") + def _requestSettingsUpdate(self): + ''' + Send command to have controller report settings + :return: + ''' + self.data.gcode_queue.put("$$") + def sendNextLine(self): """ Sends the next line of gcode to the machine """ if self.data.gcodeIndex < len(self.data.gcode): - if self.data.uploadFlag: - self._write(self.data.gcode[self.data.gcodeIndex]) + line = self.data.gcode[self.data.gcodeIndex] + # filter comments from line + filtersparsed = re.sub(r'\(([^)]*)\)', '', line) # replace mach3 style gcode comments with newline + line = re.sub(r';([^\n]*)?', '',filtersparsed) # replace standard ; initiated gcode comments with newline + # check if command is going to be issued that pauses the controller. + self.managePause(line) + self.manageToolChange(line) + if not line.isspace(): # if all spaces, don't send. likely a comment. + # put gcode home shift here.. only if in absolute mode (G90) + if self.data.positioningMode == 0: + line = self.data.gcodeFile.moveLine(line) + self._write(line) + # if there is a units change, then update the UI client so position messages are processed correctly. + if line.find("G20") != -1: + if self.data.units != "INCHES": + self.data.actions.updateSetting("toInches", 0, True) # value = doesn't matter + if line.find("G21") != -1: + if self.data.units != "MM": + self.data.actions.updateSetting("toMM", 0, True) # value = doesn't matter + # send a gcode update to UI client. + self.data.actions.sendGCodePositionUpdate(self.data.gcodeIndex) + # track current target Z-Axis position + z = re.search("Z(?=.)(([ ]*)?[+-]?([0-9]*)(\.([0-9]+))?)", line) + if z: + self.data.currentZTarget = float(z.groups()[0]) + #self.data.currentZTargetUnits = self.data.units + + # increment gcode index + if self.data.gcodeIndex + 1 < len(self.data.gcode): + self.data.gcodeIndex = self.data.gcodeIndex + 1 + else: + self.data.uploadFlag = 0 + self.data.gcodeIndex = 0 + self.data.console_queue.put("Gcode Ended") + + def managePause(self, line): + if line.find("M0 ") != -1 or line.find("M00") != -1 or line.find("M1 ") != -1 or line.find("M01") != -1: + print("found M command for pause") + self.data.uploadFlag = -1 + self.data.pausedUnits = self.data.units + self.data.ui_queue1.put("Action", "setAsResume", "") + + def manageToolChange(self, line): + if line.find("M6 ") != -1 or line.find("M06") != -1: + # if this is a different tool, the controller will respond with a 'Tool Change:' and pause. + # if this is a the same tool as the controller is currently tracking, it will continue on. + # first, determine the tool being called for... + toolNumber = int(self.extractGcodeValue(line, 'T', 0)) + print(toolNumber) + print(self.data.currentTool) + # so, in the first case... + if toolNumber != self.data.currentTool: + # set uploadFlag to -1 to turn off sending more lines (after this one) + # -1 means it was set by an M command + print("found M command") + self.data.uploadFlag = -1 + self.data.currentTool = toolNumber + self.data.pausedUnits = self.data.units + ## new stuff + #self.data.quick_queue.put("~") + #self.data.ui_queue1.put("Action", "setAsResume", "") + ## end new stuff + + # now, the issue is that if the controller gets reset, then the tool number will revert to 0.. so + # on serial port connect/reconnect, reinitialize tool number to 0 + + # but in the second case, just continue on.. + + ## new stuff + #self.data.quick_queue.put("~") + #self.data.ui_queue1.put("Action", "setAsResume", "") + ## end new stuff + + def closeConnection(self): + ''' + Closes the serial connection and updates the status. Needed for firmware upgrades. + :return: + ''' + if self.serialInstance is not None: + self.serialInstance.close() + self.data.serialPort.serialPortRequest = "Closed" + print("connection closed at serialPortThread") + else: + print("serial Instance is none??") + return - # increment gcode index - if self.data.gcodeIndex + 1 < len(self.data.gcode): - self.data.gcodeIndex = self.data.gcodeIndex + 1 - else: - self.data.uploadFlag = 0 - self.data.gcodeIndex = 0 - print("Gcode Ended") def getmessage(self): - # opens a serial connection called self.serialInstance + ''' + Opens a serial connection called self.serialInstance + Processes all the messages from controller. + :return: + ''' # check for serial version being > 3 if float(serial.VERSION[0]) < 3: - self.data.ui_queue.put( - "Pyserial version 3.x is needed, version " + self.data.ui_queue1.put("Alert", "Incompability Detected", + "Pyserial version 3.x is needed, but version " + serial.VERSION + " is installed" ) - weAreBufferingLines = bool( - int(self.data.config.getValue("Maslow Settings", "bufferOn")) - ) + self.weAreBufferingLines = bool(int(self.data.config.getValue("Maslow Settings", "bufferOn")) ) try: - print("connecting") self.data.comport = self.data.config.getValue("Maslow Settings", "COMport") + connectMessage = "Trying to connect to controller on " +self.data.comport + self.data.console_queue.put(connectMessage) self.serialInstance = serial.Serial( self.data.comport, 57600, timeout=0.25 ) # self.data.comport is the com port which is opened except: - print(self.data.comport + " is unavailable or in use") - # self.data.ui_queue.put("\n" + self.data.comport + " is unavailable or in use") + #self.data.console_queue.put(self.data.comport + " is unavailable or in use") pass else: - print("\r\nConnected on port " + self.data.comport + "\r\n") - self.data.ui_queue.put( - "\r\nConnected on port " + self.data.comport + "\r\n" - ) - print("\r\nConnected on port " + self.data.comport + "\r\n") - gcode = "" - msg = "" - subReadyFlag = True + self.data.console_queue.put("\r\nConnected on port " + self.data.comport + "\r\n") + self.data.ui_queue1.put("Action", "connectionStatus", {'status': 'connected', 'port': self.data.comport, 'fakeServoStatus': self.data.fakeServoStatus}) + self.data.ui_queue1.put("TextMessage", "", "Connected on port " + self.data.comport) self.serialInstance.parity = ( serial.PARITY_ODD @@ -139,13 +264,12 @@ def getmessage(self): self.serialInstance.parity = serial.PARITY_NONE self.serialInstance.open() - # print "port open?:" - # print self.serialInstance.isOpen() self.lastMessageTime = time.time() self.data.connectionStatus = 1 - self._getFirmwareVersion() self._setupMachineUnits() + self._requestSettingsUpdate() + self.data.currentTool = 0 # This is necessary since the controller will have reset tool to zero. while True: @@ -153,28 +277,32 @@ def getmessage(self): # ------------------------------------------------------------------------------------- lineFromMachine = "" + # check to see if the serail port needs to be closed (for firmware upgrade) + if self.data.serialPort.serialPortRequest == "requestToClose": + print("processing request to close") + self.closeConnection() + # do not change status just yet... + return + try: if self.serialInstance.in_waiting > 0: + # the need for the .decode() at the end MIGHT be a python3 thing. ground control doesn't have + # it. lineFromMachine = self.serialInstance.readline().decode() - # lineFromMachine = lineFromMachine.encode('utf-8') self.lastMessageTime = time.time() self.data.message_queue.put(lineFromMachine) except: pass # Check if a line has been completed - if lineFromMachine == "ok\r\n" or ( - len(lineFromMachine) >= 6 and lineFromMachine[0:6] == "error:" - ): + if lineFromMachine == "ok\r\n" or (len(lineFromMachine) >= 6 and lineFromMachine[0:6] == "error:"): self.machineIsReadyForData = True - if ( - bool(self.lengthOfLastLineStack) is True - ): # if we've sent lines to the machine - self.bufferSpace = ( - self.bufferSpace + self.lengthOfLastLineStack.pop() - ) # free up that space in the buffer - - # Write to the machine if ready + if bool(self.lengthOfLastLineStack) is True: # if we've sent lines to the machine + # free up that space in the buffer + self.bufferSpace = (self.bufferSpace + self.lengthOfLastLineStack.pop()) + + + # Write to the machine if ready # ------------------------------------------------------------------------------------- # send any emergency instructions to the machine if there are any @@ -183,46 +311,101 @@ def getmessage(self): self._write(command, True) # send regular instructions to the machine if there are any - if self.bufferSpace == self.bufferSize and self.machineIsReadyForData: + #print("bSpace="+str(self.bufferSpace)+", bSize="+str(self.bufferSize)+", ready="+str(self.machineIsReadyForData)) + if self.bufferSpace == self.bufferSize: if self.data.gcode_queue.empty() != True: - command = self.data.gcode_queue.get_nowait() + " " - self._write(command) - - # Send the next line of gcode to the machine if we're running a program. Will send lines to buffer if there is space - # and the feature is turned on - if weAreBufferingLines: - try: - if self.bufferSpace > len( - self.data.gcode[self.data.gcodeIndex] - ): # if there is space in the buffer keep sending lines + if self.machineIsReadyForData: + command = self.data.gcode_queue.get_nowait()# + " " + # filter out comments + # replace mach3 style gcode comments with newline + filtersparsed = re.sub(r'\(([^)]*)\)', '', command) + # replace standard ; initiated gcode comments with '' + command = re.sub(r';([^\n]*)?', '', filtersparsed) + # if there's something left.. + if len(command) != 0: + command = command + " " + self.manageToolChange(command) + self._write(command) + # if units have changed, update settings and notify UI clients to stay in sync + if command.find("G20") != -1: + if self.data.units != "INCHES": + self.data.actions.updateSetting("toInches", 0, True) # value = doesn't matter + if command.find("G21") != -1: + if self.data.units != "MM": + self.data.actions.updateSetting("toMM", 0, True) # value = doesn't matter + # send updated gcode position to UI clients + self.data.actions.sendGCodePositionUpdate(self.data.gcodeIndex, recalculate=True) + # Send the next line of gcode to the machine if we're running a program and uploadFlag is enabled. Will + # send lines to buffer if there is space and the feature is turned on + # Also, don't send if there's still data in gcode_queue. + if self.data.uploadFlag == 1 and len(self.data.gcode) > 0 and self.data.gcode_queue.empty(): + if self.weAreBufferingLines: + try: + # todo: clean this up because the line gets filtered twice.. once to make sure its not too + # long, and the second in the sendNextLine command.. bit redundant. + line = self.data.gcode[self.data.gcodeIndex] + # replace mach3 style gcode comments with newline + filtersparsed = re.sub(r'\(([^)]*)\)', '', line) + # replace standard ; initiated gcode comments with newline + line = re.sub(r';([^\n]*)?', '', filtersparsed) + # if there is space in the buffer send line + if self.bufferSpace > len(line): + self.sendNextLine() + except IndexError: + self.data.console_queue.put("index error when reading gcode") + # we don't want the whole serial thread to close if the gcode can't be sent because of an + # index error (file deleted...etc) + else: + if (self.bufferSpace == self.bufferSize and self.machineIsReadyForData ): + # if the receive buffer is empty and the machine has acked the last line complete... self.sendNextLine() - except IndexError: - print( - "index error when reading gcode" - ) # we don't want the whole serial thread to close if the gcode can't be sent because of an index error (file deleted...etc) - else: - if ( - self.bufferSpace == self.bufferSize - and self.machineIsReadyForData - ): # if the receive buffer is empty and the machine has acked the last line complete - self.sendNextLine() - # Check for serial connection loss + # Check for serial connection loss # ------------------------------------------------------------------------------------- - if time.time() - self.lastMessageTime > 2: - print("Connection Timed Out") - self.data.ui_queue.put("Connection Timed Out\n") - if self.data.uploadFlag: - self.data.ui_queue.put( - "Message: USB connection lost. This has likely caused the machine to loose it's calibration, which can cause erratic behavior. It is recommended to stop the program, remove the sled, and perform the chain calibration process. Press Continue to override and proceed with the cut." - ) + if time.time() - self.lastMessageTime > 5: + self.data.console_queue.put("Connection Timed Out") + if self.data.uploadFlag > 0: + self.data.ui_queue1.put("Alert", "Connection Lost", + "Message: USB connection lost. This has likely caused the machine to loose it's calibration, which can cause erratic behavior. It is recommended to stop the program, remove the sled, and perform the chain calibration process. Press Continue to override and proceed with the cut.") else: - self.data.ui_queue.put( - "It is possible that the serial port selected is not the one used by the Maslow's Arduino,\nor that the firmware is not loaded on the Arduino." - ) + self.data.ui_queue1.put("SendAlarm", "Alarm: Connection Failed or Invalid Firmware", "") self.data.connectionStatus = 0 self.serialInstance.close() return - # Sleep between passes to save CPU - time.sleep(0.001) + if self.data.requestSerialClose: + # close down the serial port. + self.data.requestSerialClose = False + self.data.connectionStatus = 0 + self.serialInstance.close() + + time.sleep(0.01) + + def extractGcodeValue(self, readString, target, defaultReturn): + # Reads a string and returns the value of number following the target character. + # if no number is found, defaultReturn is returned + + begin = readString.find(target) + end = self.findEndOfNumber(readString, begin + 1) + numberAsString = readString[begin + 1: end] + try: + numberAsFloat = float(numberAsString) + except: + return defaultReturn + + if begin == -1: + return defaultReturn + else: + return numberAsFloat + + def findEndOfNumber(self, textString, index): + # Return the index of the last digit of the number beginning at the index passed in + + i = index + while i < len(textString): + if textString[i].isdigit() or textString[i] == '.': + i = i + 1 + else: + return i + return i + diff --git a/DataStructures/data.py b/DataStructures/data.py index 24d88134..c678f4ce 100644 --- a/DataStructures/data.py +++ b/DataStructures/data.py @@ -1,6 +1,7 @@ from time import time from DataStructures.logger import Logger from DataStructures.loggingQueue import LoggingQueue +from DataStructures.uiQueue import UIQueue from config.config import Config import queue @@ -22,11 +23,41 @@ class Data: # Gcodes contains all of the lines of gcode in the opened file clients = [] gcode = [] - version = "1.23" + gcodeFileUnits = "INCHES" + sentCustomGCode = "" + compressedGCode = None + compressedGCode3D = None + + version = "1.27" + stockFirmwareVersion = None + customFirmwareVersion = None + holeyFirmwareVersion = None + controllerFirmwareVersion = 0 + ''' + interpreter version + if using a compiler newer than 3.5, use the newer string format methods without %'s + ''' + pythonVersion35 = False + + ''' + Version Updater + ''' + lastChecked = -1 + pyInstallCurrentVersion = 0.9335 + pyInstallUpdateAvailable = False + pyInstallUpdateBrowserUrl = "" + pyInstallUpdateVersion = 0 + pyInstallPlatform = "rpi" + pyInstallType = "singlefile" + pyInstallInstalledPath = "" + + # all of the available COM ports comPorts = [] # This defines which COM port is used comport = "" + # stores value to indicate whether or not fake_servo is enabled + fakeServoStatus = False # The index of the next unread line of Gcode gcodeIndex = 0 # Index of changes in z @@ -36,12 +67,20 @@ class Data: # holds the address of the g-code file so that the gcode can be refreshed gcodeFile = "" importFile = "" + # holds the current gcode x,y,z position + currentGcodePost = [0.0, 0.0, 0.0] # the current position of the cutting head currentpos = [0.0, 0.0, 0.0] target = [0.0, 0.0, 0.0] units = "MM" + # Gcode positioning mode: + # 0 = G90 (Absolute) + # 1 = G91 (Relative) + positioningMode = 0 tolerance = 0.5 gcodeShift = [0.0, 0.0] # the amount that the gcode has been shifted + currentTool = 0 # current tool.. upon startup, 0 is the same value as what the controller would have. + currentZTarget = 0 # current target for Z-Axis move. Need to track so if user pauses, we can move back to that spot. message = "" # used to update the client logger = Logger() # the module which records the machines behavior to review later config = Config() @@ -57,21 +96,49 @@ class Data: # sets a flag if the gcode is being uploaded currently uploadFlag = 0 previousUploadStatus = 0 + manualZAxisAdjust = False # this is used to determine the first time the position is received from the machine firstTimePosFlag = 0 # report if the serial connection is open connectionStatus = 0 # is the calibration process currently underway 0 -> false calibrationInProcess = False - + inPIDVelocityTest = False + inPIDPositionTest = False + PIDVelocityTestVersion = 0 + PIDPositionTestVersion = 0 + GPIOButtonService = False + clidisplay = False # if you have a local RPI display + wiiPendantPresent = False # has user set wiimote as active? + wiiPendantConnected = False # is the wiimote BT connected? + sledMoving = False # for LED light status + zMoving = False + #gcode position flags for imported gcode to show outer bouding box + gcode_x_min = 2000 + gcode_x_max = -2000 + gcode_y_min = 1000 + gcode_y_max = -1000 + gcode_z_min = 20 + gcode_z_max = -20 + moveDistance = 10 # CHECK UNITS!! + """ Pointers to Objects """ serialPort = None # this is a pointer to the program serial port object + requestSerialClose = False # this is used to request the serialThread to gracefully close the port triangularCalibration = None # points to the triangular calibration object + holeyCalibration = None # points to the triangular calibration object opticalCalibration = None # points to the optical calibration object opticalCalibrationImage = None # stores the current image opticalCalibrationImageUpdated = False # stores whether its been updated or not + opticalCalibrationTestImage = None # stores the current image + opticalCalibrationTestImageUpdated = False # stores whether its been updated or not + cameraImage = None + cameraImageUpdated = False + continuousCamera = False + gpioActions = None + boardManager = None """ Colors @@ -91,11 +158,18 @@ class Data: zPopupUnits = None zStepSizeVal = 0.1 + """ Queues """ message_queue = LoggingQueue(logger) - ui_queue = queue.Queue() + ui_controller_queue = queue.Queue() + ui_queue1 = UIQueue() + alog_streamer_queue = queue.Queue(1000) # used for sending log to client screen.. limit to 1000 "items" + log_streamer_queue = queue.Queue(1000) # used for sending log to client screen.. limit to 1000 "items" + console_queue = queue.Queue() # used for printing to terminal + mcp_queue = queue.Queue () # used for sending messages to WebMCP(if enabled) + webMCPActive = False # start false until WebMCP connects gcode_queue = queue.Queue() quick_queue = queue.Queue() @@ -105,11 +179,57 @@ class Data: xval = 0.0 yval = 0.0 zval = 0.0 + xval_prev = -99990.0 + yval_prev = -99990.0 + zval_prev = -99990.0 + + leftError = 0.0 + rightError = 0.0 + leftError_prev = -99999.0 + rightError_prev = -99999.9 + + """ + Chain lengths as reported by controller + """ + leftChain = 1610 + rightChain = 1610 + + """ + Sled position computed from controller reported chain lengths + """ + computedX = 0 + computedY = 0 + + """ + Buffer size as reported by controller + """ + bufferSize = 127 + pausedzval = 0.0 + pausedPositioningMode = 0 + pausedUnits = "INCHES" + + """ + GCode Position Values + """ previousPosX = 0.0 previousPosY = 0.0 previousPosZ = 0.0 + """ + Board data + """ + currentBoard = None + + + shutdown = False + + hostAddress = "-" + platform = "RPI" + platformHome = "" + + + def __init__(self): """ diff --git a/DataStructures/logger.py b/DataStructures/logger.py index ef3fc43f..32e7445f 100644 --- a/DataStructures/logger.py +++ b/DataStructures/logger.py @@ -7,24 +7,76 @@ from DataStructures.makesmithInitFuncs import MakesmithInitFuncs import threading +import os +import time +import datetime +from pathlib import Path from app import app, socketio + class Logger(MakesmithInitFuncs): errorValues = [] recordingPositionalErrors = False clients = [] + logStartTime = 0 messageBuffer = "" amessageBuffer = "" - - # clear the old log file - with open("log.txt", "a") as logFile: - logFile.truncate() - - with open("alog.txt", "a") as logFile: - logFile.truncate() + idler = 0 + suspendLogging = False + lastALogWrite = 0 + lastLogWrite = 0 + loggingTimeout = -1 + + def __init__(self): + print("Initializing Logger") + self.home = str(Path.home()) + # clear the old log file + if not os.path.isdir(self.home + "/.WebControl"): + print("creating " + self.home + "/.WebControl directory") + os.mkdir(self.home + "/.WebControl") + print(self.home+"/.WebControl/"+"log.txt") + with open(self.home+"/.WebControl/"+"log.txt", "a") as logFile: + logFile.truncate() + + with open(self.home+"/.WebControl/"+"alog.txt", "a") as logFile: + logFile.truncate() + + self.logStartTime = time.time() + dateTime = datetime.datetime.fromtimestamp(self.logStartTime).strftime('%Y-%m-%d %H:%M:%S') + self.amessageBuffer = "+++++\n"+dateTime+"\n+++++\n".format(self.logStartTime) + self.messageBuffer = "+++++\n"+dateTime+"\n+++++\n".format(self.logStartTime) + self.idler = time.time() + self.lastALogWrite = self.idler + self.lastLogWrite = self.idler + + def resetIdler(self): + self.idler = time.time() + + def getLoggerState(self): + ''' + Returns the state of the logger + :return: + ''' + return self.suspendLogging + + def setLoggingTimeout(self, timeOut): + self.loggingTimeout = timeOut + + def addToMessageBuffer(self, message): + self.messageBuffer += message + if self.data.log_streamer_queue.full(): + self.data.log_streamer_queue.get_nowait() + self.data.log_streamer_queue.put_nowait(message) + + def addToaMessageBuffer(self, message): + self.amessageBuffer += message + + if self.data.alog_streamer_queue.full(): + self.data.alog_streamer_queue.get_nowait() + self.data.alog_streamer_queue.put_nowait(message) def writeToLog(self, message): @@ -36,47 +88,96 @@ def writeToLog(self, message): way slow """ + if self.loggingTimeout == -1: #get setting for suspend time + self.loggingTimeout = self.data.config.get("WebControl Settings", "loggingTimeout") + currentTime = time.time() + logTime = "{:0.2f}".format(currentTime-self.logStartTime) + dateTime = datetime.datetime.fromtimestamp(currentTime).strftime('%Y-%m-%d %H:%M:%S') + if self.loggingTimeout > 0 and self.data.uploadFlag <= 0 and currentTime-self.idler > self.loggingTimeout: + if not self.suspendLogging: + self.addToMessageBuffer(logTime + ": " + "Logging suspended due to user idle time > "+str(self.loggingTimeout)+" seconds\n") + self.suspendLogging = True + else: + if self.suspendLogging: + self.addToMessageBuffer(logTime + ": " + "Logging resumed due to user activity\n") + self.suspendLogging = False + if message[0] != "<" and message[0] != "[": try: - self.amessageBuffer = self.amessageBuffer + message - self.messageBuffer = self.messageBuffer + message + tmessage = message.rstrip('\r\n') + self.addToaMessageBuffer(logTime+": " + tmessage+"\n") + if not self.suspendLogging: + self.addToMessageBuffer(logTime+": " + tmessage+"\n") except: pass else: try: - self.messageBuffer = self.messageBuffer + message + if not self.suspendLogging: + tmessage = message.rstrip('\r\n') + self.addToMessageBuffer(logTime+": "+ tmessage+"\n") except: pass - if len(self.messageBuffer) > 0: - t = threading.Thread( - target=self.writeToFile, args=(self.messageBuffer, True, "write") - ) - t.daemon = True - t.start() - self.messageBuffer = "" - - if len(self.amessageBuffer) > 0: - t = threading.Thread( - target=self.writeToFile, args=(self.amessageBuffer, False, "write") - ) - t.daemon = True - t.start() - self.amessageBuffer = "" + if len(self.messageBuffer) > 500 or currentTime-self.lastLogWrite > 15: + if self.messageBuffer != "": #doing it this way will flush out the buffer when in idle state. + t = threading.Thread( + target=self.writeToFile, args=(self.messageBuffer, True, "write") + ) + t.daemon = True + t.start() + self.messageBuffer = "" + self.lastLogWrite = currentTime + + if len(self.amessageBuffer) > 500 or currentTime-self.lastALogWrite > 15: + if self.amessageBuffer != "": + t = threading.Thread( + target=self.writeToFile, args=(self.amessageBuffer, False, "write") + ) + t.daemon = True + t.start() + self.amessageBuffer = "" + self.lastALogWrite = currentTime def writeToFile(self, toWrite, log, *args): """ Write to the log file """ if log is True: - with open("log.txt", "a") as logFile: + with open(self.home+"/.WebControl/"+"log.txt", "a") as logFile: logFile.write(toWrite) else: - with open("alog.txt", "a") as logFile: + with open(self.home+"/.WebControl/"+"alog.txt", "a") as logFile: logFile.write(toWrite) return + def deleteLogFiles(self): + """ + Delete log files + """ + success1 = False + for x in range(0, 1000): + try: + os.remove(self.home+"/.WebControl/"+"log.txt") + success1 = True + break + except: + success1 = False + + success2 = False + for x in range(0, 1000): + try: + os.remove(self.home+"/.WebControl/"+"alog.txt") + success2 = True + break + except: + success2 = False + + if success1 and success2: + return True + else: + return False + def writeErrorValueToLog(self, error): """ @@ -109,7 +210,7 @@ def endRecordingAvgError(self): Stops recording error values. """ - print("stopping to record") + self.data.console_queue.put("stopping to record") self.recordingPositionalErrors = False def reportAvgError(self): @@ -120,7 +221,7 @@ def reportAvgError(self): """ avg = sum(self.errorValues) / len(self.errorValues) - self.data.ui_queue.put( + self.data.ui_controller_queue.put( "Message: The average feedback system error was: " + "%.2f" % avg + "mm" ) diff --git a/DataStructures/uiQueue.py b/DataStructures/uiQueue.py new file mode 100644 index 00000000..b095699d --- /dev/null +++ b/DataStructures/uiQueue.py @@ -0,0 +1,43 @@ +""" + +This module provides a simple addition to the Queue, which is that it logs +puts to the Queue immediately. + +""" + +from queue import Queue +import json + + +class UIQueue(Queue): + def __init__(self): + super(UIQueue, self).__init__() + + def put(self, command, message, dictionary): + ''' + This is an attempt to standardize on the communications format. There's probably a fair amount of work that + should go into updating this. The concept is that there are three elements: + :param command: The type/command of the message + :param message: The actual message + :param dictionary: Values/Arguments that accompany the message + :return: + Some times things don't come across correctly, particularly text messages from the controller. need to work + on this and document it better. + All three of the elements get converted into a single json string for insertion into the queue + ''' + #if command == "TextMessage" or command == "Alert": + # data = dictionary # these are text lines, so no need to jsonify + #else: + try: + # convert the dictionary to a json + data = json.dumps(dictionary) + except Exception as e: + print("####") + print(e) + print("####") + + # convert the command, message, and jsonified dictionary into a json + msg = json.dumps({"command": command, "message": message, "data": data}) + + # put in uiqueue + return super(UIQueue, self).put(msg) diff --git a/Dockerfile b/Dockerfile index 20ebcdb3..3f88cc62 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,8 +1,7 @@ -FROM python:3.5.6-slim-stretch +FROM arm32v7/python:3.5.6-slim-stretch as builder # Install dependencies needed for building and running OpenCV -RUN apt-get update -RUN apt-get install -y --no-install-recommends \ +RUN apt-get update && apt-get install -y --no-install-recommends \ # to build and install unzip wget sed \ build-essential cmake pkg-config \ @@ -17,12 +16,15 @@ RUN apt-get install -y --no-install-recommends \ # libtbb2 libtbb-dev \ # for gevent libevent-dev \ + # for numpy (installs libf77blas.so.3) + libatlas-base-dev \ # cleanup && rm -rf /var/lib/apt/lists/* \ && apt-get -y autoremove # OpenCV dependency -RUN pip install numpy +RUN pip install numpy==1.16.2 +RUN pip install scipy==1.3.1 # Build OpenCV ADD tools/download_build_install_opencv.sh /download_build_install_opencv.sh @@ -30,15 +32,93 @@ RUN chmod +x /download_build_install_opencv.sh && /download_build_install_opencv # Get other python dependencies ADD requirements.txt /requirements.txt -# Remove opencv from requirements (use the compiled one) -RUN sed -i '/opencv-python.*/d' /requirements.txt -RUN pip install -r /requirements.txt +# Remove opencv, scipy and numpy from requirements (since they're already installed) +RUN sed -i '/opencv-python.*/d' /requirements.txt && sed -i '/scipy.*/d' /requirements.txt && sed -i '/numpy.*/d' /requirements.txt +# TODO: Maybe we can cache wheel files outside this container, for more granular reuse when requiremnts.txt changes +RUN pwd +RUN apt-get update && apt-get install -y --no-install-recommends python-dev libffi-dev +RUN pip install -r /requirements.txt + +# Download and compile the Arduino firmware +# Generates the firmware as /firmware/.pioenvs/megaatmega2560/firmware.hex +# PlatformIO doesn't support python3 yet, so also install python2 :/ +# See also: https://github.com/platformio/platformio-core/issues/895 +RUN apt-get update \ + && apt-get install -y --no-install-recommends python2.7 python-pip python-setuptools python-wheel git \ + && pip2 install -U platformio \ + && pio platform install --with-package framework-arduino atmelavr \ + && pio lib -g install "Servo" + + +ARG madgrizzle_firmware_repo=https://github.com/madgrizzle/Firmware.git +ARG madgrizzle_firmware_sha=bf4350ffd9bc154832505fc0125abd2c4c04dba7 +#ARG madgrizzle_firmware_sha=95f7d4b5c431dec162d2e2eec7c6e42530298c4b +RUN git clone $madgrizzle_firmware_repo firmware/madgrizzle \ + && cd firmware/madgrizzle \ + && git checkout $madgrizzle_firmware_sha \ + && pio run -e megaatmega2560 \ + && mkdir build \ + && mv .pio/build/megaatmega2560/firmware.hex build/$madgrizzle_firmware_sha-$(sed -n -e 's/^.*VERSIONNUMBER //p' cnc_ctrl_v1/Maslow.h).hex +ARG maslowcnc_firmware_repo=https://github.com/MaslowCNC/Firmware.git +ARG maslowcnc_firmware_sha=e1e0d020fff1f4f7c6b403a26a85a16546b7e15b +RUN git clone $maslowcnc_firmware_repo firmware/maslowcnc \ + && cd firmware/maslowcnc \ + && git checkout $maslowcnc_firmware_sha \ + && pio run -e megaatmega2560 \ + && mkdir build \ + && mv .pio/build/megaatmega2560/firmware.hex build/$maslowcnc_firmware_sha-$(sed -n -e 's/^.*VERSIONNUMBER //p' cnc_ctrl_v1/Maslow.h).hex +RUN pwd +ARG holey_firmware_repo=https://github.com/madgrizzle/Firmware.git +ARG holey_firmware_sha=950fb23396171cbd456c2d4149455cc45f5e6bc3 +RUN git clone $holey_firmware_repo firmware/holey \ + && cd firmware/holey \ + && git checkout $holey_firmware_sha \ + && pio run -e megaatmega2560 \ + && mkdir build \ + && mv .pio/build/megaatmega2560/firmware.hex build/$holey_firmware_sha-$(sed -n -e 's/^.*VERSIONNUMBER //p' cnc_ctrl_v1/Maslow.h).hex -# Remove no-longer-needed gevent compilation libraries -RUN apt-get purge -y --auto-remove --allow-remove-essential gcc libevent-dev wget unzip sed -# Copy the source ADD . /WebControl +# Clean up the /WebControl dir a bit to slim it down +# TODO: Is there a more automatic way to do this? +RUN rm -rf /WebControl/.venv && rm -rf /WebControl/.git +# Pre-compile the pyc files (for faster Docker image startup) +RUN python --version && python -m compileall /WebControl + + +FROM arm32v7/python:3.5.6-slim-stretch + +# Pip wheels compiled in the builder +COPY --from=builder /root/.cache /root/.cache +# requirements.txt with opencv, scipy and numpy removed +COPY --from=builder /requirements.txt /requirements.txt +# Required shared libraries +COPY --from=builder /usr/local/lib/python3.5/site-packages/cv2.cpython-35m-arm-linux-gnueabihf.so /usr/local/lib/python3.5/site-packages/cv2.cpython-35m-arm-linux-gnueabihf.so +COPY --from=builder /usr/lib/libf77blas.so /usr/lib/libf77blas.so +COPY --from=builder /usr/lib/libf77blas.so.3 /usr/lib/libf77blas.so.3 +COPY --from=builder /usr/lib/libcblas.so.3 /usr/lib/libcblas.so.3 +COPY --from=builder /usr/lib/libatlas.so.3 /usr/lib/libatlas.so.3 +COPY --from=builder /usr/lib/libblas.so.3 /usr/lib/libblas.so.3 +COPY --from=builder /usr/lib/arm-linux-gnueabihf/libgfortran.so.3 /usr/lib/arm-linux-gnueabihf/libgfortran.so.3 +COPY --from=builder /usr/lib/liblapack.so.3 /usr/lib/liblapack.so.3 + +RUN pip install numpy==1.16.2 && pip install scipy==1.3.1 && pip install -r /requirements.txt && rm -rf /root/.cache + + +# Install avrdude +# TODO: to speed up incremental docker builds, we can probably do this in the builder image if we can figure out +# which files we need to copy over +RUN apt-get update && apt-get install -y --no-install-recommends \ + avrdude \ + && rm -rf /var/lib/apt/lists/* \ + && apt-get -y autoremove +# Copy in the pre-compiled firmware +COPY --from=builder /firmware/madgrizzle/build/* /firmware/madgrizzle/ +COPY --from=builder /firmware/holey/build/* /firmware/holey/ +COPY --from=builder /firmware/maslowcnc/build/* /firmware/maslowcnc/ + +# Copy the pre-compiled source from the builder +COPY --from=builder /WebControl /WebControl WORKDIR /WebControl EXPOSE 5000/tcp diff --git a/File/gcodeFile.py b/File/gcodeFile.py index a5a8376d..a134945a 100644 --- a/File/gcodeFile.py +++ b/File/gcodeFile.py @@ -5,6 +5,8 @@ import json import re import math +import gzip +import io class Line: @@ -13,17 +15,20 @@ def __init__(self): self.color = None self.dashed = False self.type = None + self.radius = None + self.command = None class GCodeFile(MakesmithInitFuncs): isChanged = False canvasScaleFactor = 1 # scale from mm to pixels INCHES = 1.0 - MILLIMETERS = 1.0 / 25.4 + MILLIMETERS = 1.0/25.4 xPosition = 0 yPosition = 0 zPosition = 0 + truncate = -1 #do this to truncate after shift of home position lineNumber = 0 # the line number currently being processed @@ -34,32 +39,100 @@ class GCodeFile(MakesmithInitFuncs): maxNumberOfLinesToRead = 300000 filename = "" - line = [] + line3D = [] + ''' + prependString is defined here so it can be persistent across each 'moveOneLine'. That way, if a gcode line + does not contain a valid gcode, it uses the previous line's gcode. Thanks to @amantalion for discovering the + glitch. https://github.com/madgrizzle/WebControl/issues/78 + ''' + prependString = "" - def serializeGCode(self): - sendStr = json.dumps([ob.__dict__ for ob in self.data.gcodeFile.line]) + + def serializeGCode3D(self): + sendStr = json.dumps([ob.__dict__ for ob in self.data.gcodeFile.line3D]) return sendStr - def loadUpdateFile(self): - filename = self.data.gcodeFile.filename - # print(filename) - del self.line[:] - if filename is "": # Blank the g-code if we're loading "nothing" - self.data.gcode = "" + def saveFile(self, fileName, directory): + if fileName is "": # Blank the g-code if we're loading "nothing" + return False + if directory is "": return False - try: - filterfile = open(filename, "r") + homeX = float(self.data.config.getValue("Advanced Settings", "homeX")) + homeY = float(self.data.config.getValue("Advanced Settings", "homeY")) + fileToWrite = directory + "/" + fileName + gfile = open(fileToWrite, "w+") + print(fileToWrite) + for line in self.data.gcode: + gfile.write(line+'\n') + #gfile = open(directory+fileName, "w+") + #gfile.writelines(map(lambda s: s+ '\n', self.data.gcode)) + print("Closing File") + gfile.close() + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.console_queue.put("Gcode File Error") + self.data.ui_queue1.put("Alert", "Alert", "Cannot save gcode file.") + self.data.gcodeFile.filename = "" + return False + return True + + def loadUpdateFile(self, gcode=""): + print("At loadUpdateFile") + gcodeLoad = False + if gcode=="": + gcodeLoad = True + if self.data.units == "MM": + self.canvasScaleFactor = self.MILLIMETERS + print("units in mm") + else: + self.canvasScaleFactor = self.INCHES + print("units inches") + self.data.clidisplay = self.data.config.getValue("Maslow Settings", "clidisplay") + print ("clidisplay is ", self.data.clidisplay) + if (self.data.clidisplay): + self.data.gcode_x_min = 2000 + self.data.gcode_x_max = -2000 + self.data.gcode_y_min = 1000 + self.data.gcode_y_max = -1000 + self.data.gcode_z_min = 20 + self.data.gcode_z_max = -20 + print("reset gcode coordinates") + if gcode == "": + filename = self.data.gcodeFile.filename + self.data.gcodeShift[0] = round(float(self.data.config.getValue("Advanced Settings", "homeX")),4) + self.data.gcodeShift[1] = round(float(self.data.config.getValue("Advanced Settings", "homeY")),4) + del self.line3D[:] + if filename is "": # Blank the g-code if we're loading "nothing" + self.data.gcode = "" + return False + + try: + filterfile = open(filename, "r") + except Exception as e: + self.data.console_queue.put(str(e)) + self.data.console_queue.put("Gcode File Error") + self.data.ui_queue1.put("Alert", "Alert", "Cannot open gcode file.") + self.data.gcodeFile.filename = "" + return False rawfilters = filterfile.read() - filtersparsed = re.sub( - r"\(([^)]*)\)", "\n", rawfilters - ) # replace mach3 style gcode comments with newline + filterfile.close() # closes the filter save file + else: + del self.line3D[:] + rawfilters = gcode + + try: + if True: #this is just for testing R removal + filtersparsed = rawfilters #get rid of this if above is uncommented) + ''' filtersparsed = re.sub( r";([^\n]*)\n", "\n", filtersparsed ) # replace standard ; initiated gcode comments with newline + ''' + #print(filtersparsed) filtersparsed = re.sub(r"\n\n", "\n", filtersparsed) # removes blank lines filtersparsed = re.sub( - r"([0-9])([GXYZIJFTM]) *", "\\1 \\2", filtersparsed + r"([0-9])([GXYZIJFTMR]) *", "\\1 \\2", filtersparsed ) # put spaces between gcodes filtersparsed = re.sub(r" +", " ", filtersparsed) # condense space runs value = self.data.config.getValue("Advanced Settings", "truncate") @@ -69,57 +142,157 @@ def loadUpdateFile(self): filtersparsed = re.sub( r"([+-]?\d*\.\d{1," + digits + "})(\d*)", r"\g<1>", filtersparsed ) # truncates all long floats to 4 decimal places, leaves shorter floats - filtersparsed = re.split( - "\n", filtersparsed - ) # splits the gcode into elements to be added to the list - filtersparsed = [ - x + " " for x in filtersparsed - ] # adds a space to the end of each line - filtersparsed = [x.lstrip() for x in filtersparsed] + self.truncate = int(digits) + else: + self.truncate = -1 + filtersparsed = re.split("\n", filtersparsed) # splits the gcode into elements to be added to the list + filtersparsed = [x.lstrip() for x in filtersparsed] # remove leading spaces + filtersparsed = [x + " " for x in filtersparsed] # adds a space to the end of each line filtersparsed = [x.replace("X ", "X") for x in filtersparsed] filtersparsed = [x.replace("Y ", "Y") for x in filtersparsed] filtersparsed = [x.replace("Z ", "Z") for x in filtersparsed] filtersparsed = [x.replace("I ", "I") for x in filtersparsed] filtersparsed = [x.replace("J ", "J") for x in filtersparsed] filtersparsed = [x.replace("F ", "F") for x in filtersparsed] + filtersparsed = [x.replace("R ", "R") for x in filtersparsed] + #print (filtersparsed) + #((\%)|((?P