From 69835a97952a801aa4836d66f2ac773018354eeb Mon Sep 17 00:00:00 2001 From: David Lang Date: Fri, 15 Nov 2024 01:58:47 -0800 Subject: [PATCH 01/13] make allowed frame even larger see [Projects] Modifying Maslow 4 to a cut Area of 28'x6' to cut synthetic fabric for a use case. --- FluidNC/src/Maslow/Maslow.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 34c587d76..28a1dd716 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -226,7 +226,7 @@ class Maslow_ { //calibration stuff int frame_dimention_MIN = 400; - int frame_dimention_MAX = 5000; + int frame_dimention_MAX = 15000; double calibrationGrid[CALIBRATION_GRID_SIZE_MAX][2] = { 0 }; float calibration_grid_width_mm_X = 2000; // mm offset from the edge of the frame From 73d47bd86b5e3b20fefb2e742fc94e6dbd1f3c2d Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 20:08:47 -0800 Subject: [PATCH 02/13] introduce arm indexes instead of using magic numbers (0-3) to reference the arms, create constants tl/tr/bl/br convert Maslow.cpp to use these constants instead of numbers this first commit adds a minor amount of clarity, but it's real value will show up in later commits in the series --- FluidNC/src/Maslow/Maslow.cpp | 54 +++++++++++++++++------------------ FluidNC/src/Maslow/Maslow.h | 6 ++++ 2 files changed, 33 insertions(+), 27 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 9f5d6d01b..6a2df0809 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -279,28 +279,28 @@ void Maslow_::home() { if (retractingTL) { if (axisTL.retract()) { retractingTL = false; - axis_homed[0] = true; + axis_homed[tl] = true; extendedTL = false; } } if (retractingTR) { if (axisTR.retract()) { retractingTR = false; - axis_homed[1] = true; + axis_homed[tr] = true; extendedTR = false; } } if (retractingBL) { if (axisBL.retract()) { retractingBL = false; - axis_homed[2] = true; + axis_homed[bl] = true; extendedBL = false; } } if (retractingBR) { if (axisBR.retract()) { retractingBR = false; - axis_homed[3] = true; + axis_homed[br] = true; extendedBR = false; } } @@ -401,10 +401,10 @@ bool Maslow_::takeSlackFunc() { double offset = _beltEndExtension + _armLength; double threshold = 12; - float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); - float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); - float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); - float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); + float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); + float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); + float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); + float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?"); @@ -795,10 +795,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { //once both belts are pulled, take a measurement if (BR_tight && BL_tight) { //take measurement and record it to the calibration data array - calibration_data[0][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); - calibration_data[1][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); - calibration_data[2][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); - calibration_data[3][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); + calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); + calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); + calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); + calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); BR_tight = false; BL_tight = false; return true; @@ -888,10 +888,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { } if (pull1_tight && pull2_tight) { //take measurement and record it to the calibration data array - calibration_data[0][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); - calibration_data[1][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); - calibration_data[2][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); - calibration_data[3][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); + calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); + calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); + calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); + calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); pull1_tight = false; pull2_tight = false; return true; @@ -915,10 +915,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { return false; //discard the first three measurements } - measurements[0][run - 2] = calibration_data[0][waypoint]; //-3 cuz discarding the first 3 measurements - measurements[1][run - 2] = calibration_data[1][waypoint]; - measurements[2][run - 2] = calibration_data[2][waypoint]; - measurements[3][run - 2] = calibration_data[3][waypoint]; + measurements[tl][run - 2] = calibration_data[tl][waypoint]; //-3 cuz discarding the first 3 measurements + measurements[tr][run - 2] = calibration_data[tr][waypoint]; + measurements[bl][run - 2] = calibration_data[bl][waypoint]; + measurements[br][run - 2] = calibration_data[br][waypoint]; run++; @@ -977,10 +977,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { if(waypoint == 0){ double threshold = 100; - float diffTL = calibration_data[0][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); - float diffTR = calibration_data[1][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); - float diffBL = calibration_data[2][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); - float diffBR = calibration_data[3][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); + float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); + float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); + float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); + float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { @@ -1711,7 +1711,7 @@ void Maslow_::reset_all_axis() { // True if all axis were zeroed bool Maslow_::all_axis_homed() { - return axis_homed[0] && axis_homed[1] && axis_homed[2] && axis_homed[3]; + return axis_homed[tl] && axis_homed[tr] && axis_homed[bl] && axis_homed[br]; } // True if all axis were extended @@ -1759,8 +1759,8 @@ void Maslow_::checkCalibrationData() { void Maslow_::print_calibration_data() { String data = "CLBM:["; for (int i = 0; i < waypoint; i++) { - data += "{bl:" + String(calibration_data[2][i]) + ", br:" + String(calibration_data[3][i]) + - ", tr:" + String(calibration_data[1][i]) + ", tl:" + String(calibration_data[0][i]) + "},"; + data += "{bl:" + String(calibration_data[bl][i]) + ", br:" + String(calibration_data[br][i]) + + ", tr:" + String(calibration_data[tr][i]) + ", tl:" + String(calibration_data[tl][i]) + "},"; } data += "]"; HeartBeatEnabled = false; diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 28a1dd716..589107f20 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -268,6 +268,12 @@ class Maslow_ { bool holding = false; unsigned long holdTime = 0; + //variables to use as indexes into various arrays + const char tl = 0; + const char tr = 1; + const char bl = 2; + const char br = 3; + float tlX; float tlY; float tlZ; From 658d5c7e5932a8edefc47fcdd0ef608c98ba2ce9 Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 20:17:10 -0800 Subject: [PATCH 03/13] convert from XXZ variables to zOffset[] array --- FluidNC/src/Machine/MachineConfig.cpp | 8 +++--- FluidNC/src/Maslow/Maslow.cpp | 40 +++++++++++++-------------- FluidNC/src/Maslow/Maslow.h | 7 ++--- 3 files changed, 27 insertions(+), 28 deletions(-) diff --git a/FluidNC/src/Machine/MachineConfig.cpp b/FluidNC/src/Machine/MachineConfig.cpp index a36bdb7d4..779dc3c84 100644 --- a/FluidNC/src/Machine/MachineConfig.cpp +++ b/FluidNC/src/Machine/MachineConfig.cpp @@ -111,10 +111,10 @@ namespace Machine { handler.item(M+"_brX", Maslow.brX); handler.item(M+"_brY", Maslow.brY); - handler.item(M+"_tlZ", Maslow.tlZ); - handler.item(M+"_trZ", Maslow.trZ); - handler.item(M+"_blZ", Maslow.blZ); - handler.item(M+"_brZ", Maslow.brZ); + handler.item(M+"_tlZ", Maslow.zOffset[Maslow.tl]); + handler.item(M+"_trZ", Maslow.zOffset[Maslow.tr]); + handler.item(M+"_blZ", Maslow.zOffset[Maslow.bl]); + handler.item(M+"_brZ", Maslow.zOffset[Maslow.br]); handler.item(M+"_Retract_Current_Threshold", Maslow.retractCurrentThreshold, 0, 3500); handler.item(M+"_Calibration_Current_Threshold", Maslow.calibrationCurrentThreshold, 0, 3500); diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 6a2df0809..844d24f92 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -401,10 +401,10 @@ bool Maslow_::takeSlackFunc() { double offset = _beltEndExtension + _armLength; double threshold = 12; - float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); - float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); - float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); - float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); + float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); + float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); + float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); + float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?"); @@ -674,7 +674,7 @@ float Maslow_::computeBL(float x, float y, float z) { y = y + centerY; float a = blX - x; //X dist from corner to router center float b = blY - y; //Y dist from corner to router center - float c = 0.0 - (z + blZ); //Z dist from corner to router center + float c = 0.0 - (z + zOffset[bl]); //Z dist from corner to router center float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -690,7 +690,7 @@ float Maslow_::computeBR(float x, float y, float z) { y = y + centerY; float a = brX - x; float b = brY - y; - float c = 0.0 - (z + brZ); + float c = 0.0 - (z + zOffset[br]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -706,7 +706,7 @@ float Maslow_::computeTR(float x, float y, float z) { y = y + centerY; float a = trX - x; float b = trY - y; - float c = 0.0 - (z + trZ); + float c = 0.0 - (z + zOffset[tr]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -722,7 +722,7 @@ float Maslow_::computeTL(float x, float y, float z) { y = y + centerY; float a = tlX - x; float b = tlY - y; - float c = 0.0 - (z + tlZ); + float c = 0.0 - (z + zOffset[tl]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -795,10 +795,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { //once both belts are pulled, take a measurement if (BR_tight && BL_tight) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); - calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); - calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); - calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); + calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), zOffset[tl]); + calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), zOffset[tr]); + calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), zOffset[bl]); + calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), zOffset[br]); BR_tight = false; BL_tight = false; return true; @@ -888,10 +888,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { } if (pull1_tight && pull2_tight) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), tlZ); - calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), trZ); - calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), blZ); - calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), brZ); + calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), zOffset[tl]); + calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), zOffset[tr]); + calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), zOffset[bl]); + calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), zOffset[br]); pull1_tight = false; pull2_tight = false; return true; @@ -977,10 +977,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { if(waypoint == 0){ double threshold = 100; - float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tlZ); - float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), trZ); - float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), blZ); - float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), brZ); + float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); + float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); + float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); + float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 589107f20..0c78da4ce 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -274,18 +274,17 @@ class Maslow_ { const char bl = 2; const char br = 3; + // per-arm arrays + float zOffset[4]; + float tlX; float tlY; - float tlZ; float trX; float trY; - float trZ; float blX; float blY; - float blZ; float brX; float brY; - float brZ; float _beltEndExtension = 30; //Based on the CAD model these should add to 153.4 float _armLength = 123.4; From ca522bb7fe3a28754e142a30d7dee69376e67ea9 Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 20:56:47 -0800 Subject: [PATCH 04/13] migrate anchor defintions to arrays --- FluidNC/src/Machine/MachineConfig.cpp | 16 +++++++------- FluidNC/src/Maslow/Maslow.cpp | 32 +++++++++++++-------------- FluidNC/src/Maslow/Maslow.h | 10 +-------- 3 files changed, 25 insertions(+), 33 deletions(-) diff --git a/FluidNC/src/Machine/MachineConfig.cpp b/FluidNC/src/Machine/MachineConfig.cpp index 779dc3c84..829a3b00f 100644 --- a/FluidNC/src/Machine/MachineConfig.cpp +++ b/FluidNC/src/Machine/MachineConfig.cpp @@ -99,17 +99,17 @@ namespace Machine { handler.item(M+"_calibration_grid_height_mm_Y", Maslow.calibration_grid_height_mm_Y, 100, 3000); handler.item(M+"_calibration_grid_size", Maslow.calibrationGridSize, 3, 9); - handler.item(M+"_tlX", Maslow.tlX); - handler.item(M+"_tlY", Maslow.tlY); + handler.item(M+"_tlX", Maslow.anchor[Maslow.tl][0]); + handler.item(M+"_tlY", Maslow.anchor[Maslow.tl][1]); - handler.item(M+"_trX", Maslow.trX); - handler.item(M+"_trY", Maslow.trY); + handler.item(M+"_trX", Maslow.anchor[Maslow.tr][0]); + handler.item(M+"_trY", Maslow.anchor[Maslow.tr][1]); - handler.item(M+"_blX", Maslow.blX); - handler.item(M+"_blY", Maslow.blY); + handler.item(M+"_blX", Maslow.anchor[Maslow.bl][0]); + handler.item(M+"_blY", Maslow.anchor[Maslow.bl][1]); - handler.item(M+"_brX", Maslow.brX); - handler.item(M+"_brY", Maslow.brY); + handler.item(M+"_brX", Maslow.anchor[Maslow.br][0]); + handler.item(M+"_brY", Maslow.anchor[Maslow.br][1]); handler.item(M+"_tlZ", Maslow.zOffset[Maslow.tl]); handler.item(M+"_trZ", Maslow.zOffset[Maslow.tr]); diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 844d24f92..4e70ee4f2 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -672,8 +672,8 @@ float Maslow_::computeBL(float x, float y, float z) { //Move from lower left corner coordinates to centered coordinates x = x + centerX; y = y + centerY; - float a = blX - x; //X dist from corner to router center - float b = blY - y; //Y dist from corner to router center + float a = anchor[bl][0] - x; //X dist from corner to router center + float b = anchor[bl][1] - y; //Y dist from corner to router center float c = 0.0 - (z + zOffset[bl]); //Z dist from corner to router center float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -688,8 +688,8 @@ float Maslow_::computeBR(float x, float y, float z) { //Move from lower left corner coordinates to centered coordinates x = x + centerX; y = y + centerY; - float a = brX - x; - float b = brY - y; + float a = anchor[br][0] - x; + float b = anchor[br][1] - y; float c = 0.0 - (z + zOffset[br]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -704,8 +704,8 @@ float Maslow_::computeTR(float x, float y, float z) { //Move from lower left corner coordinates to centered coordinates x = x + centerX; y = y + centerY; - float a = trX - x; - float b = trY - y; + float a = anchor[tr][0] - x; + float b = anchor[tr][1] - y; float c = 0.0 - (z + zOffset[tr]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -720,8 +720,8 @@ float Maslow_::computeTL(float x, float y, float z) { //Move from lower left corner coordinates to centered coordinates x = x + centerX; y = y + centerY; - float a = tlX - x; - float b = tlY - y; + float a = anchor[tl][0] - x; + float b = anchor[tl][1] - y; float c = 0.0 - (z + zOffset[tl]); float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -1658,13 +1658,13 @@ void Maslow_::setZStop() { void Maslow_::set_frame_width(double width) { - trX = width; - brX = width; + anchor[tr][0] = width; + anchor[br][0] = width; updateCenterXY(); } void Maslow_::set_frame_height(double height) { - tlY = height; - trY = height; + anchor[tl][1] = height; + anchor[tr][1] = height; updateCenterXY(); } void Maslow_::take_slack() { @@ -1843,10 +1843,10 @@ double Maslow_::getTargetZ() { //Updates where the center x and y positions are void Maslow_::updateCenterXY() { - double A = (trY - blY) / (trX - blX); - double B = (brY - tlY) / (brX - tlX); - centerX = (brY - (B * brX) + (A * trX) - trY) / (A - B); - centerY = A * (centerX - trX) + trY; + double A = (anchor[tr][1] - anchor[bl][1]) / (anchor[tr][0] - anchor[bl][0]); + double B = (anchor[br][1] - anchor[tl][1]) / (anchor[br][0] - anchor[tl][0]); + centerX = (anchor[br][1] - (B * anchor[br][0]) + (A * anchor[tr][0]) - anchor[tr][1]) / (A - B); + centerY = A * (centerX - anchor[tr][0]) + anchor[tr][1]; } // Prints out state diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 0c78da4ce..c9a4305c7 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -276,15 +276,7 @@ class Maslow_ { // per-arm arrays float zOffset[4]; - - float tlX; - float tlY; - float trX; - float trY; - float blX; - float blY; - float brX; - float brY; + float anchor[4][2]; float _beltEndExtension = 30; //Based on the CAD model these should add to 153.4 float _armLength = 123.4; From 670d07019bd29905f9c71838cecf52f0ee90f96f Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 21:45:56 -0800 Subject: [PATCH 05/13] re-write many variables to arrays change many variables from separate per-axis variables to an array indexed by the arm constants This is a large diff, but it is separate so that it can easily be reviewed and validated as 'obvioulsy' correct (or not) --- FluidNC/src/Maslow/Maslow.cpp | 209 +++++++++++++++++----------------- FluidNC/src/Maslow/Maslow.h | 41 ++----- 2 files changed, 113 insertions(+), 137 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 4e70ee4f2..0716e5ec4 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -192,7 +192,7 @@ void Maslow_::update() { digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on } //If we are doing calibration turn the cooling fan on - else if (calibrationInProgress || extendingALL || retractingTL || retractingTR || retractingBL || retractingBR) { + else if (calibrationInProgress || extendingALL || retracting[tl] || retracting[tr] || retracting[bl] || retracting[br]) { digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on } else { digitalWrite(coolingFanPin, LOW); //Turn the cooling fan off @@ -276,32 +276,32 @@ void Maslow_::heartBeat(){ // -Maslow homing loop void Maslow_::home() { //run all the retract functions untill we hit the current limit - if (retractingTL) { + if (retracting[tl]) { if (axisTL.retract()) { - retractingTL = false; + retracting[tl] = false; axis_homed[tl] = true; - extendedTL = false; + extended[tl] = false; } } - if (retractingTR) { + if (retracting[tr]) { if (axisTR.retract()) { - retractingTR = false; + retracting[tr] = false; axis_homed[tr] = true; - extendedTR = false; + extended[tr] = false; } } - if (retractingBL) { + if (retracting[bl]) { if (axisBL.retract()) { - retractingBL = false; + retracting[bl] = false; axis_homed[bl] = true; - extendedBL = false; + extended[bl] = false; } } - if (retractingBR) { + if (retracting[br]) { if (axisBR.retract()) { - retractingBR = false; + retracting[br] = false; axis_homed[br] = true; - extendedBR = false; + extended[br] = false; } } @@ -320,15 +320,15 @@ void Maslow_::home() { } //then make all the belts comply until they are extended fully, or user terminates it else { - if (!extendedTL) - extendedTL = axisTL.extend(computeTL(0, 0, 0)); - if (!extendedTR) - extendedTR = axisTR.extend(computeTR(0, 0, 0)); - if (!extendedBL) - extendedBL = axisBL.extend(computeBL(0, 300, 0)); - if (!extendedBR) - extendedBR = axisBR.extend(computeBR(0, 300, 0)); - if (extendedTL && extendedTR && extendedBL && extendedBR) { + if (!extended[tl]) + extended[tl] = axisTL.extend(computeTL(0, 0, 0)); + if (!extended[tr]) + extended[tr] = axisTR.extend(computeTR(0, 0, 0)); + if (!extended[bl]) + extended[bl] = axisBL.extend(computeBL(0, 300, 0)); + if (!extended[br]) + extended[br] = axisBR.extend(computeBR(0, 300, 0)); + if (extended[tl] && extended[tr] && extended[bl] && extended[br]) { extendingALL = false; log_info("All belts extended to center position"); } @@ -367,7 +367,7 @@ void Maslow_::home() { handleMotorOverides(); //if we are done with all the homing moves, switch system state back to Idle? - if (!retractingTL && !retractingBL && !retractingBR && !retractingTR && !extendingALL && !complyALL && !calibrationInProgress && + if (!retracting[tl] && !retracting[bl] && !retracting[br] && !retracting[tr] && !extendingALL && !complyALL && !calibrationInProgress && !takeSlack && !checkOverides()) { sys.set_state(State::Idle); } @@ -401,12 +401,13 @@ bool Maslow_::takeSlackFunc() { double offset = _beltEndExtension + _armLength; double threshold = 12; - float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); - float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); - float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); - float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); - log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); - if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { + float diff[4]; + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); + diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); + log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); + if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?"); //Should we enter an alarm state here to prevent things from going wrong? @@ -976,26 +977,26 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { //A check to see if the results on the first point are within the expected range if(waypoint == 0){ double threshold = 100; - - float diffTL = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); - float diffTR = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); - float diffBL = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); - float diffBR = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); - log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR); - - if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) { + float diff[4]; + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); + diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); + log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); + + if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { log_error("Center point deviation over " << threshold << "mmm, your coordinate system is not accurate, adjust your frame dimensions and restart."); //Should we enter an alarm state here to prevent things from going wrong? String message = ""; //If both of the bottom belts are longer than expected then the frame is smaller than expected - if(diffBL > threshold && diffBR > threshold){ + if(diff[bl] > threshold && diff[br] > threshold){ log_error("Frame size error, try entering larger frame dimensions and restart."); message = "Frame size error, try entering larger frame dimensions and restart."; } //If both of the bottom belts are shorter than expected then the frame is larger than expected - else if(diffBL < -threshold && diffBR < -threshold){ + else if(diff[bl] < -threshold && diff[br] < -threshold){ log_error("Frame size error, try entering smaller frame dimensions and restart."); message = "Frame size error, try entering smaller frame dimensions and restart."; } @@ -1349,34 +1350,34 @@ void Maslow_::printCalibrationGrid() { void Maslow_::retractTL() { //We allow other bells retracting to continue - retractingTL = true; + retracting[tl] = true; complyALL = false; extendingALL = false; axisTL.reset(); } void Maslow_::retractTR() { - retractingTR = true; + retracting[tr] = true; complyALL = false; extendingALL = false; axisTR.reset(); } void Maslow_::retractBL() { - retractingBL = true; + retracting[bl] = true; complyALL = false; extendingALL = false; axisBL.reset(); } void Maslow_::retractBR() { - retractingBR = true; + retracting[br] = true; complyALL = false; extendingALL = false; axisBR.reset(); } void Maslow_::retractALL() { - retractingTL = true; - retractingTR = true; - retractingBL = true; - retractingBR = true; + retracting[tl] = true; + retracting[tr] = true; + retracting[bl] = true; + retracting[br] = true; complyALL = false; extendingALL = false; axisTL.reset(); @@ -1422,10 +1423,10 @@ void Maslow_::runCalibration() { } void Maslow_::comply() { complyCallTimer = millis(); - retractingTL = false; - retractingTR = false; - retractingBL = false; - retractingBR = false; + retracting[tl] = false; + retracting[tr] = false; + retracting[bl] = false; + retracting[br] = false; extendingALL = false; complyALL = true; axisTL.reset(); //This just resets the thresholds for pull tight @@ -1674,10 +1675,10 @@ void Maslow_::take_slack() { sys.set_state(State::Idle); return; } - retractingTL = false; - retractingTR = false; - retractingBL = false; - retractingBR = false; + retracting[tl] = false; + retracting[tr] = false; + retracting[bl] = false; + retracting[br] = false; extendingALL = false; complyALL = false; axisTL.reset(); @@ -1716,7 +1717,7 @@ bool Maslow_::all_axis_homed() { // True if all axis were extended bool Maslow_::allAxisExtended() { - return extendedTL && extendedTR && extendedBL && extendedBR; + return extended[tl] && extended[tr] && extended[bl] && extended[br]; } // True if calibration is complete or take slack has been run @@ -1777,10 +1778,10 @@ void Maslow_::calibrationDataRecieved(){ // Stop all motors and reset all state variables void Maslow_::stop() { stopMotors(); - retractingTL = false; - retractingTR = false; - retractingBL = false; - retractingBR = false; + retracting[tl] = false; + retracting[tr] = false; + retracting[bl] = false; + retracting[br] = false; extendingALL = false; complyALL = false; calibrationInProgress = false; @@ -1942,26 +1943,26 @@ void Maslow_::log_telem_hdr_csv() { void Maslow_::log_telem_pt_csv(TelemetryData data) { log_data( std::to_string(data.timestamp) + "," - + std::to_string(data.tlCurrent) + "," - + std::to_string(data.trCurrent) + "," - + std::to_string(data.blCurrent) + "," - + std::to_string(data.brCurrent) + "," - + std::to_string(data.tlPower) + "," - + std::to_string(data.trPower) + "," - + std::to_string(data.blPower) + "," - + std::to_string(data.brPower) + "," - + std::to_string(data.tlSpeed) + "," - + std::to_string(data.trSpeed) + "," - + std::to_string(data.blSpeed) + "," - + std::to_string(data.brSpeed) + "," - + std::to_string(data.tlPos) + "," - + std::to_string(data.trPos) + "," - + std::to_string(data.blPos) + "," - + std::to_string(data.brPos) + "," - + std::to_string(data.extendedTL) + "," - + std::to_string(data.extendedTR) + "," - + std::to_string(data.extendedBL) + "," - + std::to_string(data.extendedBR) + "," + + std::to_string(data.current[tl]) + "," + + std::to_string(data.current[tr]) + "," + + std::to_string(data.current[bl]) + "," + + std::to_string(data.current[br]) + "," + + std::to_string(data.power[tl]) + "," + + std::to_string(data.power[tr]) + "," + + std::to_string(data.power[bl]) + "," + + std::to_string(data.power[br]) + "," + + std::to_string(data.speed[tl]) + "," + + std::to_string(data.speed[tr]) + "," + + std::to_string(data.speed[bl]) + "," + + std::to_string(data.speed[br]) + "," + + std::to_string(data.pos[tl]) + "," + + std::to_string(data.pos[tr]) + "," + + std::to_string(data.pos[bl]) + "," + + std::to_string(data.pos[br]) + "," + + std::to_string(data.extended[tl]) + "," + + std::to_string(data.extended[tr]) + "," + + std::to_string(data.extended[bl]) + "," + + std::to_string(data.extended[br]) + "," + std::to_string(data.extendingALL) + "," + std::to_string(data.complyALL) + "," + std::to_string(data.takeSlack) + "," @@ -1996,30 +1997,30 @@ TelemetryData Maslow_::get_telemetry_data() { //if (xSemaphoreTake(telemetry_mutex, portMAX_DELAY)) { // Access shared variables here data.timestamp = millis(); - data.tlCurrent = axisTL.getMotorCurrent(); - data.trCurrent = axisTR.getMotorCurrent(); - data.blCurrent = axisBL.getMotorCurrent(); - data.brCurrent = axisBR.getMotorCurrent(); - - data.tlPower = axisTL.getMotorPower(); - data.trPower = axisTR.getMotorPower(); - data.blPower = axisBL.getMotorPower(); - data.brPower = axisBR.getMotorPower(); - - data.tlSpeed = axisTL.getBeltSpeed(); - data.trSpeed = axisTR.getBeltSpeed(); - data.blSpeed = axisBL.getBeltSpeed(); - data.brSpeed = axisBR.getBeltSpeed(); - - data.tlPos = axisTL.getPosition(); - data.trPos = axisTR.getPosition(); - data.blPos = axisBL.getPosition(); - data.brPos = axisBR.getPosition(); - - data.extendedTL = extendedTL; - data.extendedTR = extendedTR; - data.extendedBL = extendedBL; - data.extendedBR = extendedBR; + data.current[tl] = axisTL.getMotorCurrent(); + data.current[tr] = axisTR.getMotorCurrent(); + data.current[bl] = axisBL.getMotorCurrent(); + data.current[br] = axisBR.getMotorCurrent(); + + data.power[tl] = axisTL.getMotorPower(); + data.power[tr] = axisTR.getMotorPower(); + data.power[bl] = axisBL.getMotorPower(); + data.power[br] = axisBR.getMotorPower(); + + data.speed[tl] = axisTL.getBeltSpeed(); + data.speed[tr] = axisTR.getBeltSpeed(); + data.speed[bl] = axisBL.getBeltSpeed(); + data.speed[br] = axisBR.getBeltSpeed(); + + data.pos[tl] = axisTL.getPosition(); + data.pos[tr] = axisTR.getPosition(); + data.pos[bl] = axisBL.getPosition(); + data.pos[br] = axisBR.getPosition(); + + data.extended[tl] = extended[tl]; + data.extended[tr] = extended[tr]; + data.extended[bl] = extended[bl]; + data.extended[br] = extended[br]; data.extendingALL = extendingALL; data.complyALL = complyALL; data.takeSlack = takeSlack; diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index c9a4305c7..33be34039 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -40,35 +40,17 @@ struct TelemetryFileHeader { struct TelemetryData { unsigned long timestamp; // motors - double tlCurrent; - double trCurrent; - double blCurrent; - double brCurrent; + double current[4]; // power - double tlPower; - double trPower; - double blPower; - double brPower; + double power[4]; // speed - double tlSpeed; - double trSpeed; - double blSpeed; - double brSpeed; + double speed[4]; // position - double tlPos; - double trPos; - double blPos; - double brPos; + double pos[4]; - int tlState; - int trState; - int blState; - int brState; + int state[4]; - bool extendedTL; - bool extendedTR; - bool extendedBL; - bool extendedBR; + bool extended[4]; bool extendingALL; bool complyALL; @@ -161,15 +143,8 @@ class Maslow_ { void set_frame_height(double height); void update_frame_xyz(); bool axis_homed[4] = { false, false, false, false }; - bool retractingTL = false; - bool retractingTR = false; - bool retractingBL = false; - bool retractingBR = false; - - bool extendedTL = false; - bool extendedTR = false; - bool extendedBL = false; - bool extendedBR = false; + bool retracting[4] = { false, false, false, false }; + bool extended[4] = { false, false, false, false }; bool extendingALL = false; bool complyALL = false; From 73920126b085c0c6445e6a1085101a33387fea56 Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 22:39:37 -0800 Subject: [PATCH 06/13] migrate axisXX objects to axis[] array --- FluidNC/src/Maslow/Maslow.cpp | 441 +++++++++++++++++----------------- FluidNC/src/Maslow/Maslow.h | 5 +- 2 files changed, 220 insertions(+), 226 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 0716e5ec4..516481316 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -63,10 +63,10 @@ void Maslow_::begin(void (*sys_rt)()) { Wire.begin(5, 4, 200000); I2CMux.begin(TCAADDR, Wire); - axisTL.begin(tlIn1Pin, tlIn2Pin, tlADCPin, TLEncoderLine, tlIn1Channel, tlIn2Channel); - axisTR.begin(trIn1Pin, trIn2Pin, trADCPin, TREncoderLine, trIn1Channel, trIn2Channel); - axisBL.begin(blIn1Pin, blIn2Pin, blADCPin, BLEncoderLine, blIn1Channel, blIn2Channel); - axisBR.begin(brIn1Pin, brIn2Pin, brADCPin, BREncoderLine, brIn1Channel, brIn2Channel); + axis[tl].begin(tlIn1Pin, tlIn2Pin, tlADCPin, TLEncoderLine, tlIn1Channel, tlIn2Channel); + axis[tr].begin(trIn1Pin, trIn2Pin, trADCPin, TREncoderLine, trIn1Channel, trIn2Channel); + axis[bl].begin(blIn1Pin, blIn2Pin, blADCPin, BLEncoderLine, blIn1Channel, blIn2Channel); + axis[br].begin(brIn1Pin, brIn2Pin, brADCPin, BREncoderLine, brIn1Channel, brIn2Channel); axisBLHomed = false; axisBRHomed = false; @@ -147,10 +147,10 @@ void Maslow_::update() { Maslow.updateEncoderPositions(); //We always update encoder positions in any state, - axisTL.update(); //update motor currents and belt speeds like this for now - axisTR.update(); - axisBL.update(); - axisBR.update(); + axis[tl].update(); //update motor currents and belt speeds like this for now + axis[tr].update(); + axis[bl].update(); + axis[br].update(); if (safetyOn) safety_control(); @@ -277,28 +277,28 @@ void Maslow_::heartBeat(){ void Maslow_::home() { //run all the retract functions untill we hit the current limit if (retracting[tl]) { - if (axisTL.retract()) { + if (axis[tl].retract()) { retracting[tl] = false; axis_homed[tl] = true; extended[tl] = false; } } if (retracting[tr]) { - if (axisTR.retract()) { + if (axis[tr].retract()) { retracting[tr] = false; axis_homed[tr] = true; extended[tr] = false; } } if (retracting[bl]) { - if (axisBL.retract()) { + if (axis[bl].retract()) { retracting[bl] = false; axis_homed[bl] = true; extended[bl] = false; } } if (retracting[br]) { - if (axisBR.retract()) { + if (axis[br].retract()) { retracting[br] = false; axis_homed[br] = true; extended[br] = false; @@ -310,24 +310,24 @@ void Maslow_::home() { //decompress belts for the first half second if (millis() - extendCallTimer < 700) { if (millis() - extendCallTimer > 0) - axisBR.decompressBelt(); + axis[br].decompressBelt(); if (millis() - extendCallTimer > 150) - axisBL.decompressBelt(); + axis[bl].decompressBelt(); if (millis() - extendCallTimer > 250) - axisTR.decompressBelt(); + axis[tr].decompressBelt(); if (millis() - extendCallTimer > 350) - axisTL.decompressBelt(); + axis[tl].decompressBelt(); } //then make all the belts comply until they are extended fully, or user terminates it else { if (!extended[tl]) - extended[tl] = axisTL.extend(computeTL(0, 0, 0)); + extended[tl] = axis[tl].extend(computeTL(0, 0, 0)); if (!extended[tr]) - extended[tr] = axisTR.extend(computeTR(0, 0, 0)); + extended[tr] = axis[tr].extend(computeTR(0, 0, 0)); if (!extended[bl]) - extended[bl] = axisBL.extend(computeBL(0, 300, 0)); + extended[bl] = axis[bl].extend(computeBL(0, 300, 0)); if (!extended[br]) - extended[br] = axisBR.extend(computeBR(0, 300, 0)); + extended[br] = axis[br].extend(computeBR(0, 300, 0)); if (extended[tl] && extended[tr] && extended[bl] && extended[br]) { extendingALL = false; log_info("All belts extended to center position"); @@ -338,15 +338,15 @@ void Maslow_::home() { if (complyALL) { //decompress belts for the first half second if (millis() - complyCallTimer < 40) { - axisBR.decompressBelt(); - axisBL.decompressBelt(); - axisTR.decompressBelt(); - axisTL.decompressBelt(); + axis[br].decompressBelt(); + axis[bl].decompressBelt(); + axis[tr].decompressBelt(); + axis[tl].decompressBelt(); } else if(millis() - complyCallTimer < 800){ - axisTL.comply(); - axisTR.comply(); - axisBL.comply(); - axisBR.comply(); + axis[tl].comply(); + axis[tr].comply(); + axis[bl].comply(); + axis[br].comply(); } else { complyALL = false; @@ -502,22 +502,22 @@ bool Maslow_::updateEncoderPositions() { static int encoderToRead = 0; switch (encoderToRead) { case 0: - if (!axisTL.updateEncoderPosition()) { + if (!axis[tl].updateEncoderPosition()) { encoderFailCounter[TLEncoderLine]++; } break; case 1: - if (!axisTR.updateEncoderPosition()) { + if (!axis[tr].updateEncoderPosition()) { encoderFailCounter[TREncoderLine]++; } break; case 2: - if (!axisBL.updateEncoderPosition()) { + if (!axis[bl].updateEncoderPosition()) { encoderFailCounter[BLEncoderLine]++; } break; case 3: - if (!axisBR.updateEncoderPosition()) { + if (!axis[br].updateEncoderPosition()) { encoderFailCounter[BREncoderLine]++; } break; @@ -559,25 +559,25 @@ void Maslow_::setTargets(float xTarget, float yTarget, float zTarget, bool tl, b targetZ = zTarget; if (tl) { - axisTL.setTarget(computeTL(xTarget, yTarget, zTarget)); + axis[tl].setTarget(computeTL(xTarget, yTarget, zTarget)); } if (tr) { - axisTR.setTarget(computeTR(xTarget, yTarget, zTarget)); + axis[tr].setTarget(computeTR(xTarget, yTarget, zTarget)); } if (bl) { - axisBL.setTarget(computeBL(xTarget, yTarget, zTarget)); + axis[bl].setTarget(computeBL(xTarget, yTarget, zTarget)); } if (br) { - axisBR.setTarget(computeBR(xTarget, yTarget, zTarget)); + axis[br].setTarget(computeBR(xTarget, yTarget, zTarget)); } } //updates motor powers for all axis, based on targets set by setTargets() void Maslow_::recomputePID() { - axisBL.recomputePID(); - axisBR.recomputePID(); - axisTR.recomputePID(); - axisTL.recomputePID(); + axis[bl].recomputePID(); + axis[br].recomputePID(); + axis[tr].recomputePID(); + axis[tl].recomputePID(); digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on @@ -598,10 +598,9 @@ void Maslow_::safety_control() { static int positionErrorCounter[4] = { 0 }; static float previousPositionError[4] = { 0, 0, 0, 0 }; - MotorUnit* axis[4] = { &axisTL, &axisTR, &axisBL, &axisBR }; for (int i = 0; i < 4; i++) { //If the current exceeds some absolute value, we need to call panic() and stop the machine - if (axis[i]->getMotorCurrent() > 4000 && !tick[i]) { + if (axis[i].getMotorCurrent() > 4000 && !tick[i]) { panicCounter[i]++; if (panicCounter[i] > tresholdHitsBeforePanic) { if(sys.state() == State::Jog || sys.state() == State::Cycle){ @@ -622,11 +621,11 @@ void Maslow_::safety_control() { static int axisSlackCounter[4] = { 0, 0, 0, 0 }; axisSlackCounter[i] = 0; //TEMP - if (axis[i]->getMotorPower() > 450 && abs(axis[i]->getBeltSpeed()) < 0.1 && !tick[i]) { + if (axis[i].getMotorPower() > 450 && abs(axis[i].getBeltSpeed()) < 0.1 && !tick[i]) { axisSlackCounter[i]++; if (axisSlackCounter[i] > 3000) { - // log_info("SLACK:" << axis_id_to_label(i).c_str() << " motor power is " << int(axis[i]->getMotorPower()) - // << ", but the belt speed is" << axis[i]->getBeltSpeed()); + // log_info("SLACK:" << axis_id_to_label(i).c_str() << " motor power is " << int(axis[i].getMotorPower()) + // << ", but the belt speed is" << axis[i].getBeltSpeed()); // log_info(axisSlackCounter[i]); // log_info("Pull on " << axis_id_to_label(i).c_str() << " and restart!"); tick[i] = true; @@ -637,18 +636,18 @@ void Maslow_::safety_control() { axisSlackCounter[i] = 0; //If the motor has a position error greater than 1mm and we are running a file or jogging - if ((abs(axis[i]->getPositionError()) > 1) && (sys.state() == State::Jog || sys.state() == State::Cycle) && !tick[i]) { - // log_error("Position error on " << axis_id_to_label(i).c_str() << " axis exceeded 1mm, error is " << axis[i]->getPositionError() + if ((abs(axis[i].getPositionError()) > 1) && (sys.state() == State::Jog || sys.state() == State::Cycle) && !tick[i]) { + // log_error("Position error on " << axis_id_to_label(i).c_str() << " axis exceeded 1mm, error is " << axis[i].getPositionError() // << "mm"); tick[i] = true; } //If the motor has a position error greater than 15mm and we are running a file or jogging - previousPositionError[i] = axis[i]->getPositionError(); - if ((abs(axis[i]->getPositionError()) > 15) && (sys.state() == State::Cycle)) { + previousPositionError[i] = axis[i].getPositionError(); + if ((abs(axis[i].getPositionError()) > 15) && (sys.state() == State::Cycle)) { positionErrorCounter[i]++; log_warn("Position error on " << axis_id_to_label(i).c_str() << " axis exceeded 15mm while running. Error is " - << axis[i]->getPositionError() << "mm" << " Counter: " << positionErrorCounter[i]); + << axis[i].getPositionError() << "mm" << " Counter: " << positionErrorCounter[i]); log_warn("Previous error was " << previousPositionError[i] << "mm"); if(positionErrorCounter[i] > 5){ @@ -752,23 +751,22 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { //Shouldn't this be handled with the same code as below but with the direction set to UP? if (orientation == VERTICAL) { //first we pull two bottom belts tight one after another, if x<0 we pull left belt first, if x>0 we pull right belt first - static bool BL_tight = false; - static bool BR_tight = false; - axisTL.recomputePID(); - axisTR.recomputePID(); + static bool tight[4] = { false, false, false, false }; + axis[tl].recomputePID(); + axis[tr].recomputePID(); //On the left side of the sheet we want to pull the left belt tight first if (x < 0) { - if (!BL_tight) { - if (axisBL.pull_tight(calibrationCurrentThreshold)) { - BL_tight = true; + if (!tight[bl]) { + if (axis[bl].pull_tight(calibrationCurrentThreshold)) { + tight[bl] = true; //log_info("Pulled BL tight"); } return false; } - if (!BR_tight) { - if (axisBR.pull_tight(calibrationCurrentThreshold)) { - BR_tight = true; + if (!tight[br]) { + if (axis[br].pull_tight(calibrationCurrentThreshold)) { + tight[br] = true; //log_info("Pulled BR tight"); } return false; @@ -777,16 +775,16 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { //On the right side of the sheet we want to pull the right belt tight first else { - if (!BR_tight) { - if (axisBR.pull_tight(calibrationCurrentThreshold)) { - BR_tight = true; + if (!tight[br]) { + if (axis[br].pull_tight(calibrationCurrentThreshold)) { + tight[br] = true; //log_info("Pulled BR tight"); } return false; } - if (!BL_tight) { - if (axisBL.pull_tight(calibrationCurrentThreshold)) { - BL_tight = true; + if (!tight[bl]) { + if (axis[bl].pull_tight(calibrationCurrentThreshold)) { + tight[bl] = true; //log_info("Pulled BL tight"); } return false; @@ -794,14 +792,13 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { } //once both belts are pulled, take a measurement - if (BR_tight && BL_tight) { + if (tight[br]&& tight[bl]) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), zOffset[tl]); - calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), zOffset[tr]); - calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), zOffset[bl]); - calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), zOffset[br]); - BR_tight = false; - BL_tight = false; + calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), zOffset[tl]); + calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), zOffset[tr]); + calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), zOffset[bl]); + calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), zOffset[br]); + tight[br] = tight[bl] = false; return true; } return false; @@ -816,47 +813,47 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { static bool pull2_tight = false; switch (dir) { case UP: - holdAxis1 = &axisTL; - holdAxis2 = &axisTR; + holdAxis1 = &axis[tl]; + holdAxis2 = &axis[tr]; if (x < 0) { - pullAxis1 = &axisBL; - pullAxis2 = &axisBR; + pullAxis1 = &axis[bl]; + pullAxis2 = &axis[br]; } else { - pullAxis1 = &axisBR; - pullAxis2 = &axisBL; + pullAxis1 = &axis[br]; + pullAxis2 = &axis[bl]; } break; case DOWN: - holdAxis1 = &axisBL; - holdAxis2 = &axisBR; + holdAxis1 = &axis[bl]; + holdAxis2 = &axis[br]; if (x < 0) { - pullAxis1 = &axisTL; - pullAxis2 = &axisTR; + pullAxis1 = &axis[tl]; + pullAxis2 = &axis[tr]; } else { - pullAxis1 = &axisTR; - pullAxis2 = &axisTL; + pullAxis1 = &axis[tr]; + pullAxis2 = &axis[tl]; } break; case LEFT: - holdAxis1 = &axisTL; - holdAxis2 = &axisBL; + holdAxis1 = &axis[tl]; + holdAxis2 = &axis[bl]; if (y < 0) { - pullAxis1 = &axisBR; - pullAxis2 = &axisTR; + pullAxis1 = &axis[br]; + pullAxis2 = &axis[tr]; } else { - pullAxis1 = &axisTR; - pullAxis2 = &axisBR; + pullAxis1 = &axis[tr]; + pullAxis2 = &axis[br]; } break; case RIGHT: - holdAxis1 = &axisTR; - holdAxis2 = &axisBR; + holdAxis1 = &axis[tr]; + holdAxis2 = &axis[br]; if (y < 0) { - pullAxis1 = &axisBL; - pullAxis2 = &axisTL; + pullAxis1 = &axis[bl]; + pullAxis2 = &axis[tl]; } else { - pullAxis1 = &axisTL; - pullAxis2 = &axisBL; + pullAxis1 = &axis[tl]; + pullAxis2 = &axis[bl]; } break; } @@ -889,10 +886,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { } if (pull1_tight && pull2_tight) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axisTL.getPosition(), zOffset[tl]); - calibration_data[tr][waypoint] = measurementToXYPlane(axisTR.getPosition(), zOffset[tr]); - calibration_data[bl][waypoint] = measurementToXYPlane(axisBL.getPosition(), zOffset[bl]); - calibration_data[br][waypoint] = measurementToXYPlane(axisBR.getPosition(), zOffset[br]); + calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), zOffset[tl]); + calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), zOffset[tr]); + calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), zOffset[bl]); + calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), zOffset[br]); pull1_tight = false; pull2_tight = false; return true; @@ -1041,27 +1038,27 @@ bool Maslow_::move_with_slack(double fromX, double fromY, double toX, double toY //Decompress belts for 500ms...this happens by returning right away before running any of the rest of the code if (millis() - moveBeginTimer < 750 && withSlack) { if (orientation == VERTICAL) { - axisTL.recomputePID(); - axisTR.recomputePID(); - axisBL.decompressBelt(); - axisBR.decompressBelt(); + axis[tl].recomputePID(); + axis[tr].recomputePID(); + axis[bl].decompressBelt(); + axis[br].decompressBelt(); } else { switch (direction) { case UP: - axisBL.decompressBelt(); - axisBR.decompressBelt(); + axis[bl].decompressBelt(); + axis[br].decompressBelt(); break; case DOWN: - axisTL.decompressBelt(); - axisTR.decompressBelt(); + axis[tl].decompressBelt(); + axis[tr].decompressBelt(); break; case LEFT: - axisTR.decompressBelt(); - axisBR.decompressBelt(); + axis[tr].decompressBelt(); + axis[br].decompressBelt(); break; case RIGHT: - axisTL.decompressBelt(); - axisBL.decompressBelt(); + axis[tl].decompressBelt(); + axis[bl].decompressBelt(); break; } } @@ -1076,65 +1073,65 @@ bool Maslow_::move_with_slack(double fromX, double fromY, double toX, double toY return false; } if(orientation == VERTICAL){ - axisTL.recomputePID(); - axisTR.recomputePID(); + axis[tl].recomputePID(); + axis[tr].recomputePID(); if(withSlack){ - axisBL.comply(); - axisBR.comply(); + axis[bl].comply(); + axis[br].comply(); } else{ - axisBL.recomputePID(); - axisBR.recomputePID(); + axis[bl].recomputePID(); + axis[br].recomputePID(); } } else{ switch (direction) { case UP: - axisTL.recomputePID(); - axisTR.recomputePID(); + axis[tl].recomputePID(); + axis[tr].recomputePID(); if(withSlack){ - axisBL.comply(); - axisBR.comply(); + axis[bl].comply(); + axis[br].comply(); } else{ - axisBL.recomputePID(); - axisBR.recomputePID(); + axis[bl].recomputePID(); + axis[br].recomputePID(); } break; case DOWN: if(withSlack){ - axisTL.comply(); - axisTR.comply(); + axis[tl].comply(); + axis[tr].comply(); } else{ - axisTL.recomputePID(); - axisTR.recomputePID(); + axis[tl].recomputePID(); + axis[tr].recomputePID(); } - axisBL.recomputePID(); - axisBR.recomputePID(); + axis[bl].recomputePID(); + axis[br].recomputePID(); break; case LEFT: - axisTL.recomputePID(); - axisBL.recomputePID(); + axis[tl].recomputePID(); + axis[bl].recomputePID(); if(withSlack){ - axisTR.comply(); - axisBR.comply(); + axis[tr].comply(); + axis[br].comply(); } else{ - axisTR.recomputePID(); - axisBR.recomputePID(); + axis[tr].recomputePID(); + axis[br].recomputePID(); } break; case RIGHT: - axisTR.recomputePID(); - axisBR.recomputePID(); + axis[tr].recomputePID(); + axis[br].recomputePID(); if(withSlack){ - axisTL.comply(); - axisBL.comply(); + axis[tl].comply(); + axis[bl].comply(); } else{ - axisTL.recomputePID(); - axisBL.recomputePID(); + axis[tl].recomputePID(); + axis[bl].recomputePID(); } break; } @@ -1204,22 +1201,22 @@ bool Maslow_::checkValidMove(double fromX, double fromY, double toX, double toY) int direction = get_direction(fromX, fromY, toX, toY); switch(direction){ case UP: //If we are moving up we expect the top belts to get shorter so to should be shorter than they are now - if(computeTL(toX, toY, 0) > axisTL.getPosition() || computeTR(toX, toY, 0) > axisTR.getPosition()){ + if(computeTL(toX, toY, 0) > axis[tl].getPosition() || computeTR(toX, toY, 0) > axis[tr].getPosition()){ valid = false; } break; case DOWN: //If we are moving down we expect the bottom belts to get shorter so they should be shorter than they are now - if(computeBL(toX, toY, 0) > axisBL.getPosition() || computeBR(toX, toY, 0) > axisBR.getPosition()){ + if(computeBL(toX, toY, 0) > axis[bl].getPosition() || computeBR(toX, toY, 0) > axis[br].getPosition()){ valid = false; } break; case LEFT: //If we are moving left we expect the left belts to get shorter so they should be shorter than they are now - if(computeTL(toX, toY, 0) > axisTL.getPosition() || computeBL(toX, toY, 0) > axisBL.getPosition()){ + if(computeTL(toX, toY, 0) > axis[tl].getPosition() || computeBL(toX, toY, 0) > axis[bl].getPosition()){ valid = false; } break; case RIGHT: //If we are moving right we expect the right belts to get shorter so they should be shorter than they are now - if(computeTR(toX, toY, 0) > axisTR.getPosition() || computeBR(toX, toY, 0) > axisBR.getPosition()){ + if(computeTR(toX, toY, 0) > axis[tr].getPosition() || computeBR(toX, toY, 0) > axis[br].getPosition()){ valid = false; } break; @@ -1353,25 +1350,25 @@ void Maslow_::retractTL() { retracting[tl] = true; complyALL = false; extendingALL = false; - axisTL.reset(); + axis[tl].reset(); } void Maslow_::retractTR() { retracting[tr] = true; complyALL = false; extendingALL = false; - axisTR.reset(); + axis[tr].reset(); } void Maslow_::retractBL() { retracting[bl] = true; complyALL = false; extendingALL = false; - axisBL.reset(); + axis[bl].reset(); } void Maslow_::retractBR() { retracting[br] = true; complyALL = false; extendingALL = false; - axisBR.reset(); + axis[br].reset(); } void Maslow_::retractALL() { retracting[tl] = true; @@ -1380,10 +1377,10 @@ void Maslow_::retractALL() { retracting[br] = true; complyALL = false; extendingALL = false; - axisTL.reset(); - axisTR.reset(); - axisBL.reset(); - axisBR.reset(); + axis[tl].reset(); + axis[tr].reset(); + axis[bl].reset(); + axis[br].reset(); } void Maslow_::extendALL() { @@ -1429,10 +1426,10 @@ void Maslow_::comply() { retracting[br] = false; extendingALL = false; complyALL = true; - axisTL.reset(); //This just resets the thresholds for pull tight - axisTR.reset(); - axisBL.reset(); - axisBR.reset(); + axis[tl].reset(); //This just resets the thresholds for pull tight + axis[tr].reset(); + axis[bl].reset(); + axis[br].reset(); } @@ -1474,73 +1471,73 @@ void Maslow_::handleMotorOverides(){ if(TLIOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisTL.fullIn(); + axis[tl].fullIn(); }else{ TLIOveride = false; - axisTL.stop(); + axis[tl].stop(); } } if(BRIOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisBR.fullIn(); + axis[br].fullIn(); }else{ BRIOveride = false; - axisBR.stop(); + axis[br].stop(); } } if(TRIOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisTR.fullIn(); + axis[tr].fullIn(); }else{ TRIOveride = false; - axisTR.stop(); + axis[tr].stop(); } } if(BLIOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisBL.fullIn(); + axis[bl].fullIn(); }else{ BLIOveride = false; - axisBL.stop(); + axis[bl].stop(); } } if(TLOOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisTL.fullOut(); + axis[tl].fullOut(); }else{ TLOOveride = false; - axisTL.stop(); + axis[tl].stop(); } } if(BROOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisBR.fullOut(); + axis[br].fullOut(); }else{ BROOveride = false; - axisBR.stop(); + axis[br].stop(); } } if(TROOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisTR.fullOut(); + axis[tr].fullOut(); }else{ TROOveride = false; - axisTR.stop(); + axis[tr].stop(); } } if(BLOOveride){ log_info(int(millis() - overideTimer)); if(millis() - overideTimer < 200){ - axisBL.fullOut(); + axis[bl].fullOut(); }else{ BLOOveride = false; - axisBL.stop(); + axis[bl].stop(); } } } @@ -1561,10 +1558,10 @@ void Maslow_::test_() { log_info("I2C Timeout: "); log_info(Wire.getTimeOut()); - axisTL.test(); - axisTR.test(); - axisBL.test(); - axisBR.test(); + axis[tl].test(); + axis[tr].test(); + axis[bl].test(); + axis[br].test(); } //This function saves the current z-axis position to the non-volitle storage void Maslow_::saveZPos() { @@ -1681,10 +1678,10 @@ void Maslow_::take_slack() { retracting[br] = false; extendingALL = false; complyALL = false; - axisTL.reset(); - axisTR.reset(); - axisBL.reset(); - axisBR.reset(); + axis[tl].reset(); + axis[tr].reset(); + axis[bl].reset(); + axis[br].reset(); x = 0; y = 0; @@ -1704,10 +1701,10 @@ void Maslow_::hold(unsigned long time) { // Resets variables on all 4 axis void Maslow_::reset_all_axis() { - axisTL.reset(); - axisTR.reset(); - axisBL.reset(); - axisBR.reset(); + axis[tl].reset(); + axis[tr].reset(); + axis[bl].reset(); + axis[br].reset(); } // True if all axis were zeroed @@ -1788,10 +1785,10 @@ void Maslow_::stop() { test = false; takeSlack = false; - axisTL.reset(); - axisTR.reset(); - axisBL.reset(); - axisBR.reset(); + axis[tl].reset(); + axis[tr].reset(); + axis[bl].reset(); + axis[br].reset(); // if we are stopping, stop any running job too allChannels.stopJob(); @@ -1799,10 +1796,10 @@ void Maslow_::stop() { // Stop all the motors void Maslow_::stopMotors() { - axisBL.stop(); - axisBR.stop(); - axisTR.stop(); - axisTL.stop(); + axis[bl].stop(); + axis[br].stop(); + axis[tr].stop(); + axis[tl].stop(); } static void stopEverything() { @@ -1854,14 +1851,14 @@ void Maslow_::updateCenterXY() { void Maslow_::getInfo() { log_data("MINFO: { \"homed\": " << (all_axis_homed() ? "true" : "false") << "," << "\"calibrationInProgress\": " << (calibrationInProgress ? "true" : "false") << "," - << "\"tl\": " << axisTL.getPosition() << "," - << "\"tr\": " << axisTR.getPosition() << "," - << "\"br\": " << axisBR.getPosition() << "," - << "\"bl\": " << axisBL.getPosition() << "," - << "\"etl\": " << axisTL.getPositionError() << "," - << "\"etr\": " << axisTR.getPositionError() << "," - << "\"ebr\": " << axisBR.getPositionError() << "," - << "\"ebl\": " << axisBL.getPositionError() << "," + << "\"tl\": " << axis[tl].getPosition() << "," + << "\"tr\": " << axis[tr].getPosition() << "," + << "\"br\": " << axis[br].getPosition() << "," + << "\"bl\": " << axis[bl].getPosition() << "," + << "\"etl\": " << axis[tl].getPositionError() << "," + << "\"etr\": " << axis[tr].getPositionError() << "," + << "\"ebr\": " << axis[br].getPositionError() << "," + << "\"ebl\": " << axis[bl].getPositionError() << "," << "\"extended\": " << (allAxisExtended() ? "true" : "false") << "}"); } @@ -1997,25 +1994,25 @@ TelemetryData Maslow_::get_telemetry_data() { //if (xSemaphoreTake(telemetry_mutex, portMAX_DELAY)) { // Access shared variables here data.timestamp = millis(); - data.current[tl] = axisTL.getMotorCurrent(); - data.current[tr] = axisTR.getMotorCurrent(); - data.current[bl] = axisBL.getMotorCurrent(); - data.current[br] = axisBR.getMotorCurrent(); - - data.power[tl] = axisTL.getMotorPower(); - data.power[tr] = axisTR.getMotorPower(); - data.power[bl] = axisBL.getMotorPower(); - data.power[br] = axisBR.getMotorPower(); - - data.speed[tl] = axisTL.getBeltSpeed(); - data.speed[tr] = axisTR.getBeltSpeed(); - data.speed[bl] = axisBL.getBeltSpeed(); - data.speed[br] = axisBR.getBeltSpeed(); - - data.pos[tl] = axisTL.getPosition(); - data.pos[tr] = axisTR.getPosition(); - data.pos[bl] = axisBL.getPosition(); - data.pos[br] = axisBR.getPosition(); + data.current[tl] = axis[tl].getMotorCurrent(); + data.current[tr] = axis[tr].getMotorCurrent(); + data.current[bl] = axis[bl].getMotorCurrent(); + data.current[br] = axis[br].getMotorCurrent(); + + data.power[tl] = axis[tl].getMotorPower(); + data.power[tr] = axis[tr].getMotorPower(); + data.power[bl] = axis[bl].getMotorPower(); + data.power[br] = axis[br].getMotorPower(); + + data.speed[tl] = axis[tl].getBeltSpeed(); + data.speed[tr] = axis[tr].getBeltSpeed(); + data.speed[bl] = axis[bl].getBeltSpeed(); + data.speed[br] = axis[br].getBeltSpeed(); + + data.pos[tl] = axis[tl].getPosition(); + data.pos[tr] = axis[tr].getPosition(); + data.pos[bl] = axis[bl].getPosition(); + data.pos[br] = axis[br].getPosition(); data.extended[tl] = extended[tl]; data.extended[tr] = extended[tr]; diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 33be34039..248fe7a20 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -181,10 +181,7 @@ class Maslow_ { double targetY = 0; double targetZ = 0; - MotorUnit axisTL; - MotorUnit axisTR; - MotorUnit axisBL; - MotorUnit axisBR; + MotorUnit axis[4]; int retractCurrentThreshold = 1300; int calibrationCurrentThreshold = 1300; float acceptableCalibrationThreshold = 0.5; From ec6867e38ae9062bb76a8807681d555e565efd4d Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 23:48:33 -0800 Subject: [PATCH 07/13] convert measurementToXYPlane to take arm as a parameter --- FluidNC/src/Maslow/Maslow.cpp | 36 +++++++++++++++++------------------ FluidNC/src/Maslow/Maslow.h | 2 +- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 516481316..c851900a9 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -402,10 +402,10 @@ bool Maslow_::takeSlackFunc() { double threshold = 12; float diff[4]; - diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); - diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); - diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); - diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tl); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), tr); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), bl); + diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), br); log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?"); @@ -738,9 +738,9 @@ float Maslow_::computeTL(float x, float y, float z) { //------------------------------------------------------ //Takes a raw measurement, projects it into the XY plane, then adds the belt end extension and arm length to get the actual distance. -float Maslow_::measurementToXYPlane(float measurement, float zHeight){ +float Maslow_::measurementToXYPlane(float measurement, char arm){ - float lengthInXY = sqrt(measurement * measurement - zHeight * zHeight); + float lengthInXY = sqrt(measurement * measurement - zOffset[arm] * zOffset[arm]); return lengthInXY + _beltEndExtension + _armLength; //Add the belt end extension and arm length to get the actual distance } @@ -794,10 +794,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { //once both belts are pulled, take a measurement if (tight[br]&& tight[bl]) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), zOffset[tl]); - calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), zOffset[tr]); - calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), zOffset[bl]); - calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), zOffset[br]); + calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), tl); + calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), tr); + calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), bl); + calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), br); tight[br] = tight[bl] = false; return true; } @@ -886,10 +886,10 @@ bool Maslow_::take_measurement(int waypoint, int dir, int run) { } if (pull1_tight && pull2_tight) { //take measurement and record it to the calibration data array - calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), zOffset[tl]); - calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), zOffset[tr]); - calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), zOffset[bl]); - calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), zOffset[br]); + calibration_data[tl][waypoint] = measurementToXYPlane(axis[tl].getPosition(), tl); + calibration_data[tr][waypoint] = measurementToXYPlane(axis[tr].getPosition(), tr); + calibration_data[bl][waypoint] = measurementToXYPlane(axis[bl].getPosition(), bl); + calibration_data[br][waypoint] = measurementToXYPlane(axis[br].getPosition(), br); pull1_tight = false; pull2_tight = false; return true; @@ -975,10 +975,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { if(waypoint == 0){ double threshold = 100; float diff[4]; - diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), zOffset[tl]); - diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), zOffset[tr]); - diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), zOffset[bl]); - diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), zOffset[br]); + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tl); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), tr); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), bl); + diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), br); log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 248fe7a20..915d3e63d 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -216,7 +216,7 @@ class Maslow_ { bool checkValidMove(double fromX, double fromY, double toX, double toY); bool take_measurement_avg_with_check(int waypoint, int dir); bool take_measurement(int waypoint, int dir, int run); - float measurementToXYPlane(float measurement, float zHeight); + float measurementToXYPlane(float measurement, char arm); bool takeSlackFunc(); void test_(); void calibration_loop(); From d30ba7be6b7b9efd4fc1321a7a46f453abced85a Mon Sep 17 00:00:00 2001 From: David Lang Date: Thu, 14 Nov 2024 23:57:22 -0800 Subject: [PATCH 08/13] collapse computeXX into a single function --- FluidNC/src/Maslow/Maslow.cpp | 96 +++++++++-------------------------- FluidNC/src/Maslow/Maslow.h | 5 +- 2 files changed, 25 insertions(+), 76 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index c851900a9..8d8bb8734 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -321,13 +321,13 @@ void Maslow_::home() { //then make all the belts comply until they are extended fully, or user terminates it else { if (!extended[tl]) - extended[tl] = axis[tl].extend(computeTL(0, 0, 0)); + extended[tl] = axis[tl].extend(compute(0, 0, 0, tl)); if (!extended[tr]) - extended[tr] = axis[tr].extend(computeTR(0, 0, 0)); + extended[tr] = axis[tr].extend(compute(0, 0, 0, tr)); if (!extended[bl]) - extended[bl] = axis[bl].extend(computeBL(0, 300, 0)); + extended[bl] = axis[bl].extend(compute(0, 300, 0, bl)); if (!extended[br]) - extended[br] = axis[br].extend(computeBR(0, 300, 0)); + extended[br] = axis[br].extend(compute(0, 300, 0, br)); if (extended[tl] && extended[tr] && extended[bl] && extended[br]) { extendingALL = false; log_info("All belts extended to center position"); @@ -402,10 +402,10 @@ bool Maslow_::takeSlackFunc() { double threshold = 12; float diff[4]; - diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tl); - diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), tr); - diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), bl); - diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), br); + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(compute(0, 0, 0, tl), tl); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(compute(0, 0, 0, tr), tr); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(compute(0, 0, 0, bl), bl); + diff[br] = calibration_data[br][0] - measurementToXYPlane(compute(0, 0, 0, br), br); log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { log_error("Center point deviation over " << threshold << "mm, your coordinate system is not accurate, maybe try running calibration again?"); @@ -559,16 +559,16 @@ void Maslow_::setTargets(float xTarget, float yTarget, float zTarget, bool tl, b targetZ = zTarget; if (tl) { - axis[tl].setTarget(computeTL(xTarget, yTarget, zTarget)); + axis[tl].setTarget(compute(xTarget, yTarget, zTarget, tl)); } if (tr) { - axis[tr].setTarget(computeTR(xTarget, yTarget, zTarget)); + axis[tr].setTarget(compute(xTarget, yTarget, zTarget, tr)); } if (bl) { - axis[bl].setTarget(computeBL(xTarget, yTarget, zTarget)); + axis[bl].setTarget(compute(xTarget, yTarget, zTarget, bl)); } if (br) { - axis[br].setTarget(computeBR(xTarget, yTarget, zTarget)); + axis[br].setTarget(compute(xTarget, yTarget, zTarget, br)); } } @@ -668,13 +668,13 @@ void Maslow_::safety_control() { } // Compute target belt lengths based on X-Y-Z coordinates -float Maslow_::computeBL(float x, float y, float z) { +float Maslow_::compute(float x, float y, float z, char arm) { //Move from lower left corner coordinates to centered coordinates x = x + centerX; y = y + centerY; - float a = anchor[bl][0] - x; //X dist from corner to router center - float b = anchor[bl][1] - y; //Y dist from corner to router center - float c = 0.0 - (z + zOffset[bl]); //Z dist from corner to router center + float a = anchor[arm][0] - x; //X dist from corner to router center + float b = anchor[arm][1] - y; //Y dist from corner to router center + float c = 0.0 - (z + zOffset[arm]); //Z dist from corner to router center float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center @@ -684,54 +684,6 @@ float Maslow_::computeBL(float x, float y, float z) { return length; //+ lowerBeltsExtra; } -float Maslow_::computeBR(float x, float y, float z) { - //Move from lower left corner coordinates to centered coordinates - x = x + centerX; - y = y + centerY; - float a = anchor[br][0] - x; - float b = anchor[br][1] - y; - float c = 0.0 - (z + zOffset[br]); - - float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center - - float XYBeltLength = XYlength - (_beltEndExtension + _armLength); //Subtract the belt end extension and arm length to get the belt length - - float length = sqrt(XYBeltLength * XYBeltLength + c * c); //Get the angled belt length - - return length; //+ lowerBeltsExtra; -} -float Maslow_::computeTR(float x, float y, float z) { - //Move from lower left corner coordinates to centered coordinates - x = x + centerX; - y = y + centerY; - float a = anchor[tr][0] - x; - float b = anchor[tr][1] - y; - float c = 0.0 - (z + zOffset[tr]); - - float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center - - float XYBeltLength = XYlength - (_beltEndExtension + _armLength); //Subtract the belt end extension and arm length to get the belt length - - float length = sqrt(XYBeltLength * XYBeltLength + c * c); //Get the angled belt length - - return length; //+ lowerBeltsExtra; -} -float Maslow_::computeTL(float x, float y, float z) { - //Move from lower left corner coordinates to centered coordinates - x = x + centerX; - y = y + centerY; - float a = anchor[tl][0] - x; - float b = anchor[tl][1] - y; - float c = 0.0 - (z + zOffset[tl]); - - float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center - - float XYBeltLength = XYlength - (_beltEndExtension + _armLength); //Subtract the belt end extension and arm length to get the belt length - - float length = sqrt(XYBeltLength * XYBeltLength + c * c); //Get the angled belt length - - return length; //+ lowerBeltsExtra; -} //------------------------------------------------------ //------------------------------------------------------ Homing and calibration functions @@ -975,10 +927,10 @@ bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) { if(waypoint == 0){ double threshold = 100; float diff[4]; - diff[tl] = calibration_data[tl][0] - measurementToXYPlane(computeTL(0, 0, 0), tl); - diff[tr] = calibration_data[tr][0] - measurementToXYPlane(computeTR(0, 0, 0), tr); - diff[bl] = calibration_data[bl][0] - measurementToXYPlane(computeBL(0, 0, 0), bl); - diff[br] = calibration_data[br][0] - measurementToXYPlane(computeBR(0, 0, 0), br); + diff[tl] = calibration_data[tl][0] - measurementToXYPlane(compute(0, 0, 0, tl), tl); + diff[tr] = calibration_data[tr][0] - measurementToXYPlane(compute(0, 0, 0, tr), tr); + diff[bl] = calibration_data[bl][0] - measurementToXYPlane(compute(0, 0, 0, bl), bl); + diff[br] = calibration_data[br][0] - measurementToXYPlane(compute(0, 0, 0, br), br); log_info("Center point deviation: TL: " << diff[tl] << " TR: " << diff[tr] << " BL: " << diff[bl] << " BR: " << diff[br]); if (abs(diff[tl]) > threshold || abs(diff[tr]) > threshold || abs(diff[bl]) > threshold || abs(diff[br]) > threshold) { @@ -1201,22 +1153,22 @@ bool Maslow_::checkValidMove(double fromX, double fromY, double toX, double toY) int direction = get_direction(fromX, fromY, toX, toY); switch(direction){ case UP: //If we are moving up we expect the top belts to get shorter so to should be shorter than they are now - if(computeTL(toX, toY, 0) > axis[tl].getPosition() || computeTR(toX, toY, 0) > axis[tr].getPosition()){ + if(compute(toX, toY, 0, tl) > axis[tl].getPosition() || compute(toX, toY, 0, tr) > axis[tr].getPosition()){ valid = false; } break; case DOWN: //If we are moving down we expect the bottom belts to get shorter so they should be shorter than they are now - if(computeBL(toX, toY, 0) > axis[bl].getPosition() || computeBR(toX, toY, 0) > axis[br].getPosition()){ + if(compute(toX, toY, 0, bl) > axis[bl].getPosition() || compute(toX, toY, 0, br) > axis[br].getPosition()){ valid = false; } break; case LEFT: //If we are moving left we expect the left belts to get shorter so they should be shorter than they are now - if(computeTL(toX, toY, 0) > axis[tl].getPosition() || computeBL(toX, toY, 0) > axis[bl].getPosition()){ + if(compute(toX, toY, 0, tl) > axis[tl].getPosition() || compute(toX, toY, 0, bl) > axis[bl].getPosition()){ valid = false; } break; case RIGHT: //If we are moving right we expect the right belts to get shorter so they should be shorter than they are now - if(computeTR(toX, toY, 0) > axis[tr].getPosition() || computeBR(toX, toY, 0) > axis[br].getPosition()){ + if(compute(toX, toY, 0, tr) > axis[tr].getPosition() || compute(toX, toY, 0, br) > axis[br].getPosition()){ valid = false; } break; diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 915d3e63d..69007f31e 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -106,10 +106,7 @@ class Maslow_ { //math void updateCenterXY(); - float computeBL(float x, float y, float z); - float computeBR(float x, float y, float z); - float computeTR(float x, float y, float z); - float computeTL(float x, float y, float z); + float compute(float x, float y, float z, char arm); //Save and load z-axis position, set z-stop void saveZPos(); From 9548d854b5c12632f3df242e5f704faa66fdc88c Mon Sep 17 00:00:00 2001 From: David Lang Date: Fri, 15 Nov 2024 00:06:23 -0800 Subject: [PATCH 09/13] add per-arm belt extensions/stops This number is the length past where retraction stops that the belt actually extends. This can be from putting a stop on the belt so it doesn't retract all the way (useful so that you can do retraction without having to disconnect from anchors) or an added length added to the belt to be able to reach additional distances In both cases, you figure out what value to put in by retracting with and without the modification if you are adding a clamp, retract without the modification, extend, add the clamp and then retract again. The distance reported when the belt is fully retracted is the value to enter for that arm. If you are adding an extension, clamp the router in place, hook the belt to an anchor (further away than your extension), retract until it stops, extend, hook up your extension, retract again. The absolute value of the distance reported is what you enter for that arm. --- FluidNC/src/Machine/MachineConfig.cpp | 5 +++++ FluidNC/src/Maslow/Maslow.cpp | 6 +++--- FluidNC/src/Maslow/Maslow.h | 1 + 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/Machine/MachineConfig.cpp b/FluidNC/src/Machine/MachineConfig.cpp index 829a3b00f..428997019 100644 --- a/FluidNC/src/Machine/MachineConfig.cpp +++ b/FluidNC/src/Machine/MachineConfig.cpp @@ -116,6 +116,11 @@ namespace Machine { handler.item(M+"_blZ", Maslow.zOffset[Maslow.bl]); handler.item(M+"_brZ", Maslow.zOffset[Maslow.br]); + handler.item(M+"_tlExtension", Maslow.beltExtension[Maslow.tl]); + handler.item(M+"_trExtension", Maslow.beltExtension[Maslow.tr]); + handler.item(M+"_blExtension", Maslow.beltExtension[Maslow.bl]); + handler.item(M+"_brExtension", Maslow.beltExtension[Maslow.br]); + handler.item(M+"_Retract_Current_Threshold", Maslow.retractCurrentThreshold, 0, 3500); handler.item(M+"_Calibration_Current_Threshold", Maslow.calibrationCurrentThreshold, 0, 3500); handler.item(M+"_Acceptable_Calibration_Threshold", Maslow.acceptableCalibrationThreshold, 0, 1); diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index 8d8bb8734..e88cbd1ca 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -681,7 +681,7 @@ float Maslow_::compute(float x, float y, float z, char arm) { float XYBeltLength = XYlength - (_beltEndExtension + _armLength); //Subtract the belt end extension and arm length to get the belt length float length = sqrt(XYBeltLength * XYBeltLength + c * c); //Get the angled belt length - + length = length - beltExtension[arm]; // subtract the belt extension from the needed belt length. return length; //+ lowerBeltsExtra; } @@ -691,8 +691,8 @@ float Maslow_::compute(float x, float y, float z, char arm) { //Takes a raw measurement, projects it into the XY plane, then adds the belt end extension and arm length to get the actual distance. float Maslow_::measurementToXYPlane(float measurement, char arm){ - - float lengthInXY = sqrt(measurement * measurement - zOffset[arm] * zOffset[arm]); + float length = measurement + beltExtension[arm]; // add the belt extension on this arm to the measured belt length + float lengthInXY = sqrt(length * length - zOffset[arm] * zOffset[arm]); return lengthInXY + _beltEndExtension + _armLength; //Add the belt end extension and arm length to get the actual distance } diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 69007f31e..e90a6fafe 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -246,6 +246,7 @@ class Maslow_ { // per-arm arrays float zOffset[4]; float anchor[4][2]; + float beltExtension[4] = { 0.0, 0.0, 0.0, 0.0 }; float _beltEndExtension = 30; //Based on the CAD model these should add to 153.4 float _armLength = 123.4; From 4d4a34128c5ed3a9ecd0eb3d5899948bf1e4657b Mon Sep 17 00:00:00 2001 From: David Lang Date: Fri, 15 Nov 2024 00:36:11 -0800 Subject: [PATCH 10/13] support machine variations where the arms don't move with the spindle --- FluidNC/src/Machine/MachineConfig.cpp | 1 + FluidNC/src/Maslow/Maslow.cpp | 16 +++++++++++++--- FluidNC/src/Maslow/Maslow.h | 1 + 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/Machine/MachineConfig.cpp b/FluidNC/src/Machine/MachineConfig.cpp index 428997019..6a5fefeb4 100644 --- a/FluidNC/src/Machine/MachineConfig.cpp +++ b/FluidNC/src/Machine/MachineConfig.cpp @@ -95,6 +95,7 @@ namespace Machine { void MachineConfig::groupM4Items(Configuration::HandlerBase& handler) { handler.item(M+"_vertical", Maslow.orientation); + handler.item(M+"_fixedZ", Maslow.fixedZ); handler.item(M+"_calibration_grid_width_mm_X", Maslow.calibration_grid_width_mm_X, 100, 3000); handler.item(M+"_calibration_grid_height_mm_Y", Maslow.calibration_grid_height_mm_Y, 100, 3000); handler.item(M+"_calibration_grid_size", Maslow.calibrationGridSize, 3, 9); diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index e88cbd1ca..ab1b5d995 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -674,8 +674,12 @@ float Maslow_::compute(float x, float y, float z, char arm) { y = y + centerY; float a = anchor[arm][0] - x; //X dist from corner to router center float b = anchor[arm][1] - y; //Y dist from corner to router center - float c = 0.0 - (z + zOffset[arm]); //Z dist from corner to router center - + float c; + if (fixedZ) { + c = 0.0 - (zOffset[arm]); //with fixed Z, the arms don't move vertically so the offset is all that's needed. + } else { + c = 0.0 - (z + zOffset[arm]); //if the arms move, the current Z location needs to be included in the calculation. + } float XYlength = sqrt(a * a + b * b); //Get the distance in the XY plane from the corner to the router center float XYBeltLength = XYlength - (_beltEndExtension + _armLength); //Subtract the belt end extension and arm length to get the belt length @@ -692,7 +696,13 @@ float Maslow_::compute(float x, float y, float z, char arm) { //Takes a raw measurement, projects it into the XY plane, then adds the belt end extension and arm length to get the actual distance. float Maslow_::measurementToXYPlane(float measurement, char arm){ float length = measurement + beltExtension[arm]; // add the belt extension on this arm to the measured belt length - float lengthInXY = sqrt(length * length - zOffset[arm] * zOffset[arm]); + float c; + if (fixedZ) { + c = zOffset[arm]; + } else { + c = zOffset[arm] + targetZ; //if the arms move with the spindle, the current Z height needs to be included in the calculations + } + float lengthInXY = sqrt(length * length - c * c); return lengthInXY + _beltEndExtension + _armLength; //Add the belt end extension and arm length to get the actual distance } diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index e90a6fafe..80ee15f81 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -223,6 +223,7 @@ class Maslow_ { void reset_all_axis(); bool test = false; bool orientation; + bool fixedZ; double calibration_data[4][CALIBRATION_GRID_SIZE_MAX] = { 0 }; int pointCount = 0; //number of actual points in the grid, < GRID_SIZE_MAX int waypoint = 0; //The current waypoint in the calibration process From c1a4e6f586683b74fd1b36218f420fb94f23674e Mon Sep 17 00:00:00 2001 From: David Lang Date: Fri, 15 Nov 2024 00:57:24 -0800 Subject: [PATCH 11/13] collapse retractXX() functions into a single retract(arm) function --- FluidNC/src/Maslow/Maslow.cpp | 24 +++--------------------- FluidNC/src/Maslow/Maslow.h | 5 +---- FluidNC/src/ProcessSettings.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 29 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.cpp b/FluidNC/src/Maslow/Maslow.cpp index ab1b5d995..4e6a891a2 100644 --- a/FluidNC/src/Maslow/Maslow.cpp +++ b/FluidNC/src/Maslow/Maslow.cpp @@ -1307,30 +1307,12 @@ void Maslow_::printCalibrationGrid() { //------------------------------------------------------ User commands //------------------------------------------------------ -void Maslow_::retractTL() { +void Maslow_::retract(char arm) { //We allow other bells retracting to continue - retracting[tl] = true; + retracting[arm] = true; complyALL = false; extendingALL = false; - axis[tl].reset(); -} -void Maslow_::retractTR() { - retracting[tr] = true; - complyALL = false; - extendingALL = false; - axis[tr].reset(); -} -void Maslow_::retractBL() { - retracting[bl] = true; - complyALL = false; - extendingALL = false; - axis[bl].reset(); -} -void Maslow_::retractBR() { - retracting[br] = true; - complyALL = false; - extendingALL = false; - axis[br].reset(); + axis[arm].reset(); } void Maslow_::retractALL() { retracting[tl] = true; diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 80ee15f81..8852b3f0a 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -119,10 +119,7 @@ class Maslow_ { void stopMotors(); - void retractTL(); - void retractTR(); - void retractBL(); - void retractBR(); + void retract(char arm); void retractALL(); void extendALL(); void take_slack(); diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 08fc3c396..524fa2163 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -833,7 +833,7 @@ static Error maslow_retract_TL(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retractTL(); + Maslow.retract(tl); return Error::Ok; } static Error maslow_retract_TR(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -841,7 +841,7 @@ static Error maslow_retract_TR(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retractTR(); + Maslow.retract(tr); return Error::Ok; } static Error maslow_retract_BR(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -849,7 +849,7 @@ static Error maslow_retract_BR(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retractBR(); + Maslow.retract(br); return Error::Ok; } static Error maslow_retract_BL(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -857,7 +857,7 @@ static Error maslow_retract_BL(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retractBL(); + Maslow.retract(bl); return Error::Ok; } static Error maslow_retract_ALL(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { From e7031a4715dcd4219d7b390d73cab80686e9b6c6 Mon Sep 17 00:00:00 2001 From: David Lang Date: Fri, 15 Nov 2024 01:16:56 -0800 Subject: [PATCH 12/13] change arm constants in header to match EncoderLine #define in code This should not make any difference unless there is something buried somewhere that is doing calculations based on the old order but not being sure how the dats is passed to the UI for calibration this may be the case. --- FluidNC/src/Maslow/Maslow.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/FluidNC/src/Maslow/Maslow.h b/FluidNC/src/Maslow/Maslow.h index 8852b3f0a..86bffea11 100644 --- a/FluidNC/src/Maslow/Maslow.h +++ b/FluidNC/src/Maslow/Maslow.h @@ -236,10 +236,10 @@ class Maslow_ { unsigned long holdTime = 0; //variables to use as indexes into various arrays - const char tl = 0; + const char tl = 2; const char tr = 1; - const char bl = 2; - const char br = 3; + const char bl = 3; + const char br = 0; // per-arm arrays float zOffset[4]; From 19911abae20844a5bca99705d1d8f1222cfc6cb4 Mon Sep 17 00:00:00 2001 From: BarbourSmith Date: Wed, 4 Dec 2024 15:48:02 -0800 Subject: [PATCH 13/13] Add maslow. to undefined vars --- FluidNC/src/ProcessSettings.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/FluidNC/src/ProcessSettings.cpp b/FluidNC/src/ProcessSettings.cpp index 524fa2163..beaa9ce47 100644 --- a/FluidNC/src/ProcessSettings.cpp +++ b/FluidNC/src/ProcessSettings.cpp @@ -833,7 +833,7 @@ static Error maslow_retract_TL(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retract(tl); + Maslow.retract(Maslow.tl); return Error::Ok; } static Error maslow_retract_TR(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -841,7 +841,7 @@ static Error maslow_retract_TR(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retract(tr); + Maslow.retract(Maslow.tr); return Error::Ok; } static Error maslow_retract_BR(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -849,7 +849,7 @@ static Error maslow_retract_BR(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retract(br); + Maslow.retract(Maslow.br); return Error::Ok; } static Error maslow_retract_BL(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) { @@ -857,7 +857,7 @@ static Error maslow_retract_BL(const char* value, WebUI::AuthenticationLevel aut return Error::ConfigurationInvalid; } sys.set_state(State::Homing); - Maslow.retract(bl); + Maslow.retract(Maslow.bl); return Error::Ok; } static Error maslow_retract_ALL(const char* value, WebUI::AuthenticationLevel auth_level, Channel& out) {