bool ThreePointStrategy::handleGcode(Gcode *gcode)
{
if(gcode->has_g) {
// G code processing
if(gcode->g == 29) { // test probe points for level
if(!test_probe_points(gcode)) {
gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
}
return true;
} else if( gcode->g == 31 ) { // report status
if(this->plane == nullptr) {
gcode->stream->printf("Bed leveling plane is not set\n");
}else{
gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
}
gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered");
return true;
} else if( gcode->g == 32 ) { // three point probe
// first wait for an empty queue i.e. no moves left
THEKERNEL->conveyor->wait_for_idle();
// clear any existing plane and compensation
delete this->plane;
this->plane= nullptr;
setAdjustFunction(false);
if(!doProbing(gcode->stream)) {
gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
} else {
gcode->stream->printf("Probe completed, bed plane defined\n");
}
return true;
}
} else if(gcode->has_m) {
if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5 where P is 0,1,2
int idx = 0;
float x = NAN, y = NAN;
if(gcode->has_letter('P')) idx = gcode->get_value('P');
if(gcode->has_letter('X')) x = gcode->get_value('X');
if(gcode->has_letter('Y')) y = gcode->get_value('Y');
if(idx >= 0 && idx <= 2) {
probe_points[idx] = std::make_tuple(x, y);
}else{
gcode->stream->printf("only 3 probe points allowed P0-P2\n");
}
return true;
} else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
delete this->plane;
if(gcode->get_num_args() == 0) {
this->plane= nullptr;
// delete the compensationTransform in robot
setAdjustFunction(false);
gcode->stream->printf("saved plane cleared\n");
}else{
// smoothie specific way to restore a saved plane
uint32_t a,b,c,d;
a=b=c=d= 0;
if(gcode->has_letter('A')) a = gcode->get_uint('A');
if(gcode->has_letter('B')) b = gcode->get_uint('B');
if(gcode->has_letter('C')) c = gcode->get_uint('C');
if(gcode->has_letter('D')) d = gcode->get_uint('D');
this->plane= new Plane3D(a, b, c, d);
setAdjustFunction(true);
}
return true;
} else if(gcode->m == 565) { // M565: Set Z probe offsets
float x= 0, y= 0, z= 0;
if(gcode->has_letter('X')) x = gcode->get_value('X');
if(gcode->has_letter('Y')) y = gcode->get_value('Y');
if(gcode->has_letter('Z')) z = gcode->get_value('Z');
probe_offsets = std::make_tuple(x, y, z);
return true;
} else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
float x, y, z;
gcode->stream->printf(";Probe points:\n");
for (int i = 0; i < 3; ++i) {
std::tie(x, y) = probe_points[i];
gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
}
gcode->stream->printf(";Probe offsets:\n");
std::tie(x, y, z) = probe_offsets;
gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
// encode plane and save if set and M500 and enabled
if(this->save && this->plane != nullptr) {
if(gcode->m == 500) {
uint32_t a, b, c, d;
this->plane->encode(a, b, c, d);
gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d);
}else{
gcode->stream->printf(";The bed plane will be saved on M500\n");
}
}
return true;
}
#if 0
else if(gcode->m == 9999) {
// DEBUG run a test M9999 A B C X Y set Z to A B C and test for point at X Y
Vector3 v[3];
float x, y, z, a= 0, b= 0, c= 0;
if(gcode->has_letter('A')) a = gcode->get_value('A');
if(gcode->has_letter('B')) b = gcode->get_value('B');
if(gcode->has_letter('C')) c = gcode->get_value('C');
std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
delete this->plane;
this->plane = new Plane3D(v[0], v[1], v[2]);
gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
x= 0; y=0;
if(gcode->has_letter('X')) x = gcode->get_value('X');
if(gcode->has_letter('Y')) y = gcode->get_value('Y');
z= getZOffset(x, y);
gcode->stream->printf("z= %f\n", z);
// tell robot to adjust z on each move
setAdjustFunction(true);
return true;
}
#endif
}
return false;
}
bool ZGridStrategy::handleGcode(Gcode *gcode)
{
string args = get_arguments(gcode->get_command());
// G code processing
if(gcode->has_g) {
if( gcode->g == 31 ) { // report status
// Bed ZGrid data as gcode:
gcode->stream->printf(";Bed Level settings:\r\n");
for (int x=0; x<this->numRows; x++) {
gcode->stream->printf("X%i",x);
for (int y=0; y<this->numCols; y++) {
gcode->stream->printf(" %c%1.2f", 'A'+y, this->pData[(x*this->numCols)+y]);
}
gcode->stream->printf("\r\n");
}
return true;
} else if( gcode->g == 32 ) { //run probe
// first wait for an empty queue i.e. no moves left
THEKERNEL->conveyor->wait_for_empty_queue();
this->setAdjustFunction(false); // Disable leveling code
if(!doProbing(gcode->stream)) {
gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
} else {
this->setAdjustFunction(true); // Enable leveling code
gcode->stream->printf("Probe completed, bed grid defined\n");
}
return true;
}
} else if(gcode->has_m) {
switch( gcode->m ) {
// manual bed ZGrid calbration: M370 - M375
// M370: Clear current ZGrid for calibration, and move to first position
case 370: {
this->setAdjustFunction(false); // Disable leveling code
this->cal[Z_AXIS] = std::get<Z_AXIS>(this->probe_offsets) + zprobe->getProbeHeight();
if(gcode->has_letter('X')) // Rows (X)
this->numRows = gcode->get_value('X');
if(gcode->has_letter('Y')) // Cols (Y)
this->numCols = gcode->get_value('Y');
this->calcConfig(); // Run calculations for Grid size and allocate grid memory
this->homexyz();
for (int i=0; i<probe_points; i++) {
this->pData[i] = 0.0F; // Clear the ZGrid
}
this->cal[X_AXIS] = 0.0f; // Clear calibration position
this->cal[Y_AXIS] = 0.0f;
this->in_cal = true; // In calbration mode
}
return true;
// M371: Move to next manual calibration position
case 371: {
if (in_cal) {
this->move(this->cal, slow_rate);
this->next_cal();
}
}
return true;
// M372: save current position in ZGrid, and move to next calibration position
case 372: {
if (in_cal) {
float cartesian[3];
int pindex = 0;
THEKERNEL->robot->get_axis_position(cartesian); // get actual position from robot
pindex = int(cartesian[X_AXIS]/this->bed_div_x + 0.25)*this->numCols + int(cartesian[Y_AXIS]/this->bed_div_y + 0.25);
this->move(this->cal, slow_rate); // move to the next position
this->next_cal(); // to not cause damage to machine due to Z-offset
this->pData[pindex] = cartesian[Z_AXIS]; // save the offset
}
}
return true;
// M373: finalize calibration
case 373: {
// normalize the grid
this->normalize_grid_2home();
this->in_cal = false;
this->setAdjustFunction(true); // Enable leveling code
}
return true;
// M374: Save grid
case 374: {
char gridname[5];
if(gcode->has_letter('S')) // Custom grid number
snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S'));
else
gridname[0] = '\0';
if(this->saveGrid(gridname)) {
gcode->stream->printf("Grid saved: Filename: /sd/Zgrid.%s\n",gridname);
}
else {
gcode->stream->printf("Error: Grid not saved: Filename: /sd/Zgrid.%s\n",gridname);
}
}
return true;
case 375: { // Load grid values
char gridname[5];
if(gcode->has_letter('S')) // Custom grid number
snprintf(gridname, sizeof(gridname), "S%03.0f", gcode->get_value('S'));
else
gridname[0] = '\0';
if(this->loadGrid(gridname)) {
this->setAdjustFunction(true); // Enable leveling code
gcode->stream->printf("Grid loaded: /sd/Zgrid.%s\n",gridname);
}
else {
gcode->stream->printf("Error: Grid not loaded: /sd/Zgrid.%s\n",gridname);
}
}
return true;
/* case 376: { // Check grid value calculations: For debug only.
float target[3];
for(char letter = 'X'; letter <= 'Z'; letter++) {
if( gcode->has_letter(letter) ) {
target[letter - 'X'] = gcode->get_value(letter);
}
}
gcode->stream->printf(" Z0 %1.3f\n",getZOffset(target[0], target[1]));
}
return true;
*/
case 565: { // M565: Set Z probe offsets
float x= 0, y= 0, z= 0;
if(gcode->has_letter('X')) x = gcode->get_value('X');
if(gcode->has_letter('Y')) y = gcode->get_value('Y');
if(gcode->has_letter('Z')) z = gcode->get_value('Z');
probe_offsets = std::make_tuple(x, y, z);
}
return true;
case 500: // M500 saves probe_offsets config override file
gcode->stream->printf(";Load default grid\nM375\n");
case 503: { // M503 just prints the settings
float x,y,z;
gcode->stream->printf(";Probe offsets:\n");
std::tie(x, y, z) = probe_offsets;
gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
break;
}
return true;
}
}
return false;
}
