#include "libs/Module.h"
#include "libs/Kernel.h"
-#include "mbed.h" // for us_ticker_read()
-
-#include <fastmath.h>
-#include <string>
-using std::string;
-
+#include "Robot.h"
#include "Planner.h"
#include "Conveyor.h"
-#include "Robot.h"
-#include "nuts_bolts.h"
#include "Pin.h"
#include "StepperMotor.h"
#include "Gcode.h"
#include "StreamOutputPool.h"
#include "ExtruderPublicAccess.h"
#include "GcodeDispatch.h"
+#include "ActuatorCoordinates.h"
+#include "mbed.h" // for us_ticker_read()
+#include "mri.h"
+
+#include <fastmath.h>
+#include <string>
+#include <algorithm>
+using std::string;
#define default_seek_rate_checksum CHECKSUM("default_seek_rate")
#define default_feed_rate_checksum CHECKSUM("default_feed_rate")
#define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
#define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
#define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
+#define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
#define arc_correction_checksum CHECKSUM("arc_correction")
#define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
#define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
#define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
#define segment_z_moves_checksum CHECKSUM("segment_z_moves")
#define save_g92_checksum CHECKSUM("save_g92")
+#define set_g92_checksum CHECKSUM("set_g92")
// arm solutions
#define arm_solution_checksum CHECKSUM("arm_solution")
#define steps_per_mm_checksum CHECKSUM("steps_per_mm")
#define max_rate_checksum CHECKSUM("max_rate")
+#define acceleration_checksum CHECKSUM("acceleration")
+#define z_acceleration_checksum CHECKSUM("z_acceleration")
#define alpha_checksum CHECKSUM("alpha")
#define beta_checksum CHECKSUM("beta")
#define gamma_checksum CHECKSUM("gamma")
-#define NEXT_ACTION_DEFAULT 0
-#define NEXT_ACTION_DWELL 1
-#define NEXT_ACTION_GO_HOME 2
-
-#define MOTION_MODE_SEEK 0 // G0
-#define MOTION_MODE_LINEAR 1 // G1
-#define MOTION_MODE_CW_ARC 2 // G2
-#define MOTION_MODE_CCW_ARC 3 // G3
-#define MOTION_MODE_CANCEL 4 // G80
-
-#define PATH_CONTROL_MODE_EXACT_PATH 0
-#define PATH_CONTROL_MODE_EXACT_STOP 1
-#define PATH_CONTROL_MODE_CONTINOUS 2
-
-#define PROGRAM_FLOW_RUNNING 0
-#define PROGRAM_FLOW_PAUSED 1
-#define PROGRAM_FLOW_COMPLETED 2
+#define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
-#define SPINDLE_DIRECTION_CW 0
-#define SPINDLE_DIRECTION_CCW 1
-
-#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7 // Float (radians)
+#define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
+#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
// It takes care of cutting arcs into segments, same thing for line that are too long
{
this->inch_mode = false;
this->absolute_mode = true;
- this->motion_mode = MOTION_MODE_SEEK;
+ this->e_absolute_mode = true;
this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
- clear_vector(this->last_milestone);
- clear_vector(this->last_machine_position);
+ memset(this->last_milestone, 0, sizeof last_milestone);
+ memset(this->last_machine_position, 0, sizeof last_machine_position);
this->arm_solution = NULL;
seconds_per_minute = 60.0F;
this->clearToolOffset();
this->compensationTransform = nullptr;
+ this->get_e_scale_fnc= nullptr;
this->wcs_offsets.fill(wcs_t(0.0F, 0.0F, 0.0F));
this->g92_offset = wcs_t(0.0F, 0.0F, 0.0F);
this->next_command_is_MCS = false;
this->disable_segmentation= false;
+ this->disable_arm_solution= false;
+ this->n_motors= 0;
}
//Called when the module has just been loaded
CHECKSUM(X "_dir_pin"), \
CHECKSUM(X "_en_pin"), \
CHECKSUM(X "_steps_per_mm"), \
- CHECKSUM(X "_max_rate") \
+ CHECKSUM(X "_max_rate"), \
+ CHECKSUM(X "_acceleration") \
}
void Robot::load_config()
this->seek_rate = THEKERNEL->config->value(default_seek_rate_checksum )->by_default( 100.0F)->as_number();
this->mm_per_line_segment = THEKERNEL->config->value(mm_per_line_segment_checksum )->by_default( 0.0F)->as_number();
this->delta_segments_per_second = THEKERNEL->config->value(delta_segments_per_second_checksum )->by_default(0.0f )->as_number();
- this->mm_per_arc_segment = THEKERNEL->config->value(mm_per_arc_segment_checksum )->by_default( 0.5f)->as_number();
+ this->mm_per_arc_segment = THEKERNEL->config->value(mm_per_arc_segment_checksum )->by_default( 0.0f)->as_number();
+ this->mm_max_arc_error = THEKERNEL->config->value(mm_max_arc_error_checksum )->by_default( 0.01f)->as_number();
this->arc_correction = THEKERNEL->config->value(arc_correction_checksum )->by_default( 5 )->as_number();
+ // in mm/sec but specified in config as mm/min
this->max_speeds[X_AXIS] = THEKERNEL->config->value(x_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Y_AXIS] = THEKERNEL->config->value(y_axis_max_speed_checksum )->by_default(60000.0F)->as_number() / 60.0F;
this->max_speeds[Z_AXIS] = THEKERNEL->config->value(z_axis_max_speed_checksum )->by_default( 300.0F)->as_number() / 60.0F;
this->segment_z_moves = THEKERNEL->config->value(segment_z_moves_checksum )->by_default(true)->as_bool();
this->save_g92 = THEKERNEL->config->value(save_g92_checksum )->by_default(false)->as_bool();
+ string g92 = THEKERNEL->config->value(set_g92_checksum )->by_default("")->as_string();
+ if(!g92.empty()) {
+ // optional setting for a fixed G92 offset
+ std::vector<float> t= parse_number_list(g92.c_str());
+ if(t.size() == 3) {
+ g92_offset = wcs_t(t[0], t[1], t[2]);
+ }
+ }
- // Make our 3 StepperMotors
- uint16_t const checksums[][5] = {
- ACTUATOR_CHECKSUMS("alpha"),
- ACTUATOR_CHECKSUMS("beta"),
- ACTUATOR_CHECKSUMS("gamma"),
-#if MAX_ROBOT_ACTUATORS > 3
- ACTUATOR_CHECKSUMS("delta"),
- ACTUATOR_CHECKSUMS("epsilon"),
- ACTUATOR_CHECKSUMS("zeta")
-#endif
+ // default s value for laser
+ this->s_value = THEKERNEL->config->value(laser_module_default_power_checksum)->by_default(0.8F)->as_number();
+
+ // Make our Primary XYZ StepperMotors
+ uint16_t const checksums[][6] = {
+ ACTUATOR_CHECKSUMS("alpha"), // X
+ ACTUATOR_CHECKSUMS("beta"), // Y
+ ACTUATOR_CHECKSUMS("gamma"), // Z
};
- constexpr size_t actuator_checksum_count = sizeof(checksums) / sizeof(checksums[0]);
- static_assert(actuator_checksum_count >= k_max_actuators, "Robot checksum array too small for k_max_actuators");
- size_t motor_count = std::min(this->arm_solution->get_actuator_count(), k_max_actuators);
- for (size_t a = 0; a < motor_count; a++) {
+ // default acceleration setting, can be overriden with newer per axis settings
+ this->default_acceleration= THEKERNEL->config->value(acceleration_checksum)->by_default(100.0F )->as_number(); // Acceleration is in mm/s^2
+
+ // make each motor
+ for (size_t a = X_AXIS; a <= Z_AXIS; 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();
}
- actuators[a] = new StepperMotor(pins[0], pins[1], pins[2]);
+ StepperMotor *sm = new StepperMotor(pins[0], pins[1], pins[2]);
+ // register this motor (NB This must be 0,1,2) of the actuators array
+ uint8_t n= register_motor(sm);
+ if(n != a) {
+ // this is a fatal error
+ THEKERNEL->streams->printf("FATAL: motor %d does not match index %d\n", n, a);
+ __debugbreak();
+ }
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());
+ 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²
}
check_max_actuator_speeds(); // check the configs are sane
+ // if we have not specified a z acceleration see if the legacy config was set
+ if(isnan(actuators[Z_AXIS]->get_acceleration())) {
+ float acc= THEKERNEL->config->value(z_acceleration_checksum)->by_default(NAN)->as_number(); // disabled by default
+ if(!isnan(acc)) {
+ actuators[Z_AXIS]->set_acceleration(acc);
+ }
+ }
+
// initialise actuator positions to current cartesian position (X0 Y0 Z0)
// so the first move can be correct if homing is not performed
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
- for (size_t i = 0; i < actuators.size(); i++)
+ for (size_t i = 0; i < n_motors; i++)
actuators[i]->change_last_milestone(actuator_pos[i]);
//this->clearToolOffset();
}
+uint8_t Robot::register_motor(StepperMotor *motor)
+{
+ // register this motor with the step ticker
+ THEKERNEL->step_ticker->register_motor(motor);
+ if(n_motors >= k_max_actuators) {
+ // this is a fatal error
+ THEKERNEL->streams->printf("FATAL: too many motors, increase k_max_actuators\n");
+ __debugbreak();
+ }
+ actuators.push_back(motor);
+ return n_motors++;
+}
+
void Robot::push_state()
{
bool am = this->absolute_mode;
+ bool em = this->e_absolute_mode;
bool im = this->inch_mode;
- saved_state_t s(this->feed_rate, this->seek_rate, am, im, current_wcs);
+ saved_state_t s(this->feed_rate, this->seek_rate, am, em, im, current_wcs);
state_stack.push(s);
}
this->feed_rate = std::get<0>(s);
this->seek_rate = std::get<1>(s);
this->absolute_mode = std::get<2>(s);
- this->inch_mode = std::get<3>(s);
- this->current_wcs = std::get<4>(s);
+ this->e_absolute_mode = std::get<3>(s);
+ this->inch_mode = std::get<4>(s);
+ this->current_wcs = std::get<5>(s);
}
}
return v;
}
+void Robot::get_current_machine_position(float *pos) const
+{
+ // get real time current actuator position in mm
+ ActuatorCoordinates current_position{
+ actuators[X_AXIS]->get_current_position(),
+ actuators[Y_AXIS]->get_current_position(),
+ actuators[Z_AXIS]->get_current_position()
+ };
+
+ // get machine position from the actuator position using FK
+ arm_solution->actuator_to_cartesian(current_position, pos);
+}
+
int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) 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 last_milestone and does inversse transforms to get the requested position
+ // M114 just does it the old way uses last_milestone and does inverse transforms to get the requested position
int n = 0;
if(subcode == 0) { // M114 print WCS
wcs_t pos= mcs2wcs(last_milestone);
n = snprintf(buf, bufsize, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
- } else if(subcode == 4) { // M114.4 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
+ } else if(subcode == 4) {
+ // M114.4 print last milestone
n = snprintf(buf, bufsize, "LMS: X:%1.4f Y:%1.4f Z:%1.4f", last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[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)
+ } 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, "LMP: X:%1.4f Y:%1.4f Z:%1.4f", last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
} else {
// get real time positions
- // current actuator position in mm
- ActuatorCoordinates current_position{
- actuators[X_AXIS]->get_current_position(),
- actuators[Y_AXIS]->get_current_position(),
- actuators[Z_AXIS]->get_current_position()
- };
-
- // get machine position from the actuator position using FK
float mpos[3];
- arm_solution->actuator_to_cartesian(current_position, mpos);
+ get_current_machine_position(mpos);
+
+ // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
+ if(compensationTransform) compensationTransform(mpos, true); // get inverse compensation transform
if(subcode == 1) { // M114.1 print realtime WCS
- // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
wcs_t pos= mcs2wcs(mpos);
- n = snprintf(buf, bufsize, "C: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
+ n = snprintf(buf, bufsize, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
} else if(subcode == 2) { // M114.2 print realtime Machine coordinate system
- n = snprintf(buf, bufsize, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
+ n = snprintf(buf, bufsize, "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
- n = snprintf(buf, bufsize, "APOS: A:%1.4f B:%1.4f C:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
+ // get real time current actuator position in mm
+ ActuatorCoordinates current_position{
+ actuators[X_AXIS]->get_current_position(),
+ 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]);
}
}
return n;
// we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
void Robot::check_max_actuator_speeds()
{
- for (size_t i = 0; i < actuators.size(); i++) {
+ for (size_t i = 0; i < n_motors; i++) {
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()));
- THEKERNEL->streams->printf("WARNING: actuator %c rate exceeds base_stepping_frequency * alpha_steps_per_mm: %f, setting to %f\n", 'A' + i, step_freq, actuators[i]->max_rate);
+ THEKERNEL->streams->printf("WARNING: actuator %d rate exceeds base_stepping_frequency * ..._steps_per_mm: %f, setting to %f\n", i, step_freq, actuators[i]->get_max_rate());
}
}
}
{
Gcode *gcode = static_cast<Gcode *>(argument);
- this->motion_mode = -1;
+ enum MOTION_MODE_T motion_mode= NONE;
if( gcode->has_g) {
switch( gcode->g ) {
- case 0: this->motion_mode = MOTION_MODE_SEEK; break;
- case 1: this->motion_mode = MOTION_MODE_LINEAR; break;
- case 2: this->motion_mode = MOTION_MODE_CW_ARC; break;
- case 3: this->motion_mode = MOTION_MODE_CCW_ARC; break;
+ case 0: motion_mode = SEEK; break;
+ case 1: motion_mode = LINEAR; break;
+ case 2: motion_mode = CW_ARC; break;
+ case 3: motion_mode = CCW_ARC; break;
case 4: { // G4 pause
uint32_t delay_ms = 0;
if (gcode->has_letter('P')) {
}
if (delay_ms > 0) {
// drain queue
- THEKERNEL->conveyor->wait_for_empty_queue();
+ THEKERNEL->conveyor->wait_for_idle();
// wait for specified time
uint32_t start = us_ticker_read(); // mbed call
while ((us_ticker_read() - start) < delay_ms * 1000) {
}
break;
- case 90: this->absolute_mode = true; break;
- case 91: this->absolute_mode = false; break;
+ case 90: this->absolute_mode = true; this->e_absolute_mode = true; break;
+ case 91: this->absolute_mode = false; this->e_absolute_mode = false; break;
case 92: {
if(gcode->subcode == 1 || gcode->subcode == 2 || gcode->get_num_args() == 0) {
g92_offset = wcs_t(x, y, z);
}
+ #if MAX_ROBOT_ACTUATORS > 3
+ if(gcode->subcode == 0 && (gcode->has_letter('E') || gcode->get_num_args() == 0)){
+ // reset the E position, legacy for 3d Printers to be reprap compatible
+ // find the selected extruder
+ // NOTE this will only work when E is 0 if volumetric and/or scaling is used as the actuator last milestone will be different if it was scaled
+ for (int i = E_AXIS; i < n_motors; ++i) {
+ if(actuators[i]->is_selected()) {
+ float e= gcode->has_letter('E') ? gcode->get_value('E') : 0;
+ last_milestone[i]= last_machine_position[i]= e;
+ actuators[i]->change_last_milestone(e);
+ break;
+ }
+ }
+ }
+ #endif
+
return;
}
}
case 18: // this used to support parameters, now it ignores them
case 84:
- THEKERNEL->conveyor->wait_for_empty_queue();
+ THEKERNEL->conveyor->wait_for_idle();
THEKERNEL->call_event(ON_ENABLE, nullptr); // turn all enable pins off
break;
+ case 82: e_absolute_mode= true; break;
+ 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('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]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
+ 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());
gcode->add_nl = true;
check_max_actuator_speeds();
return;
pop_state();
break;
- case 203: // M203 Set maximum feedrates in mm/sec
- if (gcode->has_letter('X'))
- this->max_speeds[X_AXIS] = gcode->get_value('X');
- if (gcode->has_letter('Y'))
- this->max_speeds[Y_AXIS] = gcode->get_value('Y');
- if (gcode->has_letter('Z'))
- this->max_speeds[Z_AXIS] = gcode->get_value('Z');
- for (size_t i = 0; i < 3 && i < actuators.size(); i++) {
- if (gcode->has_letter('A' + i))
- actuators[i]->set_max_rate(gcode->get_value('A' + i));
- }
- check_max_actuator_speeds();
+ case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
+ if(gcode->get_num_args() == 0) {
+ 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());
+ }
+ gcode->add_nl = true;
+
+ }else{
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+ if (gcode->has_letter('X' + i)) {
+ float v= gcode->get_value('X'+i);
+ if(gcode->subcode == 0) this->max_speeds[i]= v;
+ else if(gcode->subcode == 1) actuators[i]->set_max_rate(v);
+ }
+ }
- if(gcode->get_num_args() == 0) {
- gcode->stream->printf("X:%g Y:%g Z:%g",
- this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]);
- for (size_t i = 0; i < 3 && i < actuators.size(); i++) {
- gcode->stream->printf(" %c : %g", 'A' + i, actuators[i]->get_max_rate()); //xxx
+ // 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");
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+ if (gcode->has_letter('A' + i)) {
+ float v= gcode->get_value('A'+i);
+ actuators[i]->set_max_rate(v);
+ }
+ }
+ }
+
+ if(gcode->subcode == 1) check_max_actuator_speeds();
}
- gcode->add_nl = true;
- }
- break;
+ break;
- case 204: // M204 Snnn - set acceleration to nnn, Znnn sets z acceleration
+ case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
if (gcode->has_letter('S')) {
float acc = gcode->get_value('S'); // mm/s^2
// enforce minimum
- if (acc < 1.0F)
- acc = 1.0F;
- THEKERNEL->planner->acceleration = acc;
+ if (acc < 1.0F) acc = 1.0F;
+ this->default_acceleration = acc;
}
- if (gcode->has_letter('Z')) {
- float acc = gcode->get_value('Z'); // mm/s^2
- // enforce positive
- if (acc < 0.0F)
- acc = 0.0F;
- THEKERNEL->planner->z_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
+ // enforce positive
+ if (acc <= 0.0F) acc = NAN;
+ actuators[i]->set_acceleration(acc);
+ }
}
break;
- case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
+ case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
if (gcode->has_letter('X')) {
float jd = gcode->get_value('X');
// enforce minimum
if (gcode->has_letter('Z')) {
float jd = gcode->get_value('Z');
// enforce minimum, -1 disables it and uses regular junction deviation
- if (jd < -1.0F)
- jd = -1.0F;
+ if (jd <= -1.0F)
+ jd = NAN;
THEKERNEL->planner->z_junction_deviation = jd;
}
if (gcode->has_letter('S')) {
mps = 0.0F;
THEKERNEL->planner->minimum_planner_speed = mps;
}
- if (gcode->has_letter('Y')) {
- actuators[0]->default_minimum_actuator_rate = gcode->get_value('Y');
- }
break;
case 220: // M220 - speed override percentage
break;
case 400: // wait until all moves are done up to this point
- THEKERNEL->conveyor->wait_for_empty_queue();
+ THEKERNEL->conveyor->wait_for_idle();
break;
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]->steps_per_mm, actuators[1]->steps_per_mm, actuators[2]->steps_per_mm);
- gcode->stream->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", THEKERNEL->planner->acceleration, THEKERNEL->planner->z_acceleration);
- 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, THEKERNEL->planner->z_junction_deviation, THEKERNEL->planner->minimum_planner_speed);
- gcode->stream->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f",
- this->max_speeds[X_AXIS], this->max_speeds[Y_AXIS], this->max_speeds[Z_AXIS]);
- for (size_t i = 0; i < 3 && i < actuators.size(); i++) {
- gcode->stream->printf(" %c%1.5f", 'A' + i, actuators[i]->get_max_rate());
+ 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());
+
+ // only print XYZ 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());
}
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());
+
// get or save any arm solution specific optional values
BaseSolution::arm_options_t options;
if(arm_solution->get_optional(options) && !options.empty()) {
}
}
- if( this->motion_mode >= 0) {
- process_move(gcode);
+ if( motion_mode != NONE) {
+ is_g123= motion_mode != SEEK;
+ process_move(gcode, motion_mode);
+
+ }else{
+ is_g123= false;
}
next_command_is_MCS = false; // must be on same line as G0 or G1
}
// process a G0/G1/G2/G3
-void Robot::process_move(Gcode *gcode)
+void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode)
{
// we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
- float param[3]{NAN, NAN, NAN};
+ // get XYZ and one E (which goes to the selected extruder)
+ float param[4]{NAN, NAN, NAN, NAN};
+
+ // process primary axis
for(int i= X_AXIS; i <= Z_AXIS; ++i) {
char letter= 'X'+i;
if( gcode->has_letter(letter) ) {
}
// calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
- float target[3]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
+ float target[n_motors];
+ memcpy(target, last_milestone, n_motors*sizeof(float));
+
if(!next_command_is_MCS) {
if(this->absolute_mode) {
// apply wcs offsets and g92 offset and tool offset
}
}
+ // process extruder parameters, for active extruder only (only one active extruder at a time)
+ selected_extruder= 0;
+ if(gcode->has_letter('E')) {
+ for (int i = E_AXIS; i < n_motors; ++i) {
+ // find first selected extruder
+ if(actuators[i]->is_selected()) {
+ param[E_AXIS]= gcode->get_value('E');
+ selected_extruder= i;
+ break;
+ }
+ }
+ }
+
+ // do E for the selected extruder
+ float delta_e= NAN;
+ if(selected_extruder > 0 && !isnan(param[E_AXIS])) {
+ if(this->e_absolute_mode) {
+ target[selected_extruder]= param[E_AXIS];
+ delta_e= target[selected_extruder] - last_milestone[selected_extruder];
+ }else{
+ delta_e= param[E_AXIS];
+ target[selected_extruder] = delta_e + last_milestone[selected_extruder];
+ }
+ }
+
if( gcode->has_letter('F') ) {
- if( this->motion_mode == MOTION_MODE_SEEK )
+ if( motion_mode == SEEK )
this->seek_rate = this->to_millimeters( gcode->get_value('F') );
else
this->feed_rate = this->to_millimeters( gcode->get_value('F') );
}
+ // S is modal When specified on a G0/1/2/3 command
+ if(gcode->has_letter('S')) s_value= gcode->get_value('S');
+
bool moved= false;
- //Perform any physical actions
- switch(this->motion_mode) {
- case MOTION_MODE_CANCEL:
- break;
- case MOTION_MODE_SEEK:
- moved= this->append_line(gcode, target, this->seek_rate / seconds_per_minute );
+
+ // Perform any physical actions
+ switch(motion_mode) {
+ case NONE: break;
+
+ case SEEK:
+ moved= this->append_line(gcode, target, this->seek_rate / seconds_per_minute, delta_e );
break;
- case MOTION_MODE_LINEAR:
- moved= this->append_line(gcode, target, this->feed_rate / seconds_per_minute );
+
+ case LINEAR:
+ moved= this->append_line(gcode, target, this->feed_rate / seconds_per_minute, delta_e );
break;
- case MOTION_MODE_CW_ARC:
- case MOTION_MODE_CCW_ARC:
- moved= this->compute_arc(gcode, offset, target );
+
+ case CW_ARC:
+ case CCW_ARC:
+ // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
+ moved= this->compute_arc(gcode, offset, target, motion_mode);
break;
}
if(moved) {
// set last_milestone to the calculated target
- memcpy(this->last_milestone, target, sizeof(this->last_milestone));
+ memcpy(last_milestone, target, n_motors*sizeof(float));
}
}
-// We received a new gcode, and one of the functions
-// determined the distance for that given gcode. So now we can attach this gcode to the right block
-// and continue
-void Robot::distance_in_gcode_is_known(Gcode * gcode)
-{
- //If the queue is empty, execute immediately, otherwise attach to the last added block
- //THEKERNEL->conveyor->append_gcode(gcode);
-}
-
// reset the machine position for all axis. Used for homing.
-// During homing compensation is turned off (actually not used as it drives steppers directly)
-// once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
-// so in those cases the final position is compensated.
+// after homing we supply the cartesian coordinates that the head is at when homed,
+// however for Z this is the compensated Z position (if enabled)
+// So we need to apply the inverse compensation transform to the supplied coordinates to get the correct last milestone
+// this will make the results from M114 and ? consistent after homing.
void Robot::reset_axis_position(float x, float y, float z)
{
- // these are set to the same as compensation was not used to get to the current position
+ // set both the same initially
last_machine_position[X_AXIS]= last_milestone[X_AXIS] = x;
last_machine_position[Y_AXIS]= last_milestone[Y_AXIS] = y;
last_machine_position[Z_AXIS]= last_milestone[Z_AXIS] = z;
- // now set the actuator positions to match
+ if(compensationTransform) {
+ // apply inverse transform to get last_milestone
+ compensationTransform(last_milestone, true);
+ }
+
+ // now set the actuator positions based on the supplied compensated position
ActuatorCoordinates actuator_pos;
arm_solution->cartesian_to_actuator(this->last_machine_position, actuator_pos);
- for (size_t i = 0; i < actuators.size(); i++)
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++)
actuators[i]->change_last_milestone(actuator_pos[i]);
}
-// Reset the position for an axis (used in homing)
+// Reset the position for an axis (used in homing, and to reset extruder after suspend)
void Robot::reset_axis_position(float position, int axis)
{
- last_milestone[axis] = position;
- reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
+ last_machine_position[axis] = position;
+ if(axis <= Z_AXIS) {
+ reset_axis_position(last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
+#if MAX_ROBOT_ACTUATORS > 3
+ }else{
+ // extruders need to be set not calculated
+ last_machine_position[axis]= position;
+#endif
+ }
}
// similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
-// then sets the axis positions to match. currently only called from Endstops.cpp
+// then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
void Robot::reset_actuator_position(const ActuatorCoordinates &ac)
{
- for (size_t i = 0; i < actuators.size(); i++)
- actuators[i]->change_last_milestone(ac[i]);
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+ if(!isnan(ac[i])) actuators[i]->change_last_milestone(ac[i]);
+ }
// now correct axis positions then recorrect actuator to account for rounding
reset_position_from_current_actuator_position();
void Robot::reset_position_from_current_actuator_position()
{
ActuatorCoordinates actuator_pos;
- for (size_t i = 0; i < actuators.size(); i++) {
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
// NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
actuator_pos[i] = actuators[i]->get_current_position();
}
// discover machine position from where actuators actually are
arm_solution->actuator_to_cartesian(actuator_pos, last_machine_position);
- // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
memcpy(last_milestone, last_machine_position, sizeof last_milestone);
+ // last_machine_position includes the compensation transform so we need to get the inverse to get actual last_milestone
+ if(compensationTransform) compensationTransform(last_milestone, true); // get inverse compensation transform
+
// now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
// NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
// to get everything in perfect sync.
arm_solution->cartesian_to_actuator(last_machine_position, actuator_pos);
- for (size_t i = 0; i < actuators.size(); i++)
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++)
actuators[i]->change_last_milestone(actuator_pos[i]);
}
-// Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
+// Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
// target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
// all transforms and is what we actually convert to actuator positions
-bool Robot::append_milestone(Gcode * gcode, const float target[], float rate_mm_s)
+bool Robot::append_milestone(const float target[], float rate_mm_s)
{
- float deltas[3];
- float unit_vec[3];
- ActuatorCoordinates actuator_pos;
- float transformed_target[3]; // adjust target for bed compensation and WCS offsets
- float millimeters_of_travel;
-
- // catch negative or zero feed rates and return the same error as GRBL does
- if(rate_mm_s <= 0.0F) {
- gcode->is_error= true;
- gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0");
- return false;
- }
+ float deltas[n_motors];
+ float transformed_target[n_motors]; // adjust target for bed compensation
+ float unit_vec[N_PRIMARY_AXIS];
// unity transform by default
- memcpy(transformed_target, target, sizeof(transformed_target));
+ memcpy(transformed_target, target, n_motors*sizeof(float));
// check function pointer and call if set to transform the target to compensate for bed
if(compensationTransform) {
// some compensation strategies can transform XYZ, some just change Z
- compensationTransform(transformed_target);
+ compensationTransform(transformed_target, false);
}
+ bool move= false;
+ float sos= 0; // sun of squares for just XYZ
+
// find distance moved by each axis, use transformed target from the current machine position
- for (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
- deltas[axis] = transformed_target[axis] - last_machine_position[axis];
+ for (size_t i = 0; i < n_motors; i++) {
+ deltas[i] = transformed_target[i] - last_machine_position[i];
+ if(deltas[i] == 0) continue;
+ // at least one non zero delta
+ move = true;
+ if(i <= Z_AXIS) {
+ sos += powf(deltas[i], 2);
+ }
}
- // Compute how long this move moves, so we can attach it to the block for later use
- millimeters_of_travel = sqrtf( powf( deltas[X_AXIS], 2 ) + powf( deltas[Y_AXIS], 2 ) + powf( deltas[Z_AXIS], 2 ) );
+ // nothing moved
+ 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;
+
+ // 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);
// it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
// as the last milestone won't be updated we do not actually lose any moves as they will be accounted for in the next move
- if(millimeters_of_travel < 0.00001F) return false;
+ if(!auxilliary_move && distance < 0.00001F) return false;
- // this is the machine position
- memcpy(this->last_machine_position, transformed_target, sizeof(this->last_machine_position));
- // find distance unit vector
- for (int i = 0; i < 3; i++)
- unit_vec[i] = deltas[i] / millimeters_of_travel;
+ if(!auxilliary_move) {
+ for (size_t i = X_AXIS; i <= Z_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 (int axis = X_AXIS; axis <= Z_AXIS; axis++) {
- if ( max_speeds[axis] > 0 ) {
- float axis_speed = fabs(unit_vec[axis] * rate_mm_s);
+ // Do not move faster than the configured cartesian limits for XYZ
+ if ( max_speeds[i] > 0 ) {
+ float axis_speed = fabsf(unit_vec[i] * rate_mm_s);
- if (axis_speed > max_speeds[axis])
- rate_mm_s *= ( max_speeds[axis] / axis_speed );
+ if (axis_speed > max_speeds[i])
+ rate_mm_s *= ( max_speeds[i] / axis_speed );
+ }
}
}
// find actuator position given the machine position, use actual adjusted target
- arm_solution->cartesian_to_actuator( this->last_machine_position, actuator_pos );
+ ActuatorCoordinates actuator_pos;
+ if(!disable_arm_solution) {
+ arm_solution->cartesian_to_actuator( transformed_target, actuator_pos );
+
+ }else{
+ // basically the same as cartesian, would be used for special homing situations like for scara
+ for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+ actuator_pos[i] = transformed_target[i];
+ }
+ }
+
+#if MAX_ROBOT_ACTUATORS > 3
+ sos= 0;
+ // for the extruders just copy the position, and possibly scale it from mm³ to mm
+ for (size_t i = E_AXIS; i < n_motors; i++) {
+ actuator_pos[i]= transformed_target[i];
+ if(get_e_scale_fnc) {
+ // NOTE this relies on the fact only one extruder is active at a time
+ // scale for volumetric or flow rate
+ // TODO is this correct? scaling the absolute target? what if the scale changes?
+ // for volumetric it basically converts mm³ to mm, but what about flow rate?
+ actuator_pos[i] *= get_e_scale_fnc();
+ }
+ 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);
+ }
+ }
+ if(auxilliary_move) {
+ distance= sqrtf(sos); // distance in mm of the e move
+ if(distance < 0.00001F) return false;
+ }
+#endif
+
+ // use default acceleration to start with
+ float acceleration = default_acceleration;
+
+ float isecs = rate_mm_s / distance;
- float isecs = rate_mm_s / millimeters_of_travel;
// check per-actuator speed limits
- for (size_t actuator = 0; actuator < actuators.size(); actuator++) {
- float actuator_rate = fabsf(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * isecs;
+ for (size_t actuator = 0; actuator < n_motors; actuator++) {
+ float d = fabsf(actuator_pos[actuator] - actuators[actuator]->get_last_milestone());
+ if(d == 0 || !actuators[actuator]->is_selected()) continue; // no movement for this actuator
+
+ float actuator_rate= d * isecs;
if (actuator_rate > actuators[actuator]->get_max_rate()) {
rate_mm_s *= (actuators[actuator]->get_max_rate() / actuator_rate);
- isecs = rate_mm_s / millimeters_of_travel;
+ isecs = rate_mm_s / distance;
+ }
+
+ // 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) {
+ 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);
+ if (ca > ma) {
+ acceleration *= ( ma / ca );
+ }
+ }
}
}
// Append the block to the planner
- THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec );
+ // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
+ 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 machine position
+ memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float));
+ return true;
+ }
+
+ // no actual move
+ return false;
+}
+
+// Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
+bool Robot::delta_move(const float *delta, float rate_mm_s, uint8_t naxis)
+{
+ if(THEKERNEL->is_halted()) return false;
+
+ // catch negative or zero feed rates
+ if(rate_mm_s <= 0.0F) {
+ return false;
+ }
+
+ // get the absolute target position, default is current last_milestone
+ float target[n_motors];
+ memcpy(target, last_milestone, n_motors*sizeof(float));
+
+ // add in the deltas to get new target
+ for (int i= 0; i < naxis; i++) {
+ target[i] += delta[i];
+ }
+
+ // submit for planning and if moved update last_milestone
+ if(append_milestone(target, rate_mm_s)) {
+ memcpy(last_milestone, target, n_motors*sizeof(float));
+ return true;
+ }
- return true;
+ return false;
}
// Append a move to the queue ( cutting it into segments if needed )
-bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s )
+bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, float delta_e)
{
- // Find out the distance for this move in MCS
- // NOTE we need to do sqrt here as this setting of millimeters_of_travel is used by extruder and other modules even if there is no XYZ move
- gcode->millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_milestone[Z_AXIS], 2 ));
-
- // We ignore non- XYZ moves ( for example, extruder moves are not XYZ moves )
- if( gcode->millimeters_of_travel < 0.00001F ) return false;
-
- // Mark the gcode as having a known distance
- this->distance_in_gcode_is_known( gcode );
-
- // if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit
- // Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
- // We also check we are not exceeding the E max_speed for the current extruder
- // We ask Extruder to do all the work, but as Extruder won't even see this gcode until after it has been planned
- // we need to ask it now passing in the relevant data.
- // NOTE we need to do this before we segment the line (for deltas)
- if(gcode->has_letter('E')) {
- float data[2];
- data[0] = gcode->get_value('E'); // E target (may be absolute or relative)
- data[1] = rate_mm_s / gcode->millimeters_of_travel; // inverted seconds for the move
+ // catch negative or zero feed rates and return the same error as GRBL does
+ if(rate_mm_s <= 0.0F) {
+ gcode->is_error= true;
+ gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0");
+ return false;
+ }
+
+ // Find out the distance for this move in XYZ in MCS
+ float millimeters_of_travel = sqrtf(powf( target[X_AXIS] - last_milestone[X_AXIS], 2 ) + powf( target[Y_AXIS] - last_milestone[Y_AXIS], 2 ) + powf( target[Z_AXIS] - last_milestone[Z_AXIS], 2 ));
+
+ if(millimeters_of_travel < 0.00001F) {
+ // we have no movement in XYZ, probably E only extrude or retract
+ return this->append_milestone(target, rate_mm_s);
+ }
+
+ /*
+ For extruders, we need to do some extra work to limit the volumetric rate if specified...
+ If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
+ We ask Extruder to do all the work but we need to pass in the relevant data.
+ NOTE we need to do this before we segment the line (for deltas)
+ */
+ if(!isnan(delta_e) && gcode->has_g && gcode->g == 1) {
+ float data[2]= {delta_e, rate_mm_s / millimeters_of_travel};
if(PublicData::set_value(extruder_checksum, target_checksum, data)) {
- rate_mm_s *= data[1];
- //THEKERNEL->streams->printf("Extruder has changed the rate by %f to %f\n", data[1], rate_mm_s);
+ rate_mm_s *= data[1]; // adjust the feedrate
}
}
// segment based on current speed and requested segments per second
// the faster the travel speed the fewer segments needed
// NOTE rate is mm/sec and we take into account any speed override
- float seconds = gcode->millimeters_of_travel / rate_mm_s;
+ float seconds = millimeters_of_travel / rate_mm_s;
segments = max(1.0F, ceilf(this->delta_segments_per_second * seconds));
// TODO if we are only moving in Z on a delta we don't really need to segment at all
if(this->mm_per_line_segment == 0.0F) {
segments = 1; // don't split it up
} else {
- segments = ceilf( gcode->millimeters_of_travel / this->mm_per_line_segment);
+ segments = ceilf( millimeters_of_travel / this->mm_per_line_segment);
}
}
bool moved= false;
if (segments > 1) {
// A vector to keep track of the endpoint of each segment
- float segment_delta[3];
- float segment_end[3]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
+ float segment_delta[n_motors];
+ float segment_end[n_motors];
+ memcpy(segment_end, last_milestone, n_motors*sizeof(float));
// How far do we move each segment?
- for (int i = X_AXIS; i <= Z_AXIS; i++)
+ for (int i = 0; i < n_motors; i++)
segment_delta[i] = (target[i] - last_milestone[i]) / segments;
// segment 0 is already done - it's the end point of the previous move so we start at segment 1
// 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 axis = X_AXIS; axis <= Z_AXIS; axis++ )
- segment_end[axis] += segment_delta[axis];
+ for (int i = 0; i < n_motors; i++)
+ segment_end[i] += segment_delta[i];
// Append the end of this segment to the queue
- bool b= this->append_milestone(gcode, segment_end, rate_mm_s);
+ bool b= this->append_milestone(segment_end, rate_mm_s);
moved= moved || b;
}
}
// Append the end of this full move to the queue
- if(this->append_milestone(gcode, target, rate_mm_s)) moved= true;
+ if(this->append_milestone(target, rate_mm_s)) moved= true;
this->next_command_is_MCS = false; // always reset this
- // this is not neede as COnveyor::on_main_loop will do something
- // if(moved) {
- // // if adding these blocks didn't start executing, do that now
- // THEKERNEL->conveyor->ensure_running();
- // }
-
return moved;
}
// Append an arc to the queue ( cutting it into segments as needed )
+// TODO does not support any E parameters so cannot be used for 3D printing.
bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[], float radius, bool is_clockwise )
{
+ float rate_mm_s= this->feed_rate / seconds_per_minute;
+ // catch negative or zero feed rates and return the same error as GRBL does
+ if(rate_mm_s <= 0.0F) {
+ gcode->is_error= true;
+ gcode->txt_after_ok= (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0");
+ return false;
+ }
// Scary math
float center_axis0 = this->last_milestone[this->plane_axis_0] + offset[this->plane_axis_0];
// 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 * (float)M_PI); }
+ if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
} else {
- if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * (float)M_PI); }
+ if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += (2 * PI); }
}
// Find the distance for this gcode
- gcode->millimeters_of_travel = hypotf(angular_travel * radius, fabsf(linear_travel));
+ 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( gcode->millimeters_of_travel < 0.00001F ) {
+ if( millimeters_of_travel < 0.00001F ) {
return false;
}
- // Mark the gcode as having a known distance
- this->distance_in_gcode_is_known( gcode );
-
+ // limit segments by maximum arc error
+ float arc_segment = this->mm_per_arc_segment;
+ if ((this->mm_max_arc_error > 0) && (2 * radius > this->mm_max_arc_error)) {
+ float min_err_segment = 2 * sqrtf((this->mm_max_arc_error * (2 * radius - this->mm_max_arc_error)));
+ if (this->mm_per_arc_segment < min_err_segment) {
+ arc_segment = min_err_segment;
+ }
+ }
// Figure out how many segments for this gcode
- uint16_t segments = floorf(gcode->millimeters_of_travel / this->mm_per_arc_segment);
+ // 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;
arc_target[this->plane_axis_2] += linear_per_segment;
// Append this segment to the queue
- bool b= this->append_milestone(gcode, arc_target, this->feed_rate / seconds_per_minute);
+ bool b= this->append_milestone(arc_target, rate_mm_s);
moved= moved || b;
}
// Ensure last segment arrives at target location.
- if(this->append_milestone(gcode, target, this->feed_rate / seconds_per_minute)) moved= true;
+ if(this->append_milestone(target, rate_mm_s)) moved= true;
return moved;
}
// Do the math for an arc and add it to the queue
-bool Robot::compute_arc(Gcode * gcode, const float offset[], const float target[])
+bool Robot::compute_arc(Gcode * gcode, const float offset[], const float target[], enum MOTION_MODE_T motion_mode)
{
// Find the radius
// Set clockwise/counter-clockwise sign for mc_arc computations
bool is_clockwise = false;
- if( this->motion_mode == MOTION_MODE_CW_ARC ) {
+ if( motion_mode == CW_ARC ) {
is_clockwise = true;
}
return(t);
} else {
if (t > 0) {
- return(M_PI - t);
+ return(PI - t);
} else {
- return(-M_PI - t);
+ return(-PI - t);
}
}
}
HCoop / clinton / Smoothieware.git / blobdiff