X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/f520c637e86a5c58be96912cb807d07fd5cbff82..3b2168fea555e259188109719b22b24363299c94:/src/modules/robot/Robot.cpp diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp index 4e26f687..c8e32157 100644 --- a/src/modules/robot/Robot.cpp +++ b/src/modules/robot/Robot.cpp @@ -33,6 +33,7 @@ #include "ExtruderPublicAccess.h" #include "GcodeDispatch.h" #include "ActuatorCoordinates.h" +#include "EndstopsPublicAccess.h" #include "mbed.h" // for us_ticker_read() #include "mri.h" @@ -40,7 +41,6 @@ #include #include #include -using std::string; #define default_seek_rate_checksum CHECKSUM("default_seek_rate") #define default_feed_rate_checksum CHECKSUM("default_feed_rate") @@ -88,7 +88,16 @@ using std::string; #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power") -#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians) +#define enable_checksum CHECKSUM("enable") +#define halt_checksum CHECKSUM("halt") +#define soft_endstop_checksum CHECKSUM("soft_endstop") +#define xmin_checksum CHECKSUM("x_min") +#define ymin_checksum CHECKSUM("y_min") +#define zmin_checksum CHECKSUM("z_min") +#define xmax_checksum CHECKSUM("x_max") +#define ymax_checksum CHECKSUM("y_max") +#define zmax_checksum CHECKSUM("z_max") + #define PI 3.14159265358979323846F // force to be float, do not use M_PI // The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue @@ -195,7 +204,7 @@ void Robot::load_config() this->s_value = THEKERNEL->config->value(laser_module_default_power_checksum)->by_default(0.8F)->as_number(); // Make our Primary XYZ StepperMotors, and potentially A B C - uint16_t const checksums[][6] = { + uint16_t const motor_checksums[][6] = { ACTUATOR_CHECKSUMS("alpha"), // X ACTUATOR_CHECKSUMS("beta"), // Y ACTUATOR_CHECKSUMS("gamma"), // Z @@ -217,11 +226,15 @@ void Robot::load_config() for (size_t a = 0; a < MAX_ROBOT_ACTUATORS; a++) { Pin pins[3]; //step, dir, enable for (size_t i = 0; i < 3; i++) { - pins[i].from_string(THEKERNEL->config->value(checksums[a][i])->by_default("nc")->as_string())->as_output(); + pins[i].from_string(THEKERNEL->config->value(motor_checksums[a][i])->by_default("nc")->as_string())->as_output(); } - if(!pins[0].connected() || !pins[1].connected() || !pins[2].connected()) { - if(a <= Z_AXIS) THEKERNEL->streams->printf("FATAL: motor %d is not defined in config\n", 'X'+a); + if(!pins[0].connected() || !pins[1].connected()) { // step and dir must be defined, but enable is optional + if(a <= Z_AXIS) { + THEKERNEL->streams->printf("FATAL: motor %c is not defined in config\n", 'X'+a); + n_motors= a; // we only have this number of motors + return; + } break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order) } @@ -231,12 +244,12 @@ void Robot::load_config() if(n != a) { // this is a fatal error THEKERNEL->streams->printf("FATAL: motor %d does not match index %d\n", n, a); - __debugbreak(); + return; } - actuators[a]->change_steps_per_mm(THEKERNEL->config->value(checksums[a][3])->by_default(a == 2 ? 2560.0F : 80.0F)->as_number()); - actuators[a]->set_max_rate(THEKERNEL->config->value(checksums[a][4])->by_default(30000.0F)->as_number()/60.0F); // it is in mm/min and converted to mm/sec - actuators[a]->set_acceleration(THEKERNEL->config->value(checksums[a][5])->by_default(NAN)->as_number()); // mm/secs² + actuators[a]->change_steps_per_mm(THEKERNEL->config->value(motor_checksums[a][3])->by_default(a == 2 ? 2560.0F : 80.0F)->as_number()); + actuators[a]->set_max_rate(THEKERNEL->config->value(motor_checksums[a][4])->by_default(30000.0F)->as_number()/60.0F); // it is in mm/min and converted to mm/sec + actuators[a]->set_acceleration(THEKERNEL->config->value(motor_checksums[a][5])->by_default(NAN)->as_number()); // mm/secs² } check_max_actuator_speeds(); // check the configs are sane @@ -257,6 +270,16 @@ void Robot::load_config() actuators[i]->change_last_milestone(actuator_pos[i]); //this->clearToolOffset(); + + soft_endstop_enabled= THEKERNEL->config->value(soft_endstop_checksum, enable_checksum)->by_default(false)->as_bool(); + soft_endstop_halt= THEKERNEL->config->value(soft_endstop_checksum, halt_checksum)->by_default(true)->as_bool(); + + soft_endstop_min[X_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, xmin_checksum)->by_default(NAN)->as_number(); + soft_endstop_min[Y_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, ymin_checksum)->by_default(NAN)->as_number(); + soft_endstop_min[Z_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, zmin_checksum)->by_default(NAN)->as_number(); + soft_endstop_max[X_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, xmax_checksum)->by_default(NAN)->as_number(); + soft_endstop_max[Y_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, ymax_checksum)->by_default(NAN)->as_number(); + soft_endstop_max[Z_AXIS]= THEKERNEL->config->value(soft_endstop_checksum, zmax_checksum)->by_default(NAN)->as_number(); } uint8_t Robot::register_motor(StepperMotor *motor) @@ -321,26 +344,27 @@ void Robot::get_current_machine_position(float *pos) const arm_solution->actuator_to_cartesian(current_position, pos); } -int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const +void Robot::print_position(uint8_t subcode, std::string& res, bool ignore_extruders) const { // M114.1 is a new way to do this (similar to how GRBL does it). // it returns the realtime position based on the current step position of the actuators. // this does require a FK to get a machine position from the actuator position // and then invert all the transforms to get a workspace position from machine position // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position - int n = 0; + uint32_t n = 0; + char buf[64]; if(subcode == 0) { // M114 print WCS wcs_t pos= mcs2wcs(machine_position); - n = snprintf(buf, bufsize, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get(pos)), from_millimeters(std::get(pos)), from_millimeters(std::get(pos))); + n = snprintf(buf, sizeof(buf), "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get(pos)), from_millimeters(std::get(pos)), from_millimeters(std::get(pos))); } else if(subcode == 4) { // M114.4 print last milestone - n = snprintf(buf, bufsize, "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]); + n = snprintf(buf, sizeof(buf), "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position[X_AXIS], machine_position[Y_AXIS], machine_position[Z_AXIS]); } else if(subcode == 5) { // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation) // will differ from LMS by the compensation at the current position otherwise - n = snprintf(buf, bufsize, "CMP: X:%1.4f Y:%1.4f Z:%1.4f", compensated_machine_position[X_AXIS], compensated_machine_position[Y_AXIS], compensated_machine_position[Z_AXIS]); + n = snprintf(buf, sizeof(buf), "CMP: X:%1.4f Y:%1.4f Z:%1.4f", compensated_machine_position[X_AXIS], compensated_machine_position[Y_AXIS], compensated_machine_position[Z_AXIS]); } else { // get real time positions @@ -352,10 +376,10 @@ int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const if(subcode == 1) { // M114.1 print realtime WCS wcs_t pos= mcs2wcs(mpos); - n = snprintf(buf, bufsize, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get(pos)), from_millimeters(std::get(pos)), from_millimeters(std::get(pos))); + n = snprintf(buf, sizeof(buf), "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get(pos)), from_millimeters(std::get(pos)), from_millimeters(std::get(pos))); } else if(subcode == 2) { // M114.2 print realtime Machine coordinate system - n = snprintf(buf, bufsize, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]); + n = snprintf(buf, sizeof(buf), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]); } else if(subcode == 3) { // M114.3 print realtime actuator position // get real time current actuator position in mm @@ -364,25 +388,29 @@ int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position() }; - n = snprintf(buf, bufsize, "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); + n = snprintf(buf, sizeof(buf), "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); } } + if(n > sizeof(buf)) n= sizeof(buf); + res.append(buf, n); + #if MAX_ROBOT_ACTUATORS > 3 // deal with the ABC axis for (int i = A_AXIS; i < n_motors; ++i) { + n= 0; + if(ignore_extruders && actuators[i]->is_extruder()) continue; // don't show an extruder as that will be E if(subcode == 4) { // M114.4 print last milestone - n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, last_milestone[i]); + n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, machine_position[i]); - }else if(subcode == 3) { // M114.3 print actuator position + }else if(subcode == 2 || subcode == 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC // current actuator position - float current_position= actuators[i]->get_current_position(); - n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, current_position); + n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, actuators[i]->get_current_position()); } + if(n > sizeof(buf)) n= sizeof(buf); + if(n > 0) res.append(buf, n); } #endif - - return n; } // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform) @@ -400,6 +428,8 @@ Robot::wcs_t Robot::mcs2wcs(const Robot::wcs_t& pos) const void Robot::check_max_actuator_speeds() { for (size_t i = 0; i < n_motors; i++) { + if(actuators[i]->is_extruder()) continue; //extruders are not included in this check + float step_freq = actuators[i]->get_max_rate() * actuators[i]->get_steps_per_mm(); if (step_freq > THEKERNEL->base_stepping_frequency) { actuators[i]->set_max_rate(floorf(THEKERNEL->base_stepping_frequency / actuators[i]->get_steps_per_mm())); @@ -422,10 +452,18 @@ void Robot::on_gcode_received(void *argument) case 1: motion_mode = LINEAR; break; case 2: motion_mode = CW_ARC; break; case 3: motion_mode = CCW_ARC; break; - case 4: { // G4 pause + case 4: { // G4 Dwell uint32_t delay_ms = 0; if (gcode->has_letter('P')) { - delay_ms = gcode->get_int('P'); + if(THEKERNEL->is_grbl_mode()) { + // in grbl mode (and linuxcnc) P is decimal seconds + float f= gcode->get_value('P'); + delay_ms= f * 1000.0F; + + }else{ + // in reprap P is milliseconds, they always have to be different! + delay_ms = gcode->get_int('P'); + } } if (gcode->has_letter('S')) { delay_ms += gcode->get_int('S') * 1000; @@ -600,22 +638,22 @@ void Robot::on_gcode_received(void *argument) case 83: e_absolute_mode= false; break; case 92: // M92 - set steps per mm - if (gcode->has_letter('X')) - actuators[0]->change_steps_per_mm(this->to_millimeters(gcode->get_value('X'))); - if (gcode->has_letter('Y')) - actuators[1]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Y'))); - if (gcode->has_letter('Z')) - actuators[2]->change_steps_per_mm(this->to_millimeters(gcode->get_value('Z'))); - - gcode->stream->printf("X:%f Y:%f Z:%f ", actuators[0]->get_steps_per_mm(), actuators[1]->get_steps_per_mm(), actuators[2]->get_steps_per_mm()); + for (int i = 0; i < n_motors; ++i) { + if(actuators[i]->is_extruder()) continue; //extruders handle this themselves + char axis= (i <= Z_AXIS ? 'X'+i : 'A'+(i-A_AXIS)); + if(gcode->has_letter(axis)) { + actuators[i]->change_steps_per_mm(this->to_millimeters(gcode->get_value(axis))); + } + gcode->stream->printf("%c:%f ", axis, actuators[i]->get_steps_per_mm()); + } gcode->add_nl = true; check_max_actuator_speeds(); return; case 114:{ - char buf[64]; - int n= print_position(gcode->subcode, buf, sizeof buf); - if(n > 0) gcode->txt_after_ok.append(buf, n); + std::string buf; + print_position(gcode->subcode, buf, true); // ignore extruders as they will print E themselves + gcode->txt_after_ok.append(buf); return; } @@ -632,6 +670,13 @@ void Robot::on_gcode_received(void *argument) for (size_t i = X_AXIS; i <= Z_AXIS; i++) { gcode->stream->printf(" %c: %g ", 'X' + i, gcode->subcode == 0 ? this->max_speeds[i] : actuators[i]->get_max_rate()); } + if(gcode->subcode == 1) { + for (size_t i = A_AXIS; i < n_motors; i++) { + if(actuators[i]->is_extruder()) continue; //extruders handle this themselves + gcode->stream->printf(" %c: %g ", 'A' + i - A_AXIS, actuators[i]->get_max_rate()); + } + } + gcode->add_nl = true; }else{ @@ -643,6 +688,19 @@ void Robot::on_gcode_received(void *argument) } } + if(gcode->subcode == 1) { + // ABC axis only handle actuator max speeds + for (size_t i = A_AXIS; i < n_motors; i++) { + if(actuators[i]->is_extruder()) continue; //extruders handle this themselves + int c= 'A' + i - A_AXIS; + if(gcode->has_letter(c)) { + float v= gcode->get_value(c); + actuators[i]->set_max_rate(v); + } + } + } + + // this format is deprecated if(gcode->subcode == 0 && (gcode->has_letter('A') || gcode->has_letter('B') || gcode->has_letter('C'))) { gcode->stream->printf("NOTE this format is deprecated, Use M203.1 instead\n"); @@ -665,9 +723,11 @@ void Robot::on_gcode_received(void *argument) if (acc < 1.0F) acc = 1.0F; this->default_acceleration = acc; } - for (int i = X_AXIS; i <= Z_AXIS; ++i) { - if (gcode->has_letter(i+'X')) { - float acc = gcode->get_value(i+'X'); // mm/s^2 + for (int i = 0; i < n_motors; ++i) { + if(actuators[i]->is_extruder()) continue; //extruders handle this themselves + char axis= (i <= Z_AXIS ? 'X'+i : 'A'+(i-A_AXIS)); + if(gcode->has_letter(axis)) { + float acc = gcode->get_value(axis); // mm/s^2 // enforce positive if (acc <= 0.0F) acc = NAN; actuators[i]->set_acceleration(acc); @@ -699,6 +759,26 @@ void Robot::on_gcode_received(void *argument) } break; + case 211: // M211 Sn turns soft endstops on/off + if(gcode->has_letter('S')) { + soft_endstop_enabled= gcode->get_uint('S') == 1; + }else{ + gcode->stream->printf("Soft endstops are %s", soft_endstop_enabled ? "Enabled" : "Disabled"); + for (int i = X_AXIS; i <= Z_AXIS; ++i) { + if(isnan(soft_endstop_min[i])) { + gcode->stream->printf(",%c min is disabled", 'X'+i); + } + if(isnan(soft_endstop_max[i])) { + gcode->stream->printf(",%c max is disabled", 'X'+i); + } + if(!is_homed(i)) { + gcode->stream->printf(",%c axis is not homed", 'X'+i); + } + } + gcode->stream->printf("\n"); + } + break; + case 220: // M220 - speed override percentage if (gcode->has_letter('S')) { float factor = gcode->get_value('S'); @@ -721,19 +801,34 @@ void Robot::on_gcode_received(void *argument) case 500: // M500 saves some volatile settings to config override file case 503: { // M503 just prints the settings - gcode->stream->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators[0]->get_steps_per_mm(), actuators[1]->get_steps_per_mm(), actuators[2]->get_steps_per_mm()); + gcode->stream->printf(";Steps per unit:\nM92 "); + for (int i = 0; i < n_motors; ++i) { + if(actuators[i]->is_extruder()) continue; //extruders handle this themselves + char axis= (i <= Z_AXIS ? 'X'+i : 'A'+(i-A_AXIS)); + gcode->stream->printf("%c%1.5f ", axis, actuators[i]->get_steps_per_mm()); + } + gcode->stream->printf("\n"); - // only print XYZ if not NAN + // only print if not NAN gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration); - for (int i = X_AXIS; i <= Z_AXIS; ++i) { - if(!isnan(actuators[i]->get_acceleration())) gcode->stream->printf("%c%1.5f ", 'X'+i, actuators[i]->get_acceleration()); + for (int i = 0; i < n_motors; ++i) { + if(actuators[i]->is_extruder()) continue; // extruders handle this themselves + char axis= (i <= Z_AXIS ? 'X'+i : 'A'+(i-A_AXIS)); + if(!isnan(actuators[i]->get_acceleration())) gcode->stream->printf("%c%1.5f ", axis, actuators[i]->get_acceleration()); } gcode->stream->printf("\n"); gcode->stream->printf(";X- Junction Deviation, Z- Z junction deviation, S - Minimum Planner speed mm/sec:\nM205 X%1.5f Z%1.5f S%1.5f\n", THEKERNEL->planner->junction_deviation, isnan(THEKERNEL->planner->z_junction_deviation)?-1:THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed); gcode->stream->printf(";Max cartesian feedrates in mm/sec:\nM203 X%1.5f Y%1.5f Z%1.5f\n", this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]); - gcode->stream->printf(";Max actuator feedrates in mm/sec:\nM203.1 X%1.5f Y%1.5f Z%1.5f\n", actuators[X_AXIS]->get_max_rate(), actuators[Y_AXIS]->get_max_rate(), actuators[Z_AXIS]->get_max_rate()); + + gcode->stream->printf(";Max actuator feedrates in mm/sec:\nM203.1 "); + for (int i = 0; i < n_motors; ++i) { + if(actuators[i]->is_extruder()) continue; // extruders handle this themselves + char axis= (i <= Z_AXIS ? 'X'+i : 'A'+(i-A_AXIS)); + gcode->stream->printf("%c%1.5f ", axis, actuators[i]->get_max_rate()); + } + gcode->stream->printf("\n"); // get or save any arm solution specific optional values BaseSolution::arm_options_t options; @@ -873,7 +968,7 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode) } }else{ - // already in machine coordinates, we do not add tool offset for that + // already in machine coordinates, we do not add wcs or tool offset for that for(int i= X_AXIS; i <= Z_AXIS; ++i) { if(!isnan(param[i])) target[i] = param[i]; } @@ -908,7 +1003,7 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode) if(this->absolute_mode) { target[i]= p; }else{ - target[i]= p + last_milestone[i]; + target[i]= p + machine_position[i]; } } } @@ -945,6 +1040,9 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode) break; } + // needed to act as start of next arc command + memcpy(arc_milestone, target, sizeof(arc_milestone)); + if(moved) { // set machine_position to the calculated target memcpy(machine_position, target, n_motors*sizeof(float)); @@ -985,15 +1083,9 @@ void Robot::reset_axis_position(float position, int axis) #if MAX_ROBOT_ACTUATORS > 3 }else if(axis < n_motors) { -<<<<<<< HEAD - // extruders need to be set not calculated - last_machine_position[axis]= position; - actuators[axis]->change_last_milestone(position); -======= // ABC and/or extruders need to be set as there is no arm solution for them machine_position[axis]= compensated_machine_position[axis]= position; actuators[axis]->change_last_milestone(machine_position[axis]); ->>>>>>> feature/e-endstop #endif } } @@ -1065,8 +1157,43 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) compensationTransform(transformed_target, false); } + // check soft endstops only for homed axis that are enabled + if(soft_endstop_enabled) { + for (int i = 0; i <= Z_AXIS; ++i) { + if(!is_homed(i)) continue; + if( (!isnan(soft_endstop_min[i]) && transformed_target[i] < soft_endstop_min[i]) || (!isnan(soft_endstop_max[i]) && transformed_target[i] > soft_endstop_max[i]) ) { + if(soft_endstop_halt) { + if(THEKERNEL->is_grbl_mode()) { + THEKERNEL->streams->printf("error: "); + }else{ + THEKERNEL->streams->printf("Error: "); + } + + THEKERNEL->streams->printf("Soft Endstop %c was exceeded - reset or $X or M999 required\n", i+'X'); + THEKERNEL->call_event(ON_HALT, nullptr); + return false; + + //} else if(soft_endstop_truncate) { + // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge + // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries + + } else { + // ignore it + if(THEKERNEL->is_grbl_mode()) { + THEKERNEL->streams->printf("error: "); + }else{ + THEKERNEL->streams->printf("Error: "); + } + THEKERNEL->streams->printf("Soft Endstop %c was exceeded - entire move ignored\n", i+'X'); + return false; + } + } + } + } + + bool move= false; - float sos= 0; // sum of squares for just XYZ + float sos= 0; // sum of squares for just primary axis (XYZ usually) // find distance moved by each axis, use transformed target from the current compensated machine position for (size_t i = 0; i < n_motors; i++) { @@ -1074,7 +1201,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) if(deltas[i] == 0) continue; // at least one non zero delta move = true; - if(i <= Z_AXIS) { + if(i < N_PRIMARY_AXIS) { sos += powf(deltas[i], 2); } } @@ -1083,7 +1210,13 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) if(!move) return false; // see if this is a primary axis move or not - bool auxilliary_move= deltas[X_AXIS] == 0 && deltas[Y_AXIS] == 0 && deltas[Z_AXIS] == 0; + bool auxilliary_move= true; + for (int i = 0; i < N_PRIMARY_AXIS; ++i) { + if(deltas[i] != 0) { + auxilliary_move= false; + break; + } + } // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm float distance= auxilliary_move ? 0 : sqrtf(sos); @@ -1094,12 +1227,12 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) if(!auxilliary_move) { - for (size_t i = X_AXIS; i <= Z_AXIS; i++) { + for (size_t i = X_AXIS; i < N_PRIMARY_AXIS; i++) { // find distance unit vector for primary axis only unit_vec[i] = deltas[i] / distance; // Do not move faster than the configured cartesian limits for XYZ - if ( max_speeds[i] > 0 ) { + if ( i <= Z_AXIS && max_speeds[i] > 0 ) { float axis_speed = fabsf(unit_vec[i] * rate_mm_s); if (axis_speed > max_speeds[i]) @@ -1134,7 +1267,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) } if(auxilliary_move) { // for E only moves we need to use the scaled E to calculate the distance - sos += pow(actuator_pos[i] - actuators[i]->get_last_milestone(), 2); + sos += powf(actuator_pos[i] - actuators[i]->get_last_milestone(), 2); } } if(auxilliary_move) { @@ -1161,7 +1294,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move // TODO we may need to do all of them, check E won't limit XYZ.. it does on long E moves, but not checking it could exceed the E acceleration. - if(auxilliary_move || actuator <= Z_AXIS) { + if(auxilliary_move || actuator < N_PRIMARY_AXIS) { float ma = actuators[actuator]->get_acceleration(); // in mm/sec² if(!isnan(ma)) { // if axis does not have acceleration set then it uses the default_acceleration float ca = fabsf((d/distance) * acceleration); @@ -1172,8 +1305,16 @@ bool Robot::append_milestone(const float target[], float rate_mm_s) } } + // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause + while(THEKERNEL->get_feed_hold()) { + THEKERNEL->call_event(ON_IDLE, this); + // if we also got a HALT then break out of this + if(THEKERNEL->is_halted()) return false; + } + // Append the block to the planner // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move + // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration, s_value, is_g123)) { // this is the new compensated machine position memcpy(this->compensated_machine_position, transformed_target, n_motors*sizeof(float)); @@ -1283,10 +1424,11 @@ bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, flo // We always add another point after this loop so we stop at segments-1, ie i < segments for (int i = 1; i < segments; i++) { if(THEKERNEL->is_halted()) return false; // don't queue any more segments - for (int i = 0; i < n_motors; i++) - segment_end[i] += segment_delta[i]; + for (int j = 0; j < n_motors; j++) + segment_end[j] += segment_delta[j]; // Append the end of this segment to the queue + // this can block waiting for free block queue or if in feed hold bool b= this->append_milestone(segment_end, rate_mm_s); moved= moved || b; } @@ -1313,29 +1455,41 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[] return false; } - // Scary math - float center_axis0 = this->machine_position[this->plane_axis_0] + offset[this->plane_axis_0]; - float center_axis1 = this->machine_position[this->plane_axis_1] + offset[this->plane_axis_1]; - float linear_travel = target[this->plane_axis_2] - this->machine_position[this->plane_axis_2]; - float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location + // Scary math. + // We need to use arc_milestone here to get accurate arcs as previous machine_position may have been skipped due to small movements + float center_axis0 = this->arc_milestone[this->plane_axis_0] + offset[this->plane_axis_0]; + float center_axis1 = this->arc_milestone[this->plane_axis_1] + offset[this->plane_axis_1]; + float linear_travel = target[this->plane_axis_2] - this->arc_milestone[this->plane_axis_2]; + float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to start position float r_axis1 = -offset[this->plane_axis_1]; - float rt_axis0 = target[this->plane_axis_0] - center_axis0; - float rt_axis1 = target[this->plane_axis_1] - center_axis1; - - // Patch from GRBL Firmware - Christoph Baumann 04072015 - // CCW angle between position and target from circle center. Only one atan2() trig computation required. - float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); - if (is_clockwise) { // Correct atan2 output per direction - if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); } + float rt_axis0 = target[this->plane_axis_0] - this->arc_milestone[this->plane_axis_0] - offset[this->plane_axis_0]; // Radius vector from center to target position + float rt_axis1 = target[this->plane_axis_1] - this->arc_milestone[this->plane_axis_1] - offset[this->plane_axis_1]; + float angular_travel = 0; + //check for condition where atan2 formula will fail due to everything canceling out exactly + if((this->arc_milestone[this->plane_axis_0]==target[this->plane_axis_0]) && (this->arc_milestone[this->plane_axis_1]==target[this->plane_axis_1])) { + if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle + angular_travel = (-2 * PI); + } else { // set angular_travel to 2pi for a counterclockwise full circle + angular_travel = (2 * PI); + } } else { - if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); } + // Patch from GRBL Firmware - Christoph Baumann 04072015 + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + // Only run if not a full circle or angular travel will incorrectly result in 0.0f + angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes + if (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs + if (angular_travel > 0) { angular_travel -= (2 * PI); } + } else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs + if (angular_travel < 0) { angular_travel += (2 * PI); } + } } // Find the distance for this gcode float millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel)); // We don't care about non-XYZ moves ( for example the extruder produces some of those ) - if( millimeters_of_travel < 0.00001F ) { + if( millimeters_of_travel < 0.000001F ) { return false; } @@ -1347,79 +1501,90 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[] arc_segment = min_err_segment; } } + + // catch fall through on above + if(arc_segment < 0.0001F) { + arc_segment= 0.5F; /// the old default, so we avoid the divide by zero + } + // Figure out how many segments for this gcode // TODO for deltas we need to make sure we are at least as many segments as requested, also if mm_per_line_segment is set we need to use the - uint16_t segments = ceilf(millimeters_of_travel / arc_segment); - - //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY - float theta_per_segment = angular_travel / segments; - float linear_per_segment = linear_travel / segments; - - /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, - and phi is the angle of rotation. Based on the solution approach by Jens Geisler. - r_T = [cos(phi) -sin(phi); - sin(phi) cos(phi] * r ; - For arc generation, the center of the circle is the axis of rotation and the radius vector is - defined from the circle center to the initial position. Each line segment is formed by successive - vector rotations. This requires only two cos() and sin() computations to form the rotation - matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since - all float numbers are single precision on the Arduino. (True float precision will not have - round off issues for CNC applications.) Single precision error can accumulate to be greater than - tool precision in some cases. Therefore, arc path correction is implemented. - - Small angle approximation may be used to reduce computation overhead further. This approximation - holds for everything, but very small circles and large mm_per_arc_segment values. In other words, - theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large - to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for - numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an - issue for CNC machines with the single precision Arduino calculations. - This approximation also allows mc_arc to immediately insert a line segment into the planner - without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied - a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. - This is important when there are successive arc motions. - */ - // Vector rotation matrix values - float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation - float sin_T = theta_per_segment; - - float arc_target[3]; - float sin_Ti; - float cos_Ti; - float r_axisi; - uint16_t i; - int8_t count = 0; + uint16_t segments = floorf(millimeters_of_travel / arc_segment); + bool moved= false; - // Initialize the linear axis - arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2]; + if(segments > 1) { + float theta_per_segment = angular_travel / segments; + float linear_per_segment = linear_travel / segments; + + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Based on the solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. This requires only two cos() and sin() computations to form the rotation + matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + all float numbers are single precision on the Arduino. (True float precision will not have + round off issues for CNC applications.) Single precision error can accumulate to be greater than + tool precision in some cases. Therefore, arc path correction is implemented. + + Small angle approximation may be used to reduce computation overhead further. This approximation + holds for everything, but very small circles and large mm_per_arc_segment values. In other words, + theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large + to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for + numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an + issue for CNC machines with the single precision Arduino calculations. + This approximation also allows mc_arc to immediately insert a line segment into the planner + without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied + a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + float cos_T = 1 - 0.5F * theta_per_segment * theta_per_segment; // Small angle approximation + float sin_T = theta_per_segment; + + // TODO we need to handle the ABC axis here by segmenting them + float arc_target[n_motors]; + float sin_Ti; + float cos_Ti; + float r_axisi; + uint16_t i; + int8_t count = 0; + + // init array for all axis + memcpy(arc_target, machine_position, n_motors*sizeof(float)); + + // Initialize the linear axis + arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2]; + + for (i = 1; i < segments; i++) { // Increment (segments-1) + if(THEKERNEL->is_halted()) return false; // don't queue any more segments - bool moved= false; - for (i = 1; i < segments; i++) { // Increment (segments-1) - if(THEKERNEL->is_halted()) return false; // don't queue any more segments - - if (count < this->arc_correction ) { - // Apply vector rotation matrix - r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; - r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; - r_axis1 = r_axisi; - count++; - } else { - // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. - // Compute exact location by applying transformation matrix from initial radius vector(=-offset). - cos_Ti = cosf(i * theta_per_segment); - sin_Ti = sinf(i * theta_per_segment); - r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti; - r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti; - count = 0; - } + if (count < this->arc_correction ) { + // Apply vector rotation matrix + r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; + r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; + r_axis1 = r_axisi; + count++; + } else { + // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. + // Compute exact location by applying transformation matrix from initial radius vector(=-offset). + cos_Ti = cosf(i * theta_per_segment); + sin_Ti = sinf(i * theta_per_segment); + r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti; + r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti; + count = 0; + } - // Update arc_target location - arc_target[this->plane_axis_0] = center_axis0 + r_axis0; - arc_target[this->plane_axis_1] = center_axis1 + r_axis1; - arc_target[this->plane_axis_2] += linear_per_segment; + // Update arc_target location + arc_target[this->plane_axis_0] = center_axis0 + r_axis0; + arc_target[this->plane_axis_1] = center_axis1 + r_axis1; + arc_target[this->plane_axis_2] += linear_per_segment; - // Append this segment to the queue - bool b= this->append_milestone(arc_target, rate_mm_s); - moved= moved || b; + // Append this segment to the queue + bool b= this->append_milestone(arc_target, rate_mm_s); + moved= moved || b; + } } // Ensure last segment arrives at target location. @@ -1481,3 +1646,19 @@ float Robot::get_feed_rate() const { return THEKERNEL->gcode_dispatch->get_modal_command() == 0 ? seek_rate : feed_rate; } + +bool Robot::is_homed(uint8_t i) const +{ + if(i >= 3) return false; // safety + + // if we are homing we ignore soft endstops so return false + bool homing; + bool ok = PublicData::get_value(endstops_checksum, get_homing_status_checksum, 0, &homing); + if(!ok || homing) return false; + + // check individual axis homing status + bool homed[3]; + ok = PublicData::get_value(endstops_checksum, get_homed_status_checksum, 0, homed); + if(!ok) return false; + return homed[i]; +}