2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
15 #include "StepperMotor.h"
17 #include "PublicDataRequest.h"
18 #include "PublicData.h"
19 #include "arm_solutions/BaseSolution.h"
20 #include "arm_solutions/CartesianSolution.h"
21 #include "arm_solutions/RotatableCartesianSolution.h"
22 #include "arm_solutions/LinearDeltaSolution.h"
23 #include "arm_solutions/RotaryDeltaSolution.h"
24 #include "arm_solutions/HBotSolution.h"
25 #include "arm_solutions/CoreXZSolution.h"
26 #include "arm_solutions/MorganSCARASolution.h"
27 #include "StepTicker.h"
28 #include "checksumm.h"
30 #include "ConfigValue.h"
31 #include "libs/StreamOutput.h"
32 #include "StreamOutputPool.h"
33 #include "ExtruderPublicAccess.h"
34 #include "GcodeDispatch.h"
35 #include "ActuatorCoordinates.h"
37 #include "mbed.h" // for us_ticker_read()
45 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
46 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
47 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
48 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
49 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
50 #define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
51 #define arc_correction_checksum CHECKSUM("arc_correction")
52 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
53 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
54 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
55 #define segment_z_moves_checksum CHECKSUM("segment_z_moves")
56 #define save_g92_checksum CHECKSUM("save_g92")
59 #define arm_solution_checksum CHECKSUM("arm_solution")
60 #define cartesian_checksum CHECKSUM("cartesian")
61 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
62 #define rostock_checksum CHECKSUM("rostock")
63 #define linear_delta_checksum CHECKSUM("linear_delta")
64 #define rotary_delta_checksum CHECKSUM("rotary_delta")
65 #define delta_checksum CHECKSUM("delta")
66 #define hbot_checksum CHECKSUM("hbot")
67 #define corexy_checksum CHECKSUM("corexy")
68 #define corexz_checksum CHECKSUM("corexz")
69 #define kossel_checksum CHECKSUM("kossel")
70 #define morgan_checksum CHECKSUM("morgan")
72 // new-style actuator stuff
73 #define actuator_checksum CHEKCSUM("actuator")
75 #define step_pin_checksum CHECKSUM("step_pin")
76 #define dir_pin_checksum CHEKCSUM("dir_pin")
77 #define en_pin_checksum CHECKSUM("en_pin")
79 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
80 #define max_rate_checksum CHECKSUM("max_rate")
81 #define acceleration_checksum CHECKSUM("acceleration")
82 #define z_acceleration_checksum CHECKSUM("z_acceleration")
84 #define alpha_checksum CHECKSUM("alpha")
85 #define beta_checksum CHECKSUM("beta")
86 #define gamma_checksum CHECKSUM("gamma")
88 #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
90 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
91 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
93 // 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
94 // It takes care of cutting arcs into segments, same thing for line that are too long
98 this->inch_mode
= false;
99 this->absolute_mode
= true;
100 this->e_absolute_mode
= true;
101 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
102 memset(this->last_milestone
, 0, sizeof last_milestone
);
103 memset(this->last_machine_position
, 0, sizeof last_machine_position
);
104 this->arm_solution
= NULL
;
105 seconds_per_minute
= 60.0F
;
106 this->clearToolOffset();
107 this->compensationTransform
= nullptr;
108 this->get_e_scale_fnc
= nullptr;
109 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
110 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
111 this->next_command_is_MCS
= false;
112 this->disable_segmentation
= false;
113 this->disable_arm_solution
= false;
117 //Called when the module has just been loaded
118 void Robot::on_module_loaded()
120 this->register_for_event(ON_GCODE_RECEIVED
);
126 #define ACTUATOR_CHECKSUMS(X) { \
127 CHECKSUM(X "_step_pin"), \
128 CHECKSUM(X "_dir_pin"), \
129 CHECKSUM(X "_en_pin"), \
130 CHECKSUM(X "_steps_per_mm"), \
131 CHECKSUM(X "_max_rate"), \
132 CHECKSUM(X "_acceleration") \
135 void Robot::load_config()
137 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
138 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
139 // To make adding those solution easier, they have their own, separate object.
140 // Here we read the config to find out which arm solution to use
141 if (this->arm_solution
) delete this->arm_solution
;
142 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
143 // Note checksums are not const expressions when in debug mode, so don't use switch
144 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
145 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
147 } else if(solution_checksum
== corexz_checksum
) {
148 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
150 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
151 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
153 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
154 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== rotary_delta_checksum
) {
157 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== morgan_checksum
) {
160 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== cartesian_checksum
) {
163 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
166 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
169 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
170 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
171 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
172 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
173 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
174 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
175 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
177 // in mm/sec but specified in config as mm/min
178 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
179 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
180 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
182 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
183 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
185 // default s value for laser
186 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
188 // Make our Primary XYZ StepperMotors
189 uint16_t const checksums
[][6] = {
190 ACTUATOR_CHECKSUMS("alpha"), // X
191 ACTUATOR_CHECKSUMS("beta"), // Y
192 ACTUATOR_CHECKSUMS("gamma"), // Z
195 // default acceleration setting, can be overriden with newer per axis settings
196 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
199 for (size_t a
= X_AXIS
; a
<= Z_AXIS
; a
++) {
200 Pin pins
[3]; //step, dir, enable
201 for (size_t i
= 0; i
< 3; i
++) {
202 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
204 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
205 // register this motor (NB This must be 0,1,2) of the actuators array
206 uint8_t n
= register_motor(sm
);
208 // this is a fatal error
209 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
213 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
214 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
215 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
218 check_max_actuator_speeds(); // check the configs are sane
220 // if we have not specified a z acceleration see if the legacy config was set
221 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
222 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
224 actuators
[Z_AXIS
]->set_acceleration(acc
);
228 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
229 // so the first move can be correct if homing is not performed
230 ActuatorCoordinates actuator_pos
;
231 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
232 for (size_t i
= 0; i
< n_motors
; i
++)
233 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
235 //this->clearToolOffset();
238 uint8_t Robot::register_motor(StepperMotor
*motor
)
240 // register this motor with the step ticker
241 THEKERNEL
->step_ticker
->register_motor(motor
);
242 if(n_motors
>= k_max_actuators
) {
243 // this is a fatal error
244 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
247 actuators
.push_back(motor
);
251 void Robot::push_state()
253 bool am
= this->absolute_mode
;
254 bool em
= this->e_absolute_mode
;
255 bool im
= this->inch_mode
;
256 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
260 void Robot::pop_state()
262 if(!state_stack
.empty()) {
263 auto s
= state_stack
.top();
265 this->feed_rate
= std::get
<0>(s
);
266 this->seek_rate
= std::get
<1>(s
);
267 this->absolute_mode
= std::get
<2>(s
);
268 this->e_absolute_mode
= std::get
<3>(s
);
269 this->inch_mode
= std::get
<4>(s
);
270 this->current_wcs
= std::get
<5>(s
);
274 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
276 std::vector
<wcs_t
> v
;
277 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
278 for(auto& i
: wcs_offsets
) {
281 v
.push_back(g92_offset
);
282 v
.push_back(tool_offset
);
286 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
288 // M114.1 is a new way to do this (similar to how GRBL does it).
289 // it returns the realtime position based on the current step position of the actuators.
290 // this does require a FK to get a machine position from the actuator position
291 // and then invert all the transforms to get a workspace position from machine position
292 // M114 just does it the old way uses last_milestone and does inverse transforms to get the requested position
294 if(subcode
== 0) { // M114 print WCS
295 wcs_t pos
= mcs2wcs(last_milestone
);
296 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
)));
298 } else if(subcode
== 4) {
299 // M114.4 print last milestone
300 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
]);
302 } else if(subcode
== 5) {
303 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
304 // will differ from LMS by the compensation at the current position otherwise
305 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
]);
308 // get real time positions
309 // current actuator position in mm
310 ActuatorCoordinates current_position
{
311 actuators
[X_AXIS
]->get_current_position(),
312 actuators
[Y_AXIS
]->get_current_position(),
313 actuators
[Z_AXIS
]->get_current_position()
316 // get machine position from the actuator position using FK
318 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
319 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
320 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
322 if(subcode
== 1) { // M114.1 print realtime WCS
323 wcs_t pos
= mcs2wcs(mpos
);
324 n
= snprintf(buf
, bufsize
, "WPOS: 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
)));
326 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
327 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
329 } else if(subcode
== 3) { // M114.3 print realtime actuator position
330 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
]);
336 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
337 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
339 return std::make_tuple(
340 std::get
<X_AXIS
>(pos
) - std::get
<X_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<X_AXIS
>(g92_offset
) - std::get
<X_AXIS
>(tool_offset
),
341 std::get
<Y_AXIS
>(pos
) - std::get
<Y_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<Y_AXIS
>(g92_offset
) - std::get
<Y_AXIS
>(tool_offset
),
342 std::get
<Z_AXIS
>(pos
) - std::get
<Z_AXIS
>(wcs_offsets
[current_wcs
]) + std::get
<Z_AXIS
>(g92_offset
) - std::get
<Z_AXIS
>(tool_offset
)
346 // this does a sanity check that actuator speeds do not exceed steps rate capability
347 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
348 void Robot::check_max_actuator_speeds()
350 for (size_t i
= 0; i
< n_motors
; i
++) {
351 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
352 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
353 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
354 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());
359 //A GCode has been received
360 //See if the current Gcode line has some orders for us
361 void Robot::on_gcode_received(void *argument
)
363 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
365 enum MOTION_MODE_T motion_mode
= NONE
;
369 case 0: motion_mode
= SEEK
; break;
370 case 1: motion_mode
= LINEAR
; break;
371 case 2: motion_mode
= CW_ARC
; break;
372 case 3: motion_mode
= CCW_ARC
; break;
373 case 4: { // G4 pause
374 uint32_t delay_ms
= 0;
375 if (gcode
->has_letter('P')) {
376 delay_ms
= gcode
->get_int('P');
378 if (gcode
->has_letter('S')) {
379 delay_ms
+= gcode
->get_int('S') * 1000;
383 THEKERNEL
->conveyor
->wait_for_idle();
384 // wait for specified time
385 uint32_t start
= us_ticker_read(); // mbed call
386 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
387 THEKERNEL
->call_event(ON_IDLE
, this);
388 if(THEKERNEL
->is_halted()) return;
394 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
395 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
396 size_t n
= gcode
->get_uint('P');
397 if(n
== 0) n
= current_wcs
; // set current coordinate system
401 std::tie(x
, y
, z
) = wcs_offsets
[n
];
402 if(gcode
->get_int('L') == 20) {
403 // this makes the current machine position (less compensation transform) the offset
404 // get current position in WCS
405 wcs_t pos
= mcs2wcs(last_milestone
);
407 if(gcode
->has_letter('X')){
408 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
411 if(gcode
->has_letter('Y')){
412 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
414 if(gcode
->has_letter('Z')) {
415 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
419 // the value is the offset from machine zero
420 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
421 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
422 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
424 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
429 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
430 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
431 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
432 case 20: this->inch_mode
= true; break;
433 case 21: this->inch_mode
= false; break;
435 case 54: case 55: case 56: case 57: case 58: case 59:
436 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
437 current_wcs
= gcode
->g
- 54;
438 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
439 current_wcs
+= gcode
->subcode
;
440 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
444 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
445 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
448 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
449 // reset G92 offsets to 0
450 g92_offset
= wcs_t(0, 0, 0);
452 } else if(gcode
->subcode
== 3) {
453 // initialize G92 to the specified values, only used for saving it with M500
454 float x
= 0, y
= 0, z
= 0;
455 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
456 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
457 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
458 g92_offset
= wcs_t(x
, y
, z
);
461 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
463 std::tie(x
, y
, z
) = g92_offset
;
464 // get current position in WCS
465 wcs_t pos
= mcs2wcs(last_milestone
);
467 // adjust g92 offset to make the current wpos == the value requested
468 if(gcode
->has_letter('X')){
469 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
471 if(gcode
->has_letter('Y')){
472 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
474 if(gcode
->has_letter('Z')) {
475 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
477 g92_offset
= wcs_t(x
, y
, z
);
480 #if MAX_ROBOT_ACTUATORS > 3
481 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
482 // reset the E position, legacy for 3d Printers to be reprap compatible
483 // find the selected extruder
484 // 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
485 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
486 if(actuators
[i
]->is_selected()) {
487 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
488 last_milestone
[i
]= last_machine_position
[i
]= e
;
489 actuators
[i
]->change_last_milestone(e
);
500 } else if( gcode
->has_m
) {
502 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
503 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
506 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
507 if(!THEKERNEL
->is_grbl_mode()) break;
508 // fall through to M2
509 case 2: // M2 end of program
511 absolute_mode
= true;
514 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
517 case 18: // this used to support parameters, now it ignores them
519 THEKERNEL
->conveyor
->wait_for_idle();
520 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
523 case 82: e_absolute_mode
= true; break;
524 case 83: e_absolute_mode
= false; break;
526 case 92: // M92 - set steps per mm
527 if (gcode
->has_letter('X'))
528 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
529 if (gcode
->has_letter('Y'))
530 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
531 if (gcode
->has_letter('Z'))
532 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
534 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());
535 gcode
->add_nl
= true;
536 check_max_actuator_speeds();
541 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
542 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
546 case 120: // push state
550 case 121: // pop state
554 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
555 if(gcode
->get_num_args() == 0) {
556 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
557 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
559 gcode
->add_nl
= true;
562 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
563 if (gcode
->has_letter('X' + i
)) {
564 float v
= gcode
->get_value('X'+i
);
565 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
566 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
570 // this format is deprecated
571 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
572 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
573 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
574 if (gcode
->has_letter('A' + i
)) {
575 float v
= gcode
->get_value('A'+i
);
576 actuators
[i
]->set_max_rate(v
);
581 if(gcode
->subcode
== 1) check_max_actuator_speeds();
585 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
586 if (gcode
->has_letter('S')) {
587 float acc
= gcode
->get_value('S'); // mm/s^2
589 if (acc
< 1.0F
) acc
= 1.0F
;
590 this->default_acceleration
= acc
;
592 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
593 if (gcode
->has_letter(i
+'X')) {
594 float acc
= gcode
->get_value(i
+'X'); // mm/s^2
596 if (acc
<= 0.0F
) acc
= NAN
;
597 actuators
[i
]->set_acceleration(acc
);
602 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
603 if (gcode
->has_letter('X')) {
604 float jd
= gcode
->get_value('X');
608 THEKERNEL
->planner
->junction_deviation
= jd
;
610 if (gcode
->has_letter('Z')) {
611 float jd
= gcode
->get_value('Z');
612 // enforce minimum, -1 disables it and uses regular junction deviation
615 THEKERNEL
->planner
->z_junction_deviation
= jd
;
617 if (gcode
->has_letter('S')) {
618 float mps
= gcode
->get_value('S');
622 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
626 case 220: // M220 - speed override percentage
627 if (gcode
->has_letter('S')) {
628 float factor
= gcode
->get_value('S');
629 // enforce minimum 10% speed
632 // enforce maximum 10x speed
633 if (factor
> 1000.0F
)
636 seconds_per_minute
= 6000.0F
/ factor
;
638 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
642 case 400: // wait until all moves are done up to this point
643 THEKERNEL
->conveyor
->wait_for_idle();
646 case 500: // M500 saves some volatile settings to config override file
647 case 503: { // M503 just prints the settings
648 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());
650 // only print XYZ if not NAN
651 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
652 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
653 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", 'X'+i
, actuators
[i
]->get_acceleration());
655 gcode
->stream
->printf("\n");
657 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
);
659 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
]);
660 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());
662 // get or save any arm solution specific optional values
663 BaseSolution::arm_options_t options
;
664 if(arm_solution
->get_optional(options
) && !options
.empty()) {
665 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
666 for(auto &i
: options
) {
667 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
669 gcode
->stream
->printf("\n");
672 // save wcs_offsets and current_wcs
673 // TODO this may need to be done whenever they change to be compliant
674 gcode
->stream
->printf(";WCS settings\n");
675 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
677 for(auto &i
: wcs_offsets
) {
678 if(i
!= wcs_t(0, 0, 0)) {
680 std::tie(x
, y
, z
) = i
;
681 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
686 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
687 // also it needs to be used to set Z0 on rotary deltas as M206/306 can't be used, so saving it is necessary in that case
688 if(g92_offset
!= wcs_t(0, 0, 0)) {
690 std::tie(x
, y
, z
) = g92_offset
;
691 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
697 case 665: { // M665 set optional arm solution variables based on arm solution.
698 // the parameter args could be any letter each arm solution only accepts certain ones
699 BaseSolution::arm_options_t options
= gcode
->get_args();
700 options
.erase('S'); // don't include the S
701 options
.erase('U'); // don't include the U
702 if(options
.size() > 0) {
703 // set the specified options
704 arm_solution
->set_optional(options
);
707 if(arm_solution
->get_optional(options
)) {
708 // foreach optional value
709 for(auto &i
: options
) {
710 // print all current values of supported options
711 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
712 gcode
->add_nl
= true;
716 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
717 this->delta_segments_per_second
= gcode
->get_value('S');
718 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
720 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
721 this->mm_per_line_segment
= gcode
->get_value('U');
722 this->delta_segments_per_second
= 0;
723 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
731 if( motion_mode
!= NONE
) {
732 is_g123
= motion_mode
!= SEEK
;
733 process_move(gcode
, motion_mode
);
739 next_command_is_MCS
= false; // must be on same line as G0 or G1
742 // process a G0/G1/G2/G3
743 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
745 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
746 // get XYZ and one E (which goes to the selected extruder)
747 float param
[4]{NAN
, NAN
, NAN
, NAN
};
749 // process primary axis
750 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
752 if( gcode
->has_letter(letter
) ) {
753 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
757 float offset
[3]{0,0,0};
758 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
759 if( gcode
->has_letter(letter
) ) {
760 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
764 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
765 float target
[n_motors
];
766 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
768 if(!next_command_is_MCS
) {
769 if(this->absolute_mode
) {
770 // apply wcs offsets and g92 offset and tool offset
771 if(!isnan(param
[X_AXIS
])) {
772 target
[X_AXIS
]= param
[X_AXIS
] + std::get
<X_AXIS
>(wcs_offsets
[current_wcs
]) - std::get
<X_AXIS
>(g92_offset
) + std::get
<X_AXIS
>(tool_offset
);
775 if(!isnan(param
[Y_AXIS
])) {
776 target
[Y_AXIS
]= param
[Y_AXIS
] + std::get
<Y_AXIS
>(wcs_offsets
[current_wcs
]) - std::get
<Y_AXIS
>(g92_offset
) + std::get
<Y_AXIS
>(tool_offset
);
779 if(!isnan(param
[Z_AXIS
])) {
780 target
[Z_AXIS
]= param
[Z_AXIS
] + std::get
<Z_AXIS
>(wcs_offsets
[current_wcs
]) - std::get
<Z_AXIS
>(g92_offset
) + std::get
<Z_AXIS
>(tool_offset
);
784 // they are deltas from the last_milestone if specified
785 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
786 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
791 // already in machine coordinates, we do not add tool offset for that
792 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
793 if(!isnan(param
[i
])) target
[i
] = param
[i
];
797 // process extruder parameters, for active extruder only (only one active extruder at a time)
798 selected_extruder
= 0;
799 if(gcode
->has_letter('E')) {
800 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
801 // find first selected extruder
802 if(actuators
[i
]->is_selected()) {
803 param
[E_AXIS
]= gcode
->get_value('E');
804 selected_extruder
= i
;
810 // do E for the selected extruder
812 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
813 if(this->e_absolute_mode
) {
814 target
[selected_extruder
]= param
[E_AXIS
];
815 delta_e
= target
[selected_extruder
] - last_milestone
[selected_extruder
];
817 delta_e
= param
[E_AXIS
];
818 target
[selected_extruder
] = delta_e
+ last_milestone
[selected_extruder
];
822 if( gcode
->has_letter('F') ) {
823 if( motion_mode
== SEEK
)
824 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
826 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
829 // S is modal When specified on a G0/1/2/3 command
830 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
834 // Perform any physical actions
835 switch(motion_mode
) {
839 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
843 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
848 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
849 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
854 // set last_milestone to the calculated target
855 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
859 // reset the machine position for all axis. Used for homing.
860 // after homing we need to set the actuator position at the position they would be at for the compensated XYZ
861 // So we need to apply the compensation transform to the last_milestone we are given to get the compensated
862 // last_machine_position which we then convert to actuator position.
863 // this will make the results from M114 and ? consistent after homing.
864 void Robot::reset_axis_position(float x
, float y
, float z
)
866 // these are set to the same as compensation was not used to get to the current position
867 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
868 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
869 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
871 if(compensationTransform
) {
872 compensationTransform(last_machine_position
, false);
875 // now set the actuator positions to match
876 ActuatorCoordinates actuator_pos
;
877 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
878 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
879 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
882 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
883 void Robot::reset_axis_position(float position
, int axis
)
885 last_milestone
[axis
] = position
;
887 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
888 #if MAX_ROBOT_ACTUATORS > 3
890 // extruders need to be set not calculated
891 last_machine_position
[axis
]= position
;
896 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
897 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
898 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
900 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
901 actuators
[i
]->change_last_milestone(ac
[i
]);
903 // now correct axis positions then recorrect actuator to account for rounding
904 reset_position_from_current_actuator_position();
907 // Use FK to find out where actuator is and reset to match
908 void Robot::reset_position_from_current_actuator_position()
910 ActuatorCoordinates actuator_pos
;
911 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
912 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
913 actuator_pos
[i
] = actuators
[i
]->get_current_position();
916 // discover machine position from where actuators actually are
917 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
918 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
920 // last_machine_position includes the compensation transform so we need to get the inverse to get actual last_milestone
921 if(compensationTransform
) compensationTransform(last_milestone
, true); // get inverse compensation transform
923 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
924 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
925 // to get everything in perfect sync.
926 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
927 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
928 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
931 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
932 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
933 // all transforms and is what we actually convert to actuator positions
934 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
936 float deltas
[n_motors
];
937 float transformed_target
[n_motors
]; // adjust target for bed compensation
938 float unit_vec
[N_PRIMARY_AXIS
];
940 // unity transform by default
941 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
943 // check function pointer and call if set to transform the target to compensate for bed
944 if(compensationTransform
) {
945 // some compensation strategies can transform XYZ, some just change Z
946 compensationTransform(transformed_target
, false);
950 float sos
= 0; // sun of squares for just XYZ
952 // find distance moved by each axis, use transformed target from the current machine position
953 for (size_t i
= 0; i
< n_motors
; i
++) {
954 deltas
[i
] = transformed_target
[i
] - last_machine_position
[i
];
955 if(deltas
[i
] == 0) continue;
956 // at least one non zero delta
959 sos
+= powf(deltas
[i
], 2);
964 if(!move
) return false;
966 // see if this is a primary axis move or not
967 bool auxilliary_move
= deltas
[X_AXIS
] == 0 && deltas
[Y_AXIS
] == 0 && deltas
[Z_AXIS
] == 0;
969 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
970 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
972 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
973 // 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
974 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
977 if(!auxilliary_move
) {
978 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
979 // find distance unit vector for primary axis only
980 unit_vec
[i
] = deltas
[i
] / distance
;
982 // Do not move faster than the configured cartesian limits for XYZ
983 if ( max_speeds
[i
] > 0 ) {
984 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
986 if (axis_speed
> max_speeds
[i
])
987 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
992 // find actuator position given the machine position, use actual adjusted target
993 ActuatorCoordinates actuator_pos
;
994 if(!disable_arm_solution
) {
995 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
998 // basically the same as cartesian, would be used for special homing situations like for scara
999 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1000 actuator_pos
[i
] = transformed_target
[i
];
1004 #if MAX_ROBOT_ACTUATORS > 3
1006 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1007 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1008 actuator_pos
[i
]= transformed_target
[i
];
1009 if(get_e_scale_fnc
) {
1010 // NOTE this relies on the fact only one extruder is active at a time
1011 // scale for volumetric or flow rate
1012 // TODO is this correct? scaling the absolute target? what if the scale changes?
1013 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1014 actuator_pos
[i
] *= get_e_scale_fnc();
1016 if(auxilliary_move
) {
1017 // for E only moves we need to use the scaled E to calculate the distance
1018 sos
+= pow(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1021 if(auxilliary_move
) {
1022 distance
= sqrtf(sos
); // distance in mm of the e move
1023 if(distance
< 0.00001F
) return false;
1027 // use default acceleration to start with
1028 float acceleration
= default_acceleration
;
1030 float isecs
= rate_mm_s
/ distance
;
1032 // check per-actuator speed limits
1033 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1034 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1035 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1037 float actuator_rate
= d
* isecs
;
1038 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1039 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1040 isecs
= rate_mm_s
/ distance
;
1043 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1044 // 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.
1045 if(auxilliary_move
|| actuator
<= Z_AXIS
) {
1046 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1047 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1048 float ca
= fabsf((d
/distance
) * acceleration
);
1050 acceleration
*= ( ma
/ ca
);
1056 // Append the block to the planner
1057 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1058 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1059 // this is the machine position
1060 memcpy(this->last_machine_position
, transformed_target
, n_motors
*sizeof(float));
1068 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1069 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1071 if(THEKERNEL
->is_halted()) return false;
1073 // catch negative or zero feed rates
1074 if(rate_mm_s
<= 0.0F
) {
1078 // get the absolute target position, default is current last_milestone
1079 float target
[n_motors
];
1080 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
1082 // add in the deltas to get new target
1083 for (int i
= 0; i
< naxis
; i
++) {
1084 target
[i
] += delta
[i
];
1087 // submit for planning and if moved update last_milestone
1088 if(append_milestone(target
, rate_mm_s
)) {
1089 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
1096 // Append a move to the queue ( cutting it into segments if needed )
1097 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1099 // catch negative or zero feed rates and return the same error as GRBL does
1100 if(rate_mm_s
<= 0.0F
) {
1101 gcode
->is_error
= true;
1102 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1106 // Find out the distance for this move in XYZ in MCS
1107 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 ));
1109 if(millimeters_of_travel
< 0.00001F
) {
1110 // we have no movement in XYZ, probably E only extrude or retract
1111 return this->append_milestone(target
, rate_mm_s
);
1115 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1116 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1117 We ask Extruder to do all the work but we need to pass in the relevant data.
1118 NOTE we need to do this before we segment the line (for deltas)
1120 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1121 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1122 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1123 rate_mm_s
*= data
[1]; // adjust the feedrate
1127 // We cut the line into smaller segments. This is only needed on a cartesian robot for zgrid, but always necessary for robots with rotational axes like Deltas.
1128 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1129 // The latter is more efficient and avoids splitting fast long lines into very small segments, like initial z move to 0, it is what Johanns Marlin delta port does
1132 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1135 } else if(this->delta_segments_per_second
> 1.0F
) {
1136 // enabled if set to something > 1, it is set to 0.0 by default
1137 // segment based on current speed and requested segments per second
1138 // the faster the travel speed the fewer segments needed
1139 // NOTE rate is mm/sec and we take into account any speed override
1140 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1141 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1142 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1145 if(this->mm_per_line_segment
== 0.0F
) {
1146 segments
= 1; // don't split it up
1148 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1154 // A vector to keep track of the endpoint of each segment
1155 float segment_delta
[n_motors
];
1156 float segment_end
[n_motors
];
1157 memcpy(segment_end
, last_milestone
, n_motors
*sizeof(float));
1159 // How far do we move each segment?
1160 for (int i
= 0; i
< n_motors
; i
++)
1161 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
1163 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1164 // We always add another point after this loop so we stop at segments-1, ie i < segments
1165 for (int i
= 1; i
< segments
; i
++) {
1166 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1167 for (int i
= 0; i
< n_motors
; i
++)
1168 segment_end
[i
] += segment_delta
[i
];
1170 // Append the end of this segment to the queue
1171 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1176 // Append the end of this full move to the queue
1177 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1179 this->next_command_is_MCS
= false; // always reset this
1185 // Append an arc to the queue ( cutting it into segments as needed )
1186 // TODO does not support any E parameters so cannot be used for 3D printing.
1187 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1189 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1190 // catch negative or zero feed rates and return the same error as GRBL does
1191 if(rate_mm_s
<= 0.0F
) {
1192 gcode
->is_error
= true;
1193 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1198 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1199 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1200 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
1201 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1202 float r_axis1
= -offset
[this->plane_axis_1
];
1203 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1204 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1206 // Patch from GRBL Firmware - Christoph Baumann 04072015
1207 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1208 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1209 if (is_clockwise
) { // Correct atan2 output per direction
1210 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1212 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1215 // Find the distance for this gcode
1216 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1218 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1219 if( millimeters_of_travel
< 0.00001F
) {
1223 // limit segments by maximum arc error
1224 float arc_segment
= this->mm_per_arc_segment
;
1225 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1226 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1227 if (this->mm_per_arc_segment
< min_err_segment
) {
1228 arc_segment
= min_err_segment
;
1231 // Figure out how many segments for this gcode
1232 // 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
1233 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1235 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1236 float theta_per_segment
= angular_travel
/ segments
;
1237 float linear_per_segment
= linear_travel
/ segments
;
1239 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1240 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1241 r_T = [cos(phi) -sin(phi);
1242 sin(phi) cos(phi] * r ;
1243 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1244 defined from the circle center to the initial position. Each line segment is formed by successive
1245 vector rotations. This requires only two cos() and sin() computations to form the rotation
1246 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1247 all float numbers are single precision on the Arduino. (True float precision will not have
1248 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1249 tool precision in some cases. Therefore, arc path correction is implemented.
1251 Small angle approximation may be used to reduce computation overhead further. This approximation
1252 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1253 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1254 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1255 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1256 issue for CNC machines with the single precision Arduino calculations.
1257 This approximation also allows mc_arc to immediately insert a line segment into the planner
1258 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1259 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1260 This is important when there are successive arc motions.
1262 // Vector rotation matrix values
1263 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1264 float sin_T
= theta_per_segment
;
1266 float arc_target
[3];
1273 // Initialize the linear axis
1274 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1277 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1278 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1280 if (count
< this->arc_correction
) {
1281 // Apply vector rotation matrix
1282 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1283 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1287 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1288 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1289 cos_Ti
= cosf(i
* theta_per_segment
);
1290 sin_Ti
= sinf(i
* theta_per_segment
);
1291 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1292 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1296 // Update arc_target location
1297 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1298 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1299 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1301 // Append this segment to the queue
1302 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1306 // Ensure last segment arrives at target location.
1307 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1312 // Do the math for an arc and add it to the queue
1313 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1317 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1319 // Set clockwise/counter-clockwise sign for mc_arc computations
1320 bool is_clockwise
= false;
1321 if( motion_mode
== CW_ARC
) {
1322 is_clockwise
= true;
1326 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1330 float Robot::theta(float x
, float y
)
1332 float t
= atanf(x
/ fabs(y
));
1344 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1346 this->plane_axis_0
= axis_0
;
1347 this->plane_axis_1
= axis_1
;
1348 this->plane_axis_2
= axis_2
;
1351 void Robot::clearToolOffset()
1353 this->tool_offset
= wcs_t(0,0,0);
1356 void Robot::setToolOffset(const float offset
[3])
1358 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1361 float Robot::get_feed_rate() const
1363 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;