19811714133

Committed 24 Nov 2025 02:34PM UTC coverage: 59.007% (-15.0%) from 74.006%

Build # 19811714133

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #730 from mar0x/master

Enable windows build in CI

Run Details

2199 of 4409 branches covered (49.88%)

Branch coverage included in aggregate %.

1902 of 2541 relevant lines covered (74.85%)

117318.67 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

7.01

/autoleveller.cpp

/*
 * This file is part of pcb2gcode.
 * 
 * Copyright (C) 2014, 2015 Nicola Corna <nicola@corna.info>
 * 
 * pcb2gcode is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * pcb2gcode is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with pcb2gcode.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "autoleveller.hpp"

#include <cmath>
#include <limits>

#include <boost/algorithm/string.hpp>
#include <boost/geometry/algorithms/distance.hpp>

#include <boost/format.hpp>
#include <memory>
#include <vector>
using boost::format;
using std::shared_ptr;
using std::vector;
using std::endl;
using std::to_string;
using std::string;

#include <utility>
using std::pair;

#include "units.hpp"

#include "bg_operators.hpp"

const string autoleveller::callSubRepeat[] = {
 "o%3$d repeat [%2%]\n%4$s    o%1% call\n%4$so%3$d endrepeat\n",
 "M98 P%1% L%2%\n",
 "M98 P%1% L%2%\n" };

const string autoleveller::probeCode[] = { "G38.2", "G31", "G31" };
const string autoleveller::zProbeResultVar[] = { "#5063", "#5063", "#2002" };
const string autoleveller::setZZero[] = { "G10 L20 P0 Z0", "G92 Z0", "G92 Z0" };

boost::format silent_format(const string &f_string)
{
    format fmter(f_string);
    fmter.exceptions( boost::io::all_error_bits ^ boost::io::too_many_args_bit ^ boost::io::too_few_args_bit );
    return fmter;
}

autoleveller::autoleveller( const boost::program_options::variables_map &options, uniqueCodes *ocodes,
                            uniqueCodes *globalVars, double xoffset, double yoffset,
                            const struct Tiling::TileInfo tileInfo ) :
    input_unitconv( options["metric"].as<bool>() ? 1.0/25.4 : 1),
    output_unitconv( options["metricoutput"].as<bool>() ? 25.4 : 1),
    cfactor( options["metricoutput"].as<bool>() ? 25.4 : 1 ),
    probeCodeCustom( options["al-probecode"].as<string>() ),
    zProbeResultVarCustom( "#" + to_string(options["al-probevar"].as<unsigned int>()) ),
    setZZeroCustom( options["al-setzzero"].as<string>() ),
    XProbeDistRequired( options["al-x"].as<Length>().asInch(input_unitconv) * output_unitconv ),
    YProbeDistRequired( options["al-y"].as<Length>().asInch(input_unitconv) * output_unitconv ),
    zprobe( str( format("%.3f") % ( options["zsafe"].as<Length>().asInch(input_unitconv) * output_unitconv ) ) ),
    zsafe( str( format("%.3f") % ( options["zsafe"].as<Length>().asInch(input_unitconv) * output_unitconv) ) ),
    zfail( str( format("%.3f") % ( options["metricoutput"].as<bool>() ? FIXED_FAIL_DEPTH_MM : FIXED_FAIL_DEPTH_IN ) ) ),
    feedrate( std::to_string( options["al-probefeed"].as<Velocity>().asInchPerMinute(input_unitconv) * output_unitconv ) ),
    probeOn( boost::replace_all_copy(options["al-probe-on"].as<string>(), "@", "\n") ),
    probeOff( boost::replace_all_copy(options["al-probe-off"].as<string>(), "@", "\n") ),
    software( options["software"].as<Software::Software>() ),
    xoffset( xoffset ),
    yoffset( yoffset ),
    g01InterpolatedNum( ocodes->getUniqueCode() ),
    yProbeNum( ocodes->getUniqueCode() ),
    xProbeNum( ocodes->getUniqueCode() ),
    returnVar( to_string(globalVars->getUniqueCode()) ),
    globalVar0( to_string(globalVars->getUniqueCode()) ),
    globalVar1( to_string(globalVars->getUniqueCode()) ),
    globalVar2( to_string(globalVars->getUniqueCode()) ),
    globalVar3( to_string(globalVars->getUniqueCode()) ),
    globalVar4( to_string(globalVars->getUniqueCode()) ),
    globalVar5( to_string(globalVars->getUniqueCode()) ),
    tileInfo( tileInfo ),
    initialXOffsetVar( globalVars->getUniqueCode() ),
    initialYOffsetVar( globalVars->getUniqueCode() ),
    ocodes( ocodes )
{
    callSub2[Software::LINUXCNC] = "o%1$s call [%2$.5f] [%3$.5f] [%4$.5f]\n";
    callSub2[Software::MACH4] = "G65 P%1$s A%2$.5f B%3$.5f C%4$.5f\n";
    callSub2[Software::MACH3] = "#" + globalVar0 + "=%2$.5f\n%5$s#" + globalVar1 + "=%3$.5f\n%6$s#" + globalVar2 + "=%4$.5f\n%7$sM98 P%1$s\n";
}

string autoleveller::getVarName(unsigned int i, unsigned int j) {
  return '#' + to_string(i * numYPoints + j + 500);        //getVarName(10,8) returns (numYPoints=10) #180
}

box_type_fp computeWorkarea(const vector<pair<coordinate_type_fp, multi_linestring_type_fp>>& toolpaths) {
  box_type_fp bounding_box = boost::geometry::make_inverse<box_type_fp>();

  for (const auto& toolpath : toolpaths) {
    for (const auto& linestring : toolpath.second) {
      boost::geometry::expand(bounding_box,
                              boost::geometry::return_envelope<box_type_fp>(linestring));
    }
  }

  return bounding_box;
}

void autoleveller::prepareWorkarea(const vector<pair<coordinate_type_fp, multi_linestring_type_fp>>& toolpaths) {
    box_type_fp workarea;
    double workareaLenX;
    double workareaLenY;

    workarea = computeWorkarea(toolpaths);
    workarea.min_corner().x(workarea.min_corner().x() - xoffset);
    workarea.min_corner().y(workarea.min_corner().y() - yoffset);
    workarea.max_corner().x(workarea.max_corner().x() - xoffset);
    workarea.max_corner().y(workarea.max_corner().y() - yoffset);

    workareaLenX = ( workarea.max_corner().x() - workarea.min_corner().x() ) * cfactor + 
                   tileInfo.boardWidth * cfactor * ( tileInfo.tileX - 1 );
    workareaLenY = ( workarea.max_corner().y() - workarea.min_corner().y() ) * cfactor +
                   tileInfo.boardHeight * cfactor * ( tileInfo.tileY - 1 );

    startPointX = workarea.min_corner().x() * cfactor;
    startPointY = workarea.min_corner().y() * cfactor;

    numXPoints = ceil(workareaLenX / XProbeDistRequired) + 1;
    numXPoints = std::max(2U, numXPoints); //We need at least 2 probe points

    numYPoints = ceil(workareaLenY / YProbeDistRequired) + 1;    //We need at least 2 probe points
    numYPoints = std::max(2U, numYPoints); //We need at least 2 probe points

    XProbeDist = workareaLenX / ( numXPoints - 1 );
    YProbeDist = workareaLenY / ( numYPoints - 1 );
    averageProbeDist = ( XProbeDist + YProbeDist ) / 2;

    if (requiredProbePoints() > maxProbePoints()) {
      options::maybe_throw(std::string("Required number of probe points (") + std::to_string(requiredProbePoints()) +
                           ") exceeds the maximum number (" + std::to_string(maxProbePoints()) + "). "
                           "Reduce either al-x or al-y.", ERR_INVALIDPARAMETER);
    }
}

void autoleveller::header(std::ofstream &of) {
    const char *logFileOpenAndComment[] = {
        "(PROBEOPEN RawProbeLog.txt) ( Record all probes in RawProbeLog.txt )",
        "M40 (Begins a probe log file, when the window appears, enter a name for the log file such as \"RawProbeLog.txt\")",
        "M40 (Begins a probe log file, when the window appears, enter a name for the log file such as \"RawProbeLog.txt\")"
    };
    const char *logFileClose[] = { "(PROBECLOSE)" , "M41", "M41" };

    if( software == Software::LINUXCNC )
        footerNoIf( of );

    if( tileInfo.enabled )
    {
        of << "#" << initialXOffsetVar << " = #5211\n";      //Save the initial offset
        of << "#" << initialYOffsetVar << " = #5212\n\n";
    }
    else
    {
        of << "#" << initialXOffsetVar << " = 0\n";
        of << "#" << initialYOffsetVar << " = 0\n\n";
    }
    of << probeOn << '\n';
    of << "G0 Z" << zsafe << " ( Move Z to safe height )\n";
    of << "G0 X" << startPointX << " Y" << startPointY << " ( Move XY to start point )\n";
    of << "G0 Z" << zprobe << " ( Move Z to probe height )\n";
    if( software != Software::CUSTOM )
        of << logFileOpenAndComment[software] << '\n';
    of << ( software == Software::CUSTOM ? probeCodeCustom : probeCode[software] ) << " Z" << zfail 
       << " F" << feedrate << " ( Z-probe )\n";
    of << "#500 = 0 ( Probe point [0, 0] is our reference )\n";
    of << ( software == Software::CUSTOM ? setZZeroCustom : setZZero[software] )
       << " ( Set the current Z as zero-value )\n";
    of << '\n';
    of << "( We now start the real probing: move the Z axis to the probing height, move to )\n";
    of << "( the probing XY position, probe it and save the result, parameter "
       << ( software == Software::CUSTOM ? zProbeResultVarCustom : zProbeResultVar[software] ) << ", )\n";
    of << "( in a numbered parameter; we will make " << numXPoints << " probes on the X-axis and )\n";
    of << "( " << numYPoints << " probes on the Y-axis, for a grand total of " << numXPoints * numYPoints << " probes )\n";
    of << '\n';

    if( software != Software::CUSTOM )
    {
        of << "#" << globalVar0 << " = 0 ( X iterator )\n";
        of << "#" << globalVar1 << " = 1 ( Y iterator )\n";
        of << "#" << globalVar2 << " = 1 ( UP or DOWN increment )\n";
        of << "#" << globalVar3 << " = " << numYPoints - 1 << " ( number of Y points; the 1st Y row can be done one time less )\n";
        of << silent_format( callSubRepeat[software] ) % xProbeNum % numXPoints % ocodes->getUniqueCode();
    }
    else
    {
      for (unsigned int i = 0; i < numXPoints; i++) {
        int j_start;
        int j_end;
        int j_direction;
        if (i % 2 == 0) {
          // Count upward.
          j_start = 0;
          j_end = numYPoints;
          j_direction = 1;
        } else {
          // Count downward.
          j_start = numYPoints - 1;
          j_end = -1;
          j_direction = -1;
        }
        if (i == 0) {
          j_start = 1; // Because the first probe was done above
        }
        for (int j = j_start; j != j_end; j += j_direction) {
          of << "G0 Z" << zprobe << '\n';
          of << "X" << i * XProbeDist + startPointX << " Y" << j * YProbeDist + startPointY << '\n';
          of << probeCodeCustom << " Z" << zfail << " F" << feedrate << '\n';
          of << getVarName(i, j) << "=" << zProbeResultVarCustom << '\n';
        }
      }
    }

    of << '\n';
    of << "G0 Z" << zsafe << " ( Move Z to safe height )\n";
    if( software != Software::CUSTOM )
       of << logFileClose[software] << " ( Close the probe log file )\n";
    of << "( Probing has ended, each Z-coordinate will be corrected with a bilinear interpolation )\n";
    of << probeOff << '\n';
    of << '\n';
}

void autoleveller::footerNoIf(std::ofstream &of) {
    const char *startSub[] = { "o%1$d sub", "O%1$d", "O%1$d" };
    const char *endSub[] = { "o%1$d endsub", "M99", "M99" };
    const char *var1[] = { "1", "1", globalVar0.c_str() };
    const char *var2[] = { "2", "2", globalVar1.c_str() };
    const char *var3[] = { "3", "3", globalVar2.c_str() };

    if(software != Software::CUSTOM) {
        of << format(startSub[software]) % g01InterpolatedNum << " ( G01 with Z-correction subroutine )\n";
        if( tileInfo.enabled )
        {
            of << "    #4 = [ #5211 - #" << initialXOffsetVar << " ] ( x-tile offset [minus the initial offset] )\n";
            of << "    #5 = [ #5212 - #" << initialYOffsetVar << " ] ( y-tile offset [minus the initial offset] )\n";
        }
        else
        {
            of << "    #4 = 0 ( x-tile offset [minus the initial offset] )\n";
            of << "    #5 = 0 ( y-tile offset [minus the initial offset] )\n";
        }
        of << "    #6 = [ FIX[ [ #" << var1[software] << " - " << startPointX << " + #4 ] / " << XProbeDist << " ] ] ( Lower left point X index )\n";
        of << "    #7 = [ FIX[ [ #" << var2[software] << " - " << startPointY << " + #5 ] / " << YProbeDist << " ] ] ( Lower left point Y index )\n";
        of << "    #8 = [ #6 * " << numYPoints << " + [ #7 + 1 ] + 500 ] ( Upper left point parameter number )\n";
        of << "    #9 = [ [ #6 + 1 ] *" << numYPoints << " + [ #7 + 1 ] + 500 ] ( Upper right point parameter number )\n";
        of << "    #10 = [ #6 * " << numYPoints << " + #7 + 500 ] ( Lower left point parameter number )\n";
        of << "    #11 = [ [ #6 + 1 ] * " << numYPoints << " + #7 + 500 ] ( Lower right point parameter number )\n";
        of << "    #12 = [ [ #" << var2[software] << " + #5 - " << startPointY << " - #7 * " << YProbeDist << " ] / " << YProbeDist << " ] "
           "( Distance between the point and the left border of the rectangle, normalized to 1 )\n";
        of << "    #13 = [ [ #" << var1[software] << " + #4 - " << startPointX << " - #6 * " << XProbeDist << " ] / " << XProbeDist << " ] "
           "( Distance between the point and the bottom border of the rectangle, normalized to 1 ) \n";
        of << "    #14 = [ ##10 + [ ##8 - ##10 ] * #12 ] ( Linear interpolation of the x-min elements )\n";
        of << "    #15 = [ ##11 + [ ##9 - ##11 ] * #12 ] ( Linear interpolation of the x-max elements )\n";
        of << "    #16 = [ #14 + [ #15 - #14 ] * #13 ] ( Linear interpolation of previously interpolated points )\n";
        of << "    G01 X#" << var1[software] << " Y#" << var2[software] << " Z[#" << var3[software] << " + #16]\n";
        of << silent_format(endSub[software]) % g01InterpolatedNum << endl;
        of << endl;
        of << format( startSub[software] ) % yProbeNum << " ( Y probe subroutine )\n";
        of << "    G0 Z" << zprobe << " ( Move to probe height )\n";
        of << "    X[#" << globalVar0 << " * " << XProbeDist << " + " << startPointX << "] Y[#" << globalVar1
           << " * " << YProbeDist << " + " << startPointY << "] ( Move to the current probe point )\n";
        of << "    " << probeCode[software] << " Z" << zfail << " F" << feedrate << " ( Probe it )\n";
        of << "    #[#" << globalVar0 << " * " << numYPoints << " + #" << globalVar1 << " + 500] = "
           << ( software == Software::CUSTOM ? zProbeResultVarCustom : zProbeResultVar[software] )
           << " ( Save the probe in the correct parameter )\n";
        of << "    #" << globalVar1 << " = [#" << globalVar1 << " + #" << globalVar2 << "] ( Increment/decrement by 1 the Y counter )\n";
        of << silent_format( endSub[software] ) % yProbeNum << endl;
        of << endl;
        of << format( startSub[software] ) % xProbeNum << " ( X probe subroutine )\n";
        of << "    " << silent_format( callSubRepeat[software] ) % yProbeNum % ( "#" + globalVar3 ) % ocodes->getUniqueCode() % "    ";
        of << "    #" << globalVar3 << " = " << numYPoints << endl;
        of << "    #" << globalVar2 << " = [0 - #" << globalVar2 << "]\n";
        of << "    #" << globalVar1 << " = [#" << globalVar1 << " + #" << globalVar2 << ']' << endl;
        of << "    #" << globalVar0 << " = [#" << globalVar0 << " + 1] ( Increment by 1 the X counter )\n";
        of << silent_format( endSub[software] ) % xProbeNum << endl;
        of << endl;
    }
}

template <typename T>
static inline T clamp(const T& x, const T& min_x, const T& max_x) {
  return std::max(min_x, std::min(x, max_x));
}

string autoleveller::interpolatePoint(point_type_fp point) {
  unsigned int xminindex;
  unsigned int yminindex;
  double x_minus_x0_rel;
  double y_minus_y0_rel;

  // Calculate point index for measurement point below and to the left
  // of `point`
  xminindex = floor((point.x() - startPointX) / XProbeDist);
  xminindex = clamp(xminindex, 0U, numXPoints - 2);

  yminindex = floor((point.y() - startPointY) / YProbeDist);
  yminindex = clamp(yminindex, 0U, numYPoints - 2);

  // Get fraction of the distance to the next measurement point up and
  // to the left that `point` is.
  x_minus_x0_rel = (point.x() - startPointX - xminindex * XProbeDist) / XProbeDist;
  x_minus_x0_rel = clamp(x_minus_x0_rel, 0.0, 1.0);
  y_minus_y0_rel = (point.y() - startPointY - yminindex * YProbeDist) / YProbeDist;
  y_minus_y0_rel = clamp(y_minus_y0_rel, 0.0, 1.0);

  if (x_minus_x0_rel == 1.0) {
    x_minus_x0_rel -= 1;
    xminindex += 1;
  }
  if (y_minus_y0_rel == 1.0) {
    y_minus_y0_rel -= 1;
    yminindex += 1;
  }

  if (y_minus_y0_rel == 0) {
    if (x_minus_x0_rel == 0) {
      // If `point` is on top of a measurement point, just copy
      // the measured height over
      return str(format("#%2$s=%1$s\n")
                 % getVarName(xminindex, yminindex)
                 % returnVar);
    } else {
      // If `point` has the same y coordinate as a row of points,
      // interpolate between the points to the left and right of
      // it
      return str(format("#%4$s=[%1$s+[%2$s-%1$s]*%3$.5f]\n")
                 % getVarName(xminindex, yminindex)
                 % getVarName(xminindex + 1, yminindex)
                 % x_minus_x0_rel % returnVar);
    }
  } else {
    if (x_minus_x0_rel == 0) {
      // If `point` has the same x coordinate as a column of
      // points, interpolate between the points above and below it
      return str(format("#%4$s=[%2$s+[%1$s-%2$s]*%3$.5f]\n")
                 % getVarName(xminindex, yminindex + 1)
                 % getVarName(xminindex, yminindex)
                 % y_minus_y0_rel % returnVar);
    } else {
      // ...else use bilinear interpolation between all four
      // points around it
      return str(format("#%7$s=[%3$s+[%1$s-%3$s]*%5$.5f]\n#%8$s=[%4$s+[%2$s-%4$s]*%5$.5f]\n#%9$s=[#%7$s+[#%8$s-#%7$s]*%6$.5f]\n")
                 % getVarName(xminindex, yminindex + 1)
                 % getVarName(xminindex + 1, yminindex + 1)
                 % getVarName(xminindex, yminindex)
                 % getVarName(xminindex + 1, yminindex)
                 % y_minus_y0_rel % x_minus_x0_rel
                 % globalVar4 % globalVar5 % returnVar);
    }
  }
}


static inline point_type_fp operator*(const point_type_fp& a, const point_type_fp& b) {
  return point_type_fp(a.x() * b.x(), a.y() * b.y());
}

static inline point_type_fp operator/(const point_type_fp& a, const point_type_fp& b) {
  return point_type_fp(a.x() / b.x(), a.y() / b.y());
}


linestring_type_fp partition_segment(const point_type_fp& source, const point_type_fp& dest,
                                     const point_type_fp& grid_zero, const point_type_fp& grid_width) {
  if (source == dest) {
    return {dest};
  }
  double current_progress = 0;
  linestring_type_fp points;
  while (current_progress != 1) {
    const auto current = source + (dest - source) * current_progress;
    points.push_back(current);

    const auto current_index = floor((current - grid_zero) / grid_width);
    double best_progress = 1;
    for (const auto& index_delta : {-1,0,1,2}) {
      const auto new_point = (current_index + point_type_fp(index_delta, index_delta)) * grid_width + grid_zero;
      const auto new_progress = (new_point - source) / (dest - source);
      if (new_progress.x() > current_progress && new_progress.x() < best_progress) {
        // This step gets us closer to the end yet moves the least amount in that direction.
        best_progress = new_progress.x();
      }
      if (new_progress.y() > current_progress && new_progress.y() < best_progress) {
        // This step gets us closer to the end yet moves the least amount in that direction.
        best_progress = new_progress.y();
      }
    }
    current_progress = best_progress;
  }
  points.push_back(dest);
  return points;
}

string autoleveller::addChainPoint (point_type_fp point, double zwork) {
    string outputStr;
    linestring_type_fp subsegments;
    linestring_type_fp::const_iterator i;

    subsegments = partition_segment(lastPoint, point, point_type_fp(startPointX, startPointY), point_type_fp(XProbeDist, YProbeDist));

    if (software == Software::LINUXCNC || software == Software::MACH4 || software == Software::MACH3) {
      for( i = subsegments.begin() + 1; i != subsegments.end(); i++ )
        outputStr += str( silent_format( callSub2[software] ) % g01InterpolatedNum % i->x() % i->y() % zwork);
    } else {
      for(i = subsegments.begin() + 1; i != subsegments.end(); i++) {
        outputStr += interpolatePoint( *i );
        outputStr += str( format( "X%1$.5f Y%2$.5f Z[#%4$s+%3$.5f]\n" ) % i->x() % i->y() % zwork % returnVar );
      }
    }

    lastPoint = point;
    return outputStr;
}

string autoleveller::g01Corrected (point_type_fp point, double zwork) {
  if( software == Software::LINUXCNC || software == Software::MACH4 || software == Software::MACH3 ) {
    return str( silent_format( callSub2[software] ) % g01InterpolatedNum % point.x() % point.y() % zwork);
  } else {
    return interpolatePoint( point ) + "G01 Z[" + str(format("%.5f")%zwork) + "+#" + returnVar + "]\n";
  }
}

1	/*
2	* This file is part of pcb2gcode.
3	*
4	* Copyright (C) 2014, 2015 Nicola Corna <nicola@corna.info>
5	*
6	* pcb2gcode is free software: you can redistribute it and/or modify
7	* it under the terms of the GNU General Public License as published by
8	* the Free Software Foundation, either version 3 of the License, or
9	* (at your option) any later version.
10	*
11	* pcb2gcode is distributed in the hope that it will be useful,
12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14	* GNU General Public License for more details.
15	*
16	* You should have received a copy of the GNU General Public License
17	* along with pcb2gcode. If not, see <http://www.gnu.org/licenses/>.
18	*/
19
20	#include "autoleveller.hpp"
21
22	#include <cmath>
23	#include <limits>
24
25	#include <boost/algorithm/string.hpp>
26	#include <boost/geometry/algorithms/distance.hpp>
27
28	#include <boost/format.hpp>
29	#include <memory>
30	#include <vector>
31	using boost::format;
32	using std::shared_ptr;
33	using std::vector;
34	using std::endl;
35	using std::to_string;
36	using std::string;
37
38	#include <utility>
39	using std::pair;
40
41	#include "units.hpp"
42
43	#include "bg_operators.hpp"
44
45	const string autoleveller::callSubRepeat[] = {
46	"o%3$d repeat [%2%]\n%4$s o%1% call\n%4$so%3$d endrepeat\n",
47	"M98 P%1% L%2%\n",
48	"M98 P%1% L%2%\n" };
49
50	const string autoleveller::probeCode[] = { "G38.2", "G31", "G31" };
51	const string autoleveller::zProbeResultVar[] = { "#5063", "#5063", "#2002" };
52	const string autoleveller::setZZero[] = { "G10 L20 P0 Z0", "G92 Z0", "G92 Z0" };
53
54	boost::format silent_format(const string &f_string)	×
55	{
56	format fmter(f_string);	×
57	fmter.exceptions( boost::io::all_error_bits ^ boost::io::too_many_args_bit ^ boost::io::too_few_args_bit );
58	return fmter;	×
59	}
60
61	autoleveller::autoleveller( const boost::program_options::variables_map &options, uniqueCodes *ocodes,	×
62	uniqueCodes *globalVars, double xoffset, double yoffset,
63	const struct Tiling::TileInfo tileInfo ) :	×
64	input_unitconv( options["metric"].as<bool>() ? 1.0/25.4 : 1),	×
65	output_unitconv( options["metricoutput"].as<bool>() ? 25.4 : 1),	×
66	cfactor( options["metricoutput"].as<bool>() ? 25.4 : 1 ),	×
67	probeCodeCustom( options["al-probecode"].as<string>() ),	×
68	zProbeResultVarCustom( "#" + to_string(options["al-probevar"].as<unsigned int>()) ),	×
69	setZZeroCustom( options["al-setzzero"].as<string>() ),	×
70	XProbeDistRequired( options["al-x"].as<Length>().asInch(input_unitconv) * output_unitconv ),	×
71	YProbeDistRequired( options["al-y"].as<Length>().asInch(input_unitconv) * output_unitconv ),	×
72	zprobe( str( format("%.3f") % ( options["zsafe"].as<Length>().asInch(input_unitconv) * output_unitconv ) ) ),	×
73	zsafe( str( format("%.3f") % ( options["zsafe"].as<Length>().asInch(input_unitconv) * output_unitconv) ) ),	×
74	zfail( str( format("%.3f") % ( options["metricoutput"].as<bool>() ? FIXED_FAIL_DEPTH_MM : FIXED_FAIL_DEPTH_IN ) ) ),	×
75	feedrate( std::to_string( options["al-probefeed"].as<Velocity>().asInchPerMinute(input_unitconv) * output_unitconv ) ),	×
76	probeOn( boost::replace_all_copy(options["al-probe-on"].as<string>(), "@", "\n") ),	×
77	probeOff( boost::replace_all_copy(options["al-probe-off"].as<string>(), "@", "\n") ),	×
78	software( options["software"].as<Software::Software>() ),	×
79	xoffset( xoffset ),	×
80	yoffset( yoffset ),	×
81	g01InterpolatedNum( ocodes->getUniqueCode() ),	×
82	yProbeNum( ocodes->getUniqueCode() ),	×
83	xProbeNum( ocodes->getUniqueCode() ),	×
84	returnVar( to_string(globalVars->getUniqueCode()) ),	×
85	globalVar0( to_string(globalVars->getUniqueCode()) ),	×
86	globalVar1( to_string(globalVars->getUniqueCode()) ),	×
87	globalVar2( to_string(globalVars->getUniqueCode()) ),	×
88	globalVar3( to_string(globalVars->getUniqueCode()) ),	×
89	globalVar4( to_string(globalVars->getUniqueCode()) ),	×
90	globalVar5( to_string(globalVars->getUniqueCode()) ),	×
91	tileInfo( tileInfo ),	×
92	initialXOffsetVar( globalVars->getUniqueCode() ),	×
93	initialYOffsetVar( globalVars->getUniqueCode() ),	×
94	ocodes( ocodes )	×
95	{
96	callSub2[Software::LINUXCNC] = "o%1$s call [%2$.5f] [%3$.5f] [%4$.5f]\n";
97	callSub2[Software::MACH4] = "G65 P%1$s A%2$.5f B%3$.5f C%4$.5f\n";	×
98	callSub2[Software::MACH3] = "#" + globalVar0 + "=%2$.5f\n%5$s#" + globalVar1 + "=%3$.5f\n%6$s#" + globalVar2 + "=%4$.5f\n%7$sM98 P%1$s\n";	×
99	}	×
100
101	string autoleveller::getVarName(unsigned int i, unsigned int j) {	×
102	return '#' + to_string(i * numYPoints + j + 500); //getVarName(10,8) returns (numYPoints=10) #180	×
103	}
104
105	box_type_fp computeWorkarea(const vector<pair<coordinate_type_fp, multi_linestring_type_fp>>& toolpaths) {	×
106	box_type_fp bounding_box = boost::geometry::make_inverse<box_type_fp>();
107
108	for (const auto& toolpath : toolpaths) {	×
109	for (const auto& linestring : toolpath.second) {	×
110	boost::geometry::expand(bounding_box,
111	boost::geometry::return_envelope<box_type_fp>(linestring));	×
112	}
113	}
114
115	return bounding_box;	×
116	}
117
118	void autoleveller::prepareWorkarea(const vector<pair<coordinate_type_fp, multi_linestring_type_fp>>& toolpaths) {	×
119	box_type_fp workarea;
120	double workareaLenX;
121	double workareaLenY;
122
123	workarea = computeWorkarea(toolpaths);	×
124	workarea.min_corner().x(workarea.min_corner().x() - xoffset);	×
125	workarea.min_corner().y(workarea.min_corner().y() - yoffset);	×
126	workarea.max_corner().x(workarea.max_corner().x() - xoffset);	×
127	workarea.max_corner().y(workarea.max_corner().y() - yoffset);	×
128
129	workareaLenX = ( workarea.max_corner().x() - workarea.min_corner().x() ) * cfactor +	×
130	tileInfo.boardWidth * cfactor * ( tileInfo.tileX - 1 );	×
131	workareaLenY = ( workarea.max_corner().y() - workarea.min_corner().y() ) * cfactor +	×
132	tileInfo.boardHeight * cfactor * ( tileInfo.tileY - 1 );	×
133
134	startPointX = workarea.min_corner().x() * cfactor;	×
135	startPointY = workarea.min_corner().y() * cfactor;	×
136
137	numXPoints = ceil(workareaLenX / XProbeDistRequired) + 1;	×
138	numXPoints = std::max(2U, numXPoints); //We need at least 2 probe points	×
139
140	numYPoints = ceil(workareaLenY / YProbeDistRequired) + 1; //We need at least 2 probe points	×
141	numYPoints = std::max(2U, numYPoints); //We need at least 2 probe points	×
142
143	XProbeDist = workareaLenX / ( numXPoints - 1 );	×
144	YProbeDist = workareaLenY / ( numYPoints - 1 );	×
145	averageProbeDist = ( XProbeDist + YProbeDist ) / 2;	×
146
147	if (requiredProbePoints() > maxProbePoints()) {	×
148	options::maybe_throw(std::string("Required number of probe points (") + std::to_string(requiredProbePoints()) +	×
149	") exceeds the maximum number (" + std::to_string(maxProbePoints()) + "). "	×
150	"Reduce either al-x or al-y.", ERR_INVALIDPARAMETER);
151	}
152	}	×
153
154	void autoleveller::header(std::ofstream &of) {	×
155	const char *logFileOpenAndComment[] = {	×
156	"(PROBEOPEN RawProbeLog.txt) ( Record all probes in RawProbeLog.txt )",
157	"M40 (Begins a probe log file, when the window appears, enter a name for the log file such as \"RawProbeLog.txt\")",
158	"M40 (Begins a probe log file, when the window appears, enter a name for the log file such as \"RawProbeLog.txt\")"
159	};
160	const char *logFileClose[] = { "(PROBECLOSE)" , "M41", "M41" };	×
161
162	if( software == Software::LINUXCNC )	×
163	footerNoIf( of );	×
164
165	if( tileInfo.enabled )	×
166	{
167	of << "#" << initialXOffsetVar << " = #5211\n"; //Save the initial offset	×
168	of << "#" << initialYOffsetVar << " = #5212\n\n";	×
169	}
170	else
171	{
172	of << "#" << initialXOffsetVar << " = 0\n";	×
173	of << "#" << initialYOffsetVar << " = 0\n\n";	×
174	}
175	of << probeOn << '\n';	×
176	of << "G0 Z" << zsafe << " ( Move Z to safe height )\n";	×
177	of << "G0 X" << startPointX << " Y" << startPointY << " ( Move XY to start point )\n";	×
178	of << "G0 Z" << zprobe << " ( Move Z to probe height )\n";	×
179	if( software != Software::CUSTOM )	×
180	of << logFileOpenAndComment[software] << '\n';	×
181	of << ( software == Software::CUSTOM ? probeCodeCustom : probeCode[software] ) << " Z" << zfail	×
182	<< " F" << feedrate << " ( Z-probe )\n";	×
183	of << "#500 = 0 ( Probe point [0, 0] is our reference )\n";	×
184	of << ( software == Software::CUSTOM ? setZZeroCustom : setZZero[software] )	×
185	<< " ( Set the current Z as zero-value )\n";	×
186	of << '\n';	×
187	of << "( We now start the real probing: move the Z axis to the probing height, move to )\n";	×
188	of << "( the probing XY position, probe it and save the result, parameter "
189	<< ( software == Software::CUSTOM ? zProbeResultVarCustom : zProbeResultVar[software] ) << ", )\n";	×
190	of << "( in a numbered parameter; we will make " << numXPoints << " probes on the X-axis and )\n";	×
191	of << "( " << numYPoints << " probes on the Y-axis, for a grand total of " << numXPoints * numYPoints << " probes )\n";	×
192	of << '\n';	×
193
194	if( software != Software::CUSTOM )	×
195	{
196	of << "#" << globalVar0 << " = 0 ( X iterator )\n";	×
197	of << "#" << globalVar1 << " = 1 ( Y iterator )\n";	×
198	of << "#" << globalVar2 << " = 1 ( UP or DOWN increment )\n";	×
199	of << "#" << globalVar3 << " = " << numYPoints - 1 << " ( number of Y points; the 1st Y row can be done one time less )\n";	×
200	of << silent_format( callSubRepeat[software] ) % xProbeNum % numXPoints % ocodes->getUniqueCode();	×
201	}
202	else
203	{
204	for (unsigned int i = 0; i < numXPoints; i++) {	×
205	int j_start;
206	int j_end;
207	int j_direction;
208	if (i % 2 == 0) {	×
209	// Count upward.
210	j_start = 0;
211	j_end = numYPoints;	×
212	j_direction = 1;
213	} else {
214	// Count downward.
215	j_start = numYPoints - 1;	×
216	j_end = -1;
217	j_direction = -1;
218	}
219	if (i == 0) {	×
220	j_start = 1; // Because the first probe was done above
221	}
222	for (int j = j_start; j != j_end; j += j_direction) {	×
223	of << "G0 Z" << zprobe << '\n';	×
224	of << "X" << i * XProbeDist + startPointX << " Y" << j * YProbeDist + startPointY << '\n';	×
225	of << probeCodeCustom << " Z" << zfail << " F" << feedrate << '\n';	×
226	of << getVarName(i, j) << "=" << zProbeResultVarCustom << '\n';	×
227	}
228	}
229	}
230
231	of << '\n';	×
232	of << "G0 Z" << zsafe << " ( Move Z to safe height )\n";	×
233	if( software != Software::CUSTOM )	×
234	of << logFileClose[software] << " ( Close the probe log file )\n";	×
235	of << "( Probing has ended, each Z-coordinate will be corrected with a bilinear interpolation )\n";	×
236	of << probeOff << '\n';	×
237	of << '\n';	×
238	}	×
239
240	void autoleveller::footerNoIf(std::ofstream &of) {	×
241	const char *startSub[] = { "o%1$d sub", "O%1$d", "O%1$d" };	×
242	const char *endSub[] = { "o%1$d endsub", "M99", "M99" };	×
243	const char *var1[] = { "1", "1", globalVar0.c_str() };	×
244	const char *var2[] = { "2", "2", globalVar1.c_str() };	×
245	const char *var3[] = { "3", "3", globalVar2.c_str() };	×
246
247	if(software != Software::CUSTOM) {	×
248	of << format(startSub[software]) % g01InterpolatedNum << " ( G01 with Z-correction subroutine )\n";	×
249	if( tileInfo.enabled )	×
250	{
251	of << " #4 = [ #5211 - #" << initialXOffsetVar << " ] ( x-tile offset [minus the initial offset] )\n";	×
252	of << " #5 = [ #5212 - #" << initialYOffsetVar << " ] ( y-tile offset [minus the initial offset] )\n";	×
253	}
254	else
255	{
256	of << " #4 = 0 ( x-tile offset [minus the initial offset] )\n";	×
257	of << " #5 = 0 ( y-tile offset [minus the initial offset] )\n";	×
258	}
259	of << " #6 = [ FIX[ [ #" << var1[software] << " - " << startPointX << " + #4 ] / " << XProbeDist << " ] ] ( Lower left point X index )\n";	×
260	of << " #7 = [ FIX[ [ #" << var2[software] << " - " << startPointY << " + #5 ] / " << YProbeDist << " ] ] ( Lower left point Y index )\n";	×
261	of << " #8 = [ #6 * " << numYPoints << " + [ #7 + 1 ] + 500 ] ( Upper left point parameter number )\n";	×
262	of << " #9 = [ [ #6 + 1 ] *" << numYPoints << " + [ #7 + 1 ] + 500 ] ( Upper right point parameter number )\n";	×
263	of << " #10 = [ #6 * " << numYPoints << " + #7 + 500 ] ( Lower left point parameter number )\n";	×
264	of << " #11 = [ [ #6 + 1 ] * " << numYPoints << " + #7 + 500 ] ( Lower right point parameter number )\n";	×
265	of << " #12 = [ [ #" << var2[software] << " + #5 - " << startPointY << " - #7 * " << YProbeDist << " ] / " << YProbeDist << " ] "	×
266	"( Distance between the point and the left border of the rectangle, normalized to 1 )\n";	×
267	of << " #13 = [ [ #" << var1[software] << " + #4 - " << startPointX << " - #6 * " << XProbeDist << " ] / " << XProbeDist << " ] "	×
268	"( Distance between the point and the bottom border of the rectangle, normalized to 1 ) \n";	×
269	of << " #14 = [ ##10 + [ ##8 - ##10 ] * #12 ] ( Linear interpolation of the x-min elements )\n";	×
270	of << " #15 = [ ##11 + [ ##9 - ##11 ] * #12 ] ( Linear interpolation of the x-max elements )\n";	×
271	of << " #16 = [ #14 + [ #15 - #14 ] * #13 ] ( Linear interpolation of previously interpolated points )\n";	×
272	of << " G01 X#" << var1[software] << " Y#" << var2[software] << " Z[#" << var3[software] << " + #16]\n";	×
273	of << silent_format(endSub[software]) % g01InterpolatedNum << endl;	×
274	of << endl;
275	of << format( startSub[software] ) % yProbeNum << " ( Y probe subroutine )\n";	×
276	of << " G0 Z" << zprobe << " ( Move to probe height )\n";	×
277	of << " X[#" << globalVar0 << " * " << XProbeDist << " + " << startPointX << "] Y[#" << globalVar1	×
278	<< " * " << YProbeDist << " + " << startPointY << "] ( Move to the current probe point )\n";	×
279	of << " " << probeCode[software] << " Z" << zfail << " F" << feedrate << " ( Probe it )\n";	×
280	of << " #[#" << globalVar0 << " * " << numYPoints << " + #" << globalVar1 << " + 500] = "	×
281	<< ( software == Software::CUSTOM ? zProbeResultVarCustom : zProbeResultVar[software] )	×
282	<< " ( Save the probe in the correct parameter )\n";	×
283	of << " #" << globalVar1 << " = [#" << globalVar1 << " + #" << globalVar2 << "] ( Increment/decrement by 1 the Y counter )\n";	×
284	of << silent_format( endSub[software] ) % yProbeNum << endl;	×
285	of << endl;
286	of << format( startSub[software] ) % xProbeNum << " ( X probe subroutine )\n";	×
287	of << " " << silent_format( callSubRepeat[software] ) % yProbeNum % ( "#" + globalVar3 ) % ocodes->getUniqueCode() % " ";	×
288	of << " #" << globalVar3 << " = " << numYPoints << endl;	×
289	of << " #" << globalVar2 << " = [0 - #" << globalVar2 << "]\n";	×
290	of << " #" << globalVar1 << " = [#" << globalVar1 << " + #" << globalVar2 << ']' << endl;	×
291	of << " #" << globalVar0 << " = [#" << globalVar0 << " + 1] ( Increment by 1 the X counter )\n";	×
292	of << silent_format( endSub[software] ) % xProbeNum << endl;	×
293	of << endl;
294	}
295	}	×
296
297	template <typename T>
298	static inline T clamp(const T& x, const T& min_x, const T& max_x) {
299	return std::max(min_x, std::min(x, max_x));	×
300	}
301
302	string autoleveller::interpolatePoint(point_type_fp point) {	×
303	unsigned int xminindex;
304	unsigned int yminindex;
305	double x_minus_x0_rel;
306	double y_minus_y0_rel;
307
308	// Calculate point index for measurement point below and to the left
309	// of `point`
310	xminindex = floor((point.x() - startPointX) / XProbeDist);	×
311	xminindex = clamp(xminindex, 0U, numXPoints - 2);	×
312
313	yminindex = floor((point.y() - startPointY) / YProbeDist);	×
314	yminindex = clamp(yminindex, 0U, numYPoints - 2);	×
315
316	// Get fraction of the distance to the next measurement point up and
317	// to the left that `point` is.
318	x_minus_x0_rel = (point.x() - startPointX - xminindex * XProbeDist) / XProbeDist;	×
319	x_minus_x0_rel = clamp(x_minus_x0_rel, 0.0, 1.0);	×
320	y_minus_y0_rel = (point.y() - startPointY - yminindex * YProbeDist) / YProbeDist;	×
321	y_minus_y0_rel = clamp(y_minus_y0_rel, 0.0, 1.0);	×
322
323	if (x_minus_x0_rel == 1.0) {	×
324	x_minus_x0_rel -= 1;	×
325	xminindex += 1;	×
326	}
327	if (y_minus_y0_rel == 1.0) {	×
328	y_minus_y0_rel -= 1;	×
329	yminindex += 1;	×
330	}
331
332	if (y_minus_y0_rel == 0) {	×
333	if (x_minus_x0_rel == 0) {	×
334	// If `point` is on top of a measurement point, just copy
335	// the measured height over
336	return str(format("#%2$s=%1$s\n")	×
337	% getVarName(xminindex, yminindex)	×
338	% returnVar);	×
339	} else {
340	// If `point` has the same y coordinate as a row of points,
341	// interpolate between the points to the left and right of
342	// it
343	return str(format("#%4$s=[%1$s+[%2$s-%1$s]*%3$.5f]\n")	×
344	% getVarName(xminindex, yminindex)	×
345	% getVarName(xminindex + 1, yminindex)	×
346	% x_minus_x0_rel % returnVar);	×
347	}
348	} else {
349	if (x_minus_x0_rel == 0) {	×
350	// If `point` has the same x coordinate as a column of
351	// points, interpolate between the points above and below it
352	return str(format("#%4$s=[%2$s+[%1$s-%2$s]*%3$.5f]\n")	×
353	% getVarName(xminindex, yminindex + 1)	×
354	% getVarName(xminindex, yminindex)	×
355	% y_minus_y0_rel % returnVar);	×
356	} else {
357	// ...else use bilinear interpolation between all four
358	// points around it
359	return str(format("#%7$s=[%3$s+[%1$s-%3$s]%5$.5f]\n#%8$s=[%4$s+[%2$s-%4$s]%5$.5f]\n#%9$s=[#%7$s+[#%8$s-#%7$s]*%6$.5f]\n")	×
360	% getVarName(xminindex, yminindex + 1)	×
361	% getVarName(xminindex + 1, yminindex + 1)	×
362	% getVarName(xminindex, yminindex)	×
363	% getVarName(xminindex + 1, yminindex)	×
364	% y_minus_y0_rel % x_minus_x0_rel
365	% globalVar4 % globalVar5 % returnVar);	×
366	}
367	}
368	}
369
370
371	static inline point_type_fp operator*(const point_type_fp& a, const point_type_fp& b) {
372	return point_type_fp(a.x() * b.x(), a.y() * b.y());	196✔
373	}
374
375	static inline point_type_fp operator/(const point_type_fp& a, const point_type_fp& b) {
376	return point_type_fp(a.x() / b.x(), a.y() / b.y());	245✔
377	}
378
379
380	linestring_type_fp partition_segment(const point_type_fp& source, const point_type_fp& dest,	6✔
381	const point_type_fp& grid_zero, const point_type_fp& grid_width) {
382	if (source == dest) {
383	return {dest};	1✔
384	}
385	double current_progress = 0;
386	linestring_type_fp points;
387	while (current_progress != 1) {	54✔
388	const auto current = source + (dest - source) * current_progress;	49✔
389	points.push_back(current);	49!
390
391	const auto current_index = floor((current - grid_zero) / grid_width);	49✔
392	double best_progress = 1;
393	for (const auto& index_delta : {-1,0,1,2}) {	245✔
394	const auto new_point = (current_index + point_type_fp(index_delta, index_delta)) * grid_width + grid_zero;	196✔
395	const auto new_progress = (new_point - source) / (dest - source);
396	if (new_progress.x() > current_progress && new_progress.x() < best_progress) {	196✔
397	// This step gets us closer to the end yet moves the least amount in that direction.
398	best_progress = new_progress.x();
399	}
400	if (new_progress.y() > current_progress && new_progress.y() < best_progress) {	196✔
401	// This step gets us closer to the end yet moves the least amount in that direction.
402	best_progress = new_progress.y();
403	}
404	}
405	current_progress = best_progress;
406	}
407	points.push_back(dest);	5!
408	return points;
409	}
410
411	string autoleveller::addChainPoint (point_type_fp point, double zwork) {	×
412	string outputStr;
413	linestring_type_fp subsegments;
414	linestring_type_fp::const_iterator i;
415
416	subsegments = partition_segment(lastPoint, point, point_type_fp(startPointX, startPointY), point_type_fp(XProbeDist, YProbeDist));	×
417
418	if (software == Software::LINUXCNC \|\| software == Software::MACH4 \|\| software == Software::MACH3) {	×
419	for( i = subsegments.begin() + 1; i != subsegments.end(); i++ )	×
420	outputStr += str( silent_format( callSub2[software] ) % g01InterpolatedNum % i->x() % i->y() % zwork);	×
421	} else {
422	for(i = subsegments.begin() + 1; i != subsegments.end(); i++) {	×
423	outputStr += interpolatePoint( *i );	×
424	outputStr += str( format( "X%1$.5f Y%2$.5f Z[#%4$s+%3$.5f]\n" ) % i->x() % i->y() % zwork % returnVar );	×
425	}
426	}
427
428	lastPoint = point;	×
429	return outputStr;	×
430	}
431
432	string autoleveller::g01Corrected (point_type_fp point, double zwork) {	×
433	if( software == Software::LINUXCNC \|\| software == Software::MACH4 \|\| software == Software::MACH3 ) {	×
434	return str( silent_format( callSub2[software] ) % g01InterpolatedNum % point.x() % point.y() % zwork);	×
435	} else {
436	return interpolatePoint( point ) + "G01 Z[" + str(format("%.5f")%zwork) + "+#" + returnVar + "]\n";	×
437	}
438	}

pcb2gcode / pcb2gcode / 19811714133

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous