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 ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
89 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
91 // 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
92 // It takes care of cutting arcs into segments, same thing for line that are too long
96 this->inch_mode
= false;
97 this->absolute_mode
= true;
98 this->e_absolute_mode
= true;
99 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
100 memset(this->last_milestone
, 0, sizeof last_milestone
);
101 memset(this->last_machine_position
, 0, sizeof last_machine_position
);
102 this->arm_solution
= NULL
;
103 seconds_per_minute
= 60.0F
;
104 this->clearToolOffset();
105 this->compensationTransform
= nullptr;
106 this->get_e_scale_fnc
= nullptr;
107 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
108 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
109 this->next_command_is_MCS
= false;
110 this->disable_segmentation
= false;
114 //Called when the module has just been loaded
115 void Robot::on_module_loaded()
117 this->register_for_event(ON_GCODE_RECEIVED
);
123 #define ACTUATOR_CHECKSUMS(X) { \
124 CHECKSUM(X "_step_pin"), \
125 CHECKSUM(X "_dir_pin"), \
126 CHECKSUM(X "_en_pin"), \
127 CHECKSUM(X "_steps_per_mm"), \
128 CHECKSUM(X "_max_rate"), \
129 CHECKSUM(X "_acceleration") \
132 void Robot::load_config()
134 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
135 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
136 // To make adding those solution easier, they have their own, separate object.
137 // Here we read the config to find out which arm solution to use
138 if (this->arm_solution
) delete this->arm_solution
;
139 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
140 // Note checksums are not const expressions when in debug mode, so don't use switch
141 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
142 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
144 } else if(solution_checksum
== corexz_checksum
) {
145 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
147 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
148 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
150 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
151 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
153 } else if(solution_checksum
== rotary_delta_checksum
) {
154 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== morgan_checksum
) {
157 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== cartesian_checksum
) {
160 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
163 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
166 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
167 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
168 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
169 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
170 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
171 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
172 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
174 // in mm/sec but specified in config as mm/min
175 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
176 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
177 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
179 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
180 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
182 // Make our Primary XYZ StepperMotors
183 uint16_t const checksums
[][6] = {
184 ACTUATOR_CHECKSUMS("alpha"), // X
185 ACTUATOR_CHECKSUMS("beta"), // Y
186 ACTUATOR_CHECKSUMS("gamma"), // Z
189 // default acceleration setting, can be overriden with newer per axis settings
190 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
193 for (size_t a
= X_AXIS
; a
<= Z_AXIS
; a
++) {
194 Pin pins
[3]; //step, dir, enable
195 for (size_t i
= 0; i
< 3; i
++) {
196 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
198 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
199 // register this motor (NB This must be 0,1,2) of the actuators array
200 uint8_t n
= register_motor(sm
);
202 // this is a fatal error
203 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
207 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
208 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
209 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
212 check_max_actuator_speeds(); // check the configs are sane
214 // if we have not specified a z acceleration see if the legacy config was set
215 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
216 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
218 actuators
[Z_AXIS
]->set_acceleration(acc
);
222 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
223 // so the first move can be correct if homing is not performed
224 ActuatorCoordinates actuator_pos
;
225 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
226 for (size_t i
= 0; i
< n_motors
; i
++)
227 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
229 //this->clearToolOffset();
232 uint8_t Robot::register_motor(StepperMotor
*motor
)
234 // register this motor with the step ticker
235 THEKERNEL
->step_ticker
->register_motor(motor
);
236 if(n_motors
>= k_max_actuators
) {
237 // this is a fatal error
238 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
241 actuators
.push_back(motor
);
245 void Robot::push_state()
247 bool am
= this->absolute_mode
;
248 bool em
= this->e_absolute_mode
;
249 bool im
= this->inch_mode
;
250 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
254 void Robot::pop_state()
256 if(!state_stack
.empty()) {
257 auto s
= state_stack
.top();
259 this->feed_rate
= std::get
<0>(s
);
260 this->seek_rate
= std::get
<1>(s
);
261 this->absolute_mode
= std::get
<2>(s
);
262 this->e_absolute_mode
= std::get
<3>(s
);
263 this->inch_mode
= std::get
<4>(s
);
264 this->current_wcs
= std::get
<5>(s
);
268 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
270 std::vector
<wcs_t
> v
;
271 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
272 for(auto& i
: wcs_offsets
) {
275 v
.push_back(g92_offset
);
276 v
.push_back(tool_offset
);
280 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
282 // M114.1 is a new way to do this (similar to how GRBL does it).
283 // it returns the realtime position based on the current step position of the actuators.
284 // this does require a FK to get a machine position from the actuator position
285 // and then invert all the transforms to get a workspace position from machine position
286 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
288 if(subcode
== 0) { // M114 print WCS
289 wcs_t pos
= mcs2wcs(last_milestone
);
290 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
)));
292 } 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)
293 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
]);
295 } 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)
296 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
]);
299 // get real time positions
300 // current actuator position in mm
301 ActuatorCoordinates current_position
{
302 actuators
[X_AXIS
]->get_current_position(),
303 actuators
[Y_AXIS
]->get_current_position(),
304 actuators
[Z_AXIS
]->get_current_position()
307 // get machine position from the actuator position using FK
309 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
311 if(subcode
== 1) { // M114.1 print realtime WCS
312 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
313 wcs_t pos
= mcs2wcs(mpos
);
314 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
)));
316 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
317 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
319 } else if(subcode
== 3) { // M114.3 print realtime actuator position
320 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
]);
326 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
327 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
329 return std::make_tuple(
330 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
),
331 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
),
332 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
)
336 // this does a sanity check that actuator speeds do not exceed steps rate capability
337 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
338 void Robot::check_max_actuator_speeds()
340 for (size_t i
= 0; i
< n_motors
; i
++) {
341 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
342 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
343 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
344 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());
349 //A GCode has been received
350 //See if the current Gcode line has some orders for us
351 void Robot::on_gcode_received(void *argument
)
353 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
355 enum MOTION_MODE_T motion_mode
= NONE
;
359 case 0: motion_mode
= SEEK
; break;
360 case 1: motion_mode
= LINEAR
; break;
361 case 2: motion_mode
= CW_ARC
; break;
362 case 3: motion_mode
= CCW_ARC
; break;
363 case 4: { // G4 pause
364 uint32_t delay_ms
= 0;
365 if (gcode
->has_letter('P')) {
366 delay_ms
= gcode
->get_int('P');
368 if (gcode
->has_letter('S')) {
369 delay_ms
+= gcode
->get_int('S') * 1000;
373 THEKERNEL
->conveyor
->wait_for_idle();
374 // wait for specified time
375 uint32_t start
= us_ticker_read(); // mbed call
376 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
377 THEKERNEL
->call_event(ON_IDLE
, this);
378 if(THEKERNEL
->is_halted()) return;
384 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
385 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
386 size_t n
= gcode
->get_uint('P');
387 if(n
== 0) n
= current_wcs
; // set current coordinate system
391 std::tie(x
, y
, z
) = wcs_offsets
[n
];
392 if(gcode
->get_int('L') == 20) {
393 // this makes the current machine position (less compensation transform) the offset
394 // get current position in WCS
395 wcs_t pos
= mcs2wcs(last_milestone
);
397 if(gcode
->has_letter('X')){
398 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
401 if(gcode
->has_letter('Y')){
402 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
404 if(gcode
->has_letter('Z')) {
405 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
409 // the value is the offset from machine zero
410 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
411 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
412 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
414 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
419 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
420 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
421 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
422 case 20: this->inch_mode
= true; break;
423 case 21: this->inch_mode
= false; break;
425 case 54: case 55: case 56: case 57: case 58: case 59:
426 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
427 current_wcs
= gcode
->g
- 54;
428 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
429 current_wcs
+= gcode
->subcode
;
430 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
434 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
435 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
438 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
439 // reset G92 offsets to 0
440 g92_offset
= wcs_t(0, 0, 0);
442 } else if(gcode
->subcode
== 3) {
443 // initialize G92 to the specified values, only used for saving it with M500
444 float x
= 0, y
= 0, z
= 0;
445 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
446 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
447 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
448 g92_offset
= wcs_t(x
, y
, z
);
451 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
453 std::tie(x
, y
, z
) = g92_offset
;
454 // get current position in WCS
455 wcs_t pos
= mcs2wcs(last_milestone
);
457 // adjust g92 offset to make the current wpos == the value requested
458 if(gcode
->has_letter('X')){
459 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
461 if(gcode
->has_letter('Y')){
462 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
464 if(gcode
->has_letter('Z')) {
465 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
467 g92_offset
= wcs_t(x
, y
, z
);
470 #if MAX_ROBOT_ACTUATORS > 3
471 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
472 // reset the E position, legacy for 3d Printers to be reprap compatible
473 // find the selected extruder
474 // 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
475 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
476 if(actuators
[i
]->is_selected()) {
477 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
478 last_milestone
[i
]= last_machine_position
[i
]= e
;
479 actuators
[i
]->change_last_milestone(e
);
490 } else if( gcode
->has_m
) {
492 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
493 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
496 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
497 if(!THEKERNEL
->is_grbl_mode()) break;
498 // fall through to M2
499 case 2: // M2 end of program
501 absolute_mode
= true;
504 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
507 case 18: // this used to support parameters, now it ignores them
509 THEKERNEL
->conveyor
->wait_for_idle();
510 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
513 case 82: e_absolute_mode
= true; break;
514 case 83: e_absolute_mode
= false; break;
516 case 92: // M92 - set steps per mm
517 if (gcode
->has_letter('X'))
518 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
519 if (gcode
->has_letter('Y'))
520 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
521 if (gcode
->has_letter('Z'))
522 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
524 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());
525 gcode
->add_nl
= true;
526 check_max_actuator_speeds();
531 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
532 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
536 case 120: // push state
540 case 121: // pop state
544 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
545 if(gcode
->get_num_args() == 0) {
546 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
547 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
549 gcode
->add_nl
= true;
552 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
553 if (gcode
->has_letter('X' + i
)) {
554 float v
= gcode
->get_value('X'+i
);
555 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
556 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
560 // this format is deprecated
561 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
562 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
563 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
564 if (gcode
->has_letter('A' + i
)) {
565 float v
= gcode
->get_value('A'+i
);
566 actuators
[i
]->set_max_rate(v
);
571 if(gcode
->subcode
== 1) check_max_actuator_speeds();
575 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
576 if (gcode
->has_letter('S')) {
577 float acc
= gcode
->get_value('S'); // mm/s^2
579 if (acc
< 1.0F
) acc
= 1.0F
;
580 this->default_acceleration
= acc
;
582 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
583 if (gcode
->has_letter(i
+'X')) {
584 float acc
= gcode
->get_value(i
+'X'); // mm/s^2
586 if (acc
<= 0.0F
) acc
= NAN
;
587 actuators
[i
]->set_acceleration(acc
);
592 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
593 if (gcode
->has_letter('X')) {
594 float jd
= gcode
->get_value('X');
598 THEKERNEL
->planner
->junction_deviation
= jd
;
600 if (gcode
->has_letter('Z')) {
601 float jd
= gcode
->get_value('Z');
602 // enforce minimum, -1 disables it and uses regular junction deviation
605 THEKERNEL
->planner
->z_junction_deviation
= jd
;
607 if (gcode
->has_letter('S')) {
608 float mps
= gcode
->get_value('S');
612 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
616 case 220: // M220 - speed override percentage
617 if (gcode
->has_letter('S')) {
618 float factor
= gcode
->get_value('S');
619 // enforce minimum 10% speed
622 // enforce maximum 10x speed
623 if (factor
> 1000.0F
)
626 seconds_per_minute
= 6000.0F
/ factor
;
628 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
632 case 400: // wait until all moves are done up to this point
633 THEKERNEL
->conveyor
->wait_for_idle();
636 case 500: // M500 saves some volatile settings to config override file
637 case 503: { // M503 just prints the settings
638 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());
640 // only print XYZ if not NAN
641 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
642 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
643 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", 'X'+i
, actuators
[i
]->get_acceleration());
645 gcode
->stream
->printf("\n");
647 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
);
649 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
]);
650 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());
652 // get or save any arm solution specific optional values
653 BaseSolution::arm_options_t options
;
654 if(arm_solution
->get_optional(options
) && !options
.empty()) {
655 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
656 for(auto &i
: options
) {
657 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
659 gcode
->stream
->printf("\n");
662 // save wcs_offsets and current_wcs
663 // TODO this may need to be done whenever they change to be compliant
664 gcode
->stream
->printf(";WCS settings\n");
665 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
667 for(auto &i
: wcs_offsets
) {
668 if(i
!= wcs_t(0, 0, 0)) {
670 std::tie(x
, y
, z
) = i
;
671 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
676 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
677 // 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
678 if(g92_offset
!= wcs_t(0, 0, 0)) {
680 std::tie(x
, y
, z
) = g92_offset
;
681 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
687 case 665: { // M665 set optional arm solution variables based on arm solution.
688 // the parameter args could be any letter each arm solution only accepts certain ones
689 BaseSolution::arm_options_t options
= gcode
->get_args();
690 options
.erase('S'); // don't include the S
691 options
.erase('U'); // don't include the U
692 if(options
.size() > 0) {
693 // set the specified options
694 arm_solution
->set_optional(options
);
697 if(arm_solution
->get_optional(options
)) {
698 // foreach optional value
699 for(auto &i
: options
) {
700 // print all current values of supported options
701 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
702 gcode
->add_nl
= true;
706 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
707 this->delta_segments_per_second
= gcode
->get_value('S');
708 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
710 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
711 this->mm_per_line_segment
= gcode
->get_value('U');
712 this->delta_segments_per_second
= 0;
713 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
721 if( motion_mode
!= NONE
) {
722 process_move(gcode
, motion_mode
);
725 next_command_is_MCS
= false; // must be on same line as G0 or G1
728 // process a G0/G1/G2/G3
729 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
731 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
732 // get XYZ and one E (which goes to the selected extruder)
733 float param
[4]{NAN
, NAN
, NAN
, NAN
};
735 // process primary axis
736 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
738 if( gcode
->has_letter(letter
) ) {
739 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
743 float offset
[3]{0,0,0};
744 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
745 if( gcode
->has_letter(letter
) ) {
746 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
750 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
751 float target
[n_motors
];
752 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
754 if(!next_command_is_MCS
) {
755 if(this->absolute_mode
) {
756 // apply wcs offsets and g92 offset and tool offset
757 if(!isnan(param
[X_AXIS
])) {
758 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
);
761 if(!isnan(param
[Y_AXIS
])) {
762 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
);
765 if(!isnan(param
[Z_AXIS
])) {
766 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
);
770 // they are deltas from the last_milestone if specified
771 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
772 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
777 // already in machine coordinates, we do not add tool offset for that
778 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
779 if(!isnan(param
[i
])) target
[i
] = param
[i
];
783 // process extruder parameters, for active extruder only (only one active extruder at a time)
784 selected_extruder
= 0;
785 if(gcode
->has_letter('E')) {
786 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
787 // find first selected extruder
788 if(actuators
[i
]->is_selected()) {
789 param
[E_AXIS
]= gcode
->get_value('E');
790 selected_extruder
= i
;
796 // do E for the selected extruder
798 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
799 if(this->e_absolute_mode
) {
800 target
[selected_extruder
]= param
[E_AXIS
];
801 delta_e
= target
[selected_extruder
] - last_milestone
[selected_extruder
];
803 delta_e
= param
[E_AXIS
];
804 target
[selected_extruder
] = delta_e
+ last_milestone
[selected_extruder
];
808 if( gcode
->has_letter('F') ) {
809 if( motion_mode
== SEEK
)
810 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
812 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
817 // Perform any physical actions
818 switch(motion_mode
) {
822 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
826 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
831 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
832 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
837 // set last_milestone to the calculated target
838 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
842 // reset the machine position for all axis. Used for homing.
843 // During homing compensation is turned off (actually not used as it drives steppers directly)
844 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
845 // so in those cases the final position is compensated.
846 void Robot::reset_axis_position(float x
, float y
, float z
)
848 // these are set to the same as compensation was not used to get to the current position
849 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
850 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
851 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
853 // now set the actuator positions to match
854 ActuatorCoordinates actuator_pos
;
855 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
856 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
857 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
860 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
861 void Robot::reset_axis_position(float position
, int axis
)
863 last_milestone
[axis
] = position
;
865 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
866 #if MAX_ROBOT_ACTUATORS > 3
868 // extruders need to be set not calculated
869 last_machine_position
[axis
]= position
;
874 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
875 // then sets the axis positions to match. currently only called from Endstops.cpp
876 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
878 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
879 actuators
[i
]->change_last_milestone(ac
[i
]);
881 // now correct axis positions then recorrect actuator to account for rounding
882 reset_position_from_current_actuator_position();
885 // Use FK to find out where actuator is and reset to match
886 void Robot::reset_position_from_current_actuator_position()
888 ActuatorCoordinates actuator_pos
;
889 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
890 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
891 actuator_pos
[i
] = actuators
[i
]->get_current_position();
894 // discover machine position from where actuators actually are
895 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
896 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
897 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
899 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
900 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
901 // to get everything in perfect sync.
902 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
903 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
904 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
907 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
908 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
909 // all transforms and is what we actually convert to actuator positions
910 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
912 float deltas
[n_motors
];
913 float transformed_target
[n_motors
]; // adjust target for bed compensation
914 float unit_vec
[N_PRIMARY_AXIS
];
916 // unity transform by default
917 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
919 // check function pointer and call if set to transform the target to compensate for bed
920 if(compensationTransform
) {
921 // some compensation strategies can transform XYZ, some just change Z
922 compensationTransform(transformed_target
);
926 float sos
= 0; // sun of squares for just XYZ
928 // find distance moved by each axis, use transformed target from the current machine position
929 for (size_t i
= 0; i
< n_motors
; i
++) {
930 deltas
[i
] = transformed_target
[i
] - last_machine_position
[i
];
931 if(deltas
[i
] == 0) continue;
932 // at least one non zero delta
935 sos
+= powf(deltas
[i
], 2);
940 if(!move
) return false;
942 // see if this is a primary axis move or not
943 bool auxilliary_move
= deltas
[X_AXIS
] == 0 && deltas
[Y_AXIS
] == 0 && deltas
[Z_AXIS
] == 0;
945 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
946 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
948 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
949 // 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
950 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
953 if(!auxilliary_move
) {
954 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
955 // find distance unit vector for primary axis only
956 unit_vec
[i
] = deltas
[i
] / distance
;
958 // Do not move faster than the configured cartesian limits for XYZ
959 if ( max_speeds
[i
] > 0 ) {
960 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
962 if (axis_speed
> max_speeds
[i
])
963 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
968 // find actuator position given the machine position, use actual adjusted target
969 ActuatorCoordinates actuator_pos
;
970 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
972 #if MAX_ROBOT_ACTUATORS > 3
974 // for the extruders just copy the position, and possibly scale it from mm³ to mm
975 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
976 actuator_pos
[i
]= transformed_target
[i
];
977 if(get_e_scale_fnc
) {
978 // NOTE this relies on the fact only one extruder is active at a time
979 // scale for volumetric or flow rate
980 // TODO is this correct? scaling the absolute target? what if the scale changes?
981 // for volumetric it basically converts mm³ to mm, but what about flow rate?
982 actuator_pos
[i
] *= get_e_scale_fnc();
984 if(auxilliary_move
) {
985 // for E only moves we need to use the scaled E to calculate the distance
986 sos
+= pow(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
989 if(auxilliary_move
) {
990 distance
= sqrtf(sos
); // distance in mm of the e move
991 if(distance
< 0.00001F
) return false;
995 // use default acceleration to start with
996 float acceleration
= default_acceleration
;
998 float isecs
= rate_mm_s
/ distance
;
1000 // check per-actuator speed limits
1001 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1002 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1003 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1005 float actuator_rate
= d
* isecs
;
1006 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1007 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1008 isecs
= rate_mm_s
/ distance
;
1011 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1012 // 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.
1013 if(auxilliary_move
|| actuator
<= Z_AXIS
) {
1014 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1015 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1016 float ca
= fabsf((d
/distance
) * acceleration
);
1018 acceleration
*= ( ma
/ ca
);
1024 // Append the block to the planner
1025 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1026 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
)) {
1027 // this is the machine position
1028 memcpy(this->last_machine_position
, transformed_target
, n_motors
*sizeof(float));
1035 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1036 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1038 if(THEKERNEL
->is_halted()) return false;
1040 // catch negative or zero feed rates
1041 if(rate_mm_s
<= 0.0F
) {
1045 // get the absolute target position, default is current last_milestone
1046 float target
[n_motors
];
1047 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
1049 // add in the deltas to get new target
1050 for (int i
= 0; i
< naxis
; i
++) {
1051 target
[i
] += delta
[i
];
1054 // submit for planning and if moved update last_milestone
1055 if(append_milestone(target
, rate_mm_s
)) {
1056 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
1063 // Append a move to the queue ( cutting it into segments if needed )
1064 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1066 // catch negative or zero feed rates and return the same error as GRBL does
1067 if(rate_mm_s
<= 0.0F
) {
1068 gcode
->is_error
= true;
1069 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1073 // Find out the distance for this move in XYZ in MCS
1074 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 ));
1076 if(millimeters_of_travel
< 0.00001F
) {
1077 // we have no movement in XYZ, probably E only extrude or retract
1078 return this->append_milestone(target
, rate_mm_s
);
1082 For extruders, we need to do some extra work...
1083 if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit.
1084 Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
1085 We ask Extruder to do all the work but we need to pass in the relevant data.
1086 NOTE we need to do this before we segment the line (for deltas)
1087 This also sets any scaling due to flow rate and volumetric if a G1
1089 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1090 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1091 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1092 rate_mm_s
*= data
[1]; // adjust the feedrate
1096 // 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.
1097 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1098 // 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
1101 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1104 } else if(this->delta_segments_per_second
> 1.0F
) {
1105 // enabled if set to something > 1, it is set to 0.0 by default
1106 // segment based on current speed and requested segments per second
1107 // the faster the travel speed the fewer segments needed
1108 // NOTE rate is mm/sec and we take into account any speed override
1109 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1110 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1111 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1114 if(this->mm_per_line_segment
== 0.0F
) {
1115 segments
= 1; // don't split it up
1117 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1123 // A vector to keep track of the endpoint of each segment
1124 float segment_delta
[n_motors
];
1125 float segment_end
[n_motors
];
1126 memcpy(segment_end
, last_milestone
, n_motors
*sizeof(float));
1128 // How far do we move each segment?
1129 for (int i
= 0; i
< n_motors
; i
++)
1130 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
1132 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1133 // We always add another point after this loop so we stop at segments-1, ie i < segments
1134 for (int i
= 1; i
< segments
; i
++) {
1135 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1136 for (int i
= 0; i
< n_motors
; i
++)
1137 segment_end
[i
] += segment_delta
[i
];
1139 // Append the end of this segment to the queue
1140 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1145 // Append the end of this full move to the queue
1146 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1148 this->next_command_is_MCS
= false; // always reset this
1154 // Append an arc to the queue ( cutting it into segments as needed )
1155 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1157 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1158 // catch negative or zero feed rates and return the same error as GRBL does
1159 if(rate_mm_s
<= 0.0F
) {
1160 gcode
->is_error
= true;
1161 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1166 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1167 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1168 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
1169 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1170 float r_axis1
= -offset
[this->plane_axis_1
];
1171 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1172 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1174 // Patch from GRBL Firmware - Christoph Baumann 04072015
1175 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1176 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1177 if (is_clockwise
) { // Correct atan2 output per direction
1178 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1180 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1183 // Find the distance for this gcode
1184 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1186 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1187 if( millimeters_of_travel
< 0.00001F
) {
1191 // limit segments by maximum arc error
1192 float arc_segment
= this->mm_per_arc_segment
;
1193 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1194 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1195 if (this->mm_per_arc_segment
< min_err_segment
) {
1196 arc_segment
= min_err_segment
;
1199 // Figure out how many segments for this gcode
1200 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1202 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1203 float theta_per_segment
= angular_travel
/ segments
;
1204 float linear_per_segment
= linear_travel
/ segments
;
1206 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1207 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1208 r_T = [cos(phi) -sin(phi);
1209 sin(phi) cos(phi] * r ;
1210 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1211 defined from the circle center to the initial position. Each line segment is formed by successive
1212 vector rotations. This requires only two cos() and sin() computations to form the rotation
1213 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1214 all float numbers are single precision on the Arduino. (True float precision will not have
1215 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1216 tool precision in some cases. Therefore, arc path correction is implemented.
1218 Small angle approximation may be used to reduce computation overhead further. This approximation
1219 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1220 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1221 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1222 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1223 issue for CNC machines with the single precision Arduino calculations.
1224 This approximation also allows mc_arc to immediately insert a line segment into the planner
1225 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1226 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1227 This is important when there are successive arc motions.
1229 // Vector rotation matrix values
1230 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1231 float sin_T
= theta_per_segment
;
1233 float arc_target
[3];
1240 // Initialize the linear axis
1241 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1244 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1245 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1247 if (count
< this->arc_correction
) {
1248 // Apply vector rotation matrix
1249 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1250 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1254 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1255 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1256 cos_Ti
= cosf(i
* theta_per_segment
);
1257 sin_Ti
= sinf(i
* theta_per_segment
);
1258 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1259 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1263 // Update arc_target location
1264 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1265 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1266 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1268 // Append this segment to the queue
1269 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1273 // Ensure last segment arrives at target location.
1274 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1279 // Do the math for an arc and add it to the queue
1280 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1284 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1286 // Set clockwise/counter-clockwise sign for mc_arc computations
1287 bool is_clockwise
= false;
1288 if( motion_mode
== CW_ARC
) {
1289 is_clockwise
= true;
1293 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1297 float Robot::theta(float x
, float y
)
1299 float t
= atanf(x
/ fabs(y
));
1311 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1313 this->plane_axis_0
= axis_0
;
1314 this->plane_axis_1
= axis_1
;
1315 this->plane_axis_2
= axis_2
;
1318 void Robot::clearToolOffset()
1320 this->tool_offset
= wcs_t(0,0,0);
1323 void Robot::setToolOffset(const float offset
[3])
1325 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1328 float Robot::get_feed_rate() const
1330 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;