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")
57 #define set_g92_checksum CHECKSUM("set_g92")
60 #define arm_solution_checksum CHECKSUM("arm_solution")
61 #define cartesian_checksum CHECKSUM("cartesian")
62 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
63 #define rostock_checksum CHECKSUM("rostock")
64 #define linear_delta_checksum CHECKSUM("linear_delta")
65 #define rotary_delta_checksum CHECKSUM("rotary_delta")
66 #define delta_checksum CHECKSUM("delta")
67 #define hbot_checksum CHECKSUM("hbot")
68 #define corexy_checksum CHECKSUM("corexy")
69 #define corexz_checksum CHECKSUM("corexz")
70 #define kossel_checksum CHECKSUM("kossel")
71 #define morgan_checksum CHECKSUM("morgan")
73 // new-style actuator stuff
74 #define actuator_checksum CHEKCSUM("actuator")
76 #define step_pin_checksum CHECKSUM("step_pin")
77 #define dir_pin_checksum CHEKCSUM("dir_pin")
78 #define en_pin_checksum CHECKSUM("en_pin")
80 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
81 #define max_rate_checksum CHECKSUM("max_rate")
82 #define acceleration_checksum CHECKSUM("acceleration")
83 #define z_acceleration_checksum CHECKSUM("z_acceleration")
85 #define alpha_checksum CHECKSUM("alpha")
86 #define beta_checksum CHECKSUM("beta")
87 #define gamma_checksum CHECKSUM("gamma")
89 #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
91 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
92 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
94 // 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
95 // It takes care of cutting arcs into segments, same thing for line that are too long
99 this->inch_mode
= false;
100 this->absolute_mode
= true;
101 this->e_absolute_mode
= true;
102 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
103 memset(this->last_milestone
, 0, sizeof last_milestone
);
104 memset(this->last_machine_position
, 0, sizeof last_machine_position
);
105 this->arm_solution
= NULL
;
106 seconds_per_minute
= 60.0F
;
107 this->clearToolOffset();
108 this->compensationTransform
= nullptr;
109 this->get_e_scale_fnc
= nullptr;
110 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
111 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
112 this->next_command_is_MCS
= false;
113 this->disable_segmentation
= false;
114 this->disable_arm_solution
= false;
118 //Called when the module has just been loaded
119 void Robot::on_module_loaded()
121 this->register_for_event(ON_GCODE_RECEIVED
);
127 #define ACTUATOR_CHECKSUMS(X) { \
128 CHECKSUM(X "_step_pin"), \
129 CHECKSUM(X "_dir_pin"), \
130 CHECKSUM(X "_en_pin"), \
131 CHECKSUM(X "_steps_per_mm"), \
132 CHECKSUM(X "_max_rate"), \
133 CHECKSUM(X "_acceleration") \
136 void Robot::load_config()
138 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
139 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
140 // To make adding those solution easier, they have their own, separate object.
141 // Here we read the config to find out which arm solution to use
142 if (this->arm_solution
) delete this->arm_solution
;
143 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
144 // Note checksums are not const expressions when in debug mode, so don't use switch
145 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
146 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
148 } else if(solution_checksum
== corexz_checksum
) {
149 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
151 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
152 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
154 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
155 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
157 } else if(solution_checksum
== rotary_delta_checksum
) {
158 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
160 } else if(solution_checksum
== morgan_checksum
) {
161 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
163 } else if(solution_checksum
== cartesian_checksum
) {
164 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
167 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
170 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
171 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
172 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
173 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
174 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
175 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
176 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
178 // in mm/sec but specified in config as mm/min
179 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
180 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
181 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
183 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
184 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
185 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
187 // optional setting for a fixed G92 offset
188 std::vector
<float> t
= parse_number_list(g92
.c_str());
190 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
194 // default s value for laser
195 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
197 // Make our Primary XYZ StepperMotors
198 uint16_t const checksums
[][6] = {
199 ACTUATOR_CHECKSUMS("alpha"), // X
200 ACTUATOR_CHECKSUMS("beta"), // Y
201 ACTUATOR_CHECKSUMS("gamma"), // Z
204 // default acceleration setting, can be overriden with newer per axis settings
205 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
208 for (size_t a
= X_AXIS
; a
<= Z_AXIS
; a
++) {
209 Pin pins
[3]; //step, dir, enable
210 for (size_t i
= 0; i
< 3; i
++) {
211 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
213 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
214 // register this motor (NB This must be 0,1,2) of the actuators array
215 uint8_t n
= register_motor(sm
);
217 // this is a fatal error
218 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
222 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
223 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
224 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
227 check_max_actuator_speeds(); // check the configs are sane
229 // if we have not specified a z acceleration see if the legacy config was set
230 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
231 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
233 actuators
[Z_AXIS
]->set_acceleration(acc
);
237 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
238 // so the first move can be correct if homing is not performed
239 ActuatorCoordinates actuator_pos
;
240 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
241 for (size_t i
= 0; i
< n_motors
; i
++)
242 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
244 //this->clearToolOffset();
247 uint8_t Robot::register_motor(StepperMotor
*motor
)
249 // register this motor with the step ticker
250 THEKERNEL
->step_ticker
->register_motor(motor
);
251 if(n_motors
>= k_max_actuators
) {
252 // this is a fatal error
253 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
256 actuators
.push_back(motor
);
260 void Robot::push_state()
262 bool am
= this->absolute_mode
;
263 bool em
= this->e_absolute_mode
;
264 bool im
= this->inch_mode
;
265 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
269 void Robot::pop_state()
271 if(!state_stack
.empty()) {
272 auto s
= state_stack
.top();
274 this->feed_rate
= std::get
<0>(s
);
275 this->seek_rate
= std::get
<1>(s
);
276 this->absolute_mode
= std::get
<2>(s
);
277 this->e_absolute_mode
= std::get
<3>(s
);
278 this->inch_mode
= std::get
<4>(s
);
279 this->current_wcs
= std::get
<5>(s
);
283 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
285 std::vector
<wcs_t
> v
;
286 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
287 for(auto& i
: wcs_offsets
) {
290 v
.push_back(g92_offset
);
291 v
.push_back(tool_offset
);
295 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
297 // M114.1 is a new way to do this (similar to how GRBL does it).
298 // it returns the realtime position based on the current step position of the actuators.
299 // this does require a FK to get a machine position from the actuator position
300 // and then invert all the transforms to get a workspace position from machine position
301 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
303 if(subcode
== 0) { // M114 print WCS
304 wcs_t pos
= mcs2wcs(last_milestone
);
305 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
)));
307 } 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)
308 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
]);
310 } 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)
311 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
]);
314 // get real time positions
315 // current actuator position in mm
316 ActuatorCoordinates current_position
{
317 actuators
[X_AXIS
]->get_current_position(),
318 actuators
[Y_AXIS
]->get_current_position(),
319 actuators
[Z_AXIS
]->get_current_position()
322 // get machine position from the actuator position using FK
324 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
326 if(subcode
== 1) { // M114.1 print realtime WCS
327 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
328 wcs_t pos
= mcs2wcs(mpos
);
329 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
)));
331 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
332 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
334 } else if(subcode
== 3) { // M114.3 print realtime actuator position
335 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
]);
341 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
342 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
344 return std::make_tuple(
345 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
),
346 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
),
347 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
)
351 // this does a sanity check that actuator speeds do not exceed steps rate capability
352 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
353 void Robot::check_max_actuator_speeds()
355 for (size_t i
= 0; i
< n_motors
; i
++) {
356 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
357 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
358 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
359 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());
364 //A GCode has been received
365 //See if the current Gcode line has some orders for us
366 void Robot::on_gcode_received(void *argument
)
368 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
370 enum MOTION_MODE_T motion_mode
= NONE
;
374 case 0: motion_mode
= SEEK
; break;
375 case 1: motion_mode
= LINEAR
; break;
376 case 2: motion_mode
= CW_ARC
; break;
377 case 3: motion_mode
= CCW_ARC
; break;
378 case 4: { // G4 pause
379 uint32_t delay_ms
= 0;
380 if (gcode
->has_letter('P')) {
381 delay_ms
= gcode
->get_int('P');
383 if (gcode
->has_letter('S')) {
384 delay_ms
+= gcode
->get_int('S') * 1000;
388 THEKERNEL
->conveyor
->wait_for_idle();
389 // wait for specified time
390 uint32_t start
= us_ticker_read(); // mbed call
391 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
392 THEKERNEL
->call_event(ON_IDLE
, this);
393 if(THEKERNEL
->is_halted()) return;
399 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
400 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
401 size_t n
= gcode
->get_uint('P');
402 if(n
== 0) n
= current_wcs
; // set current coordinate system
406 std::tie(x
, y
, z
) = wcs_offsets
[n
];
407 if(gcode
->get_int('L') == 20) {
408 // this makes the current machine position (less compensation transform) the offset
409 // get current position in WCS
410 wcs_t pos
= mcs2wcs(last_milestone
);
412 if(gcode
->has_letter('X')){
413 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
416 if(gcode
->has_letter('Y')){
417 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
419 if(gcode
->has_letter('Z')) {
420 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
424 // the value is the offset from machine zero
425 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
426 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
427 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
429 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
434 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
435 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
436 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
437 case 20: this->inch_mode
= true; break;
438 case 21: this->inch_mode
= false; break;
440 case 54: case 55: case 56: case 57: case 58: case 59:
441 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
442 current_wcs
= gcode
->g
- 54;
443 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
444 current_wcs
+= gcode
->subcode
;
445 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
449 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
450 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
453 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
454 // reset G92 offsets to 0
455 g92_offset
= wcs_t(0, 0, 0);
457 } else if(gcode
->subcode
== 3) {
458 // initialize G92 to the specified values, only used for saving it with M500
459 float x
= 0, y
= 0, z
= 0;
460 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
461 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
462 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
463 g92_offset
= wcs_t(x
, y
, z
);
466 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
468 std::tie(x
, y
, z
) = g92_offset
;
469 // get current position in WCS
470 wcs_t pos
= mcs2wcs(last_milestone
);
472 // adjust g92 offset to make the current wpos == the value requested
473 if(gcode
->has_letter('X')){
474 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
476 if(gcode
->has_letter('Y')){
477 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
479 if(gcode
->has_letter('Z')) {
480 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
482 g92_offset
= wcs_t(x
, y
, z
);
485 #if MAX_ROBOT_ACTUATORS > 3
486 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
487 // reset the E position, legacy for 3d Printers to be reprap compatible
488 // find the selected extruder
489 // 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
490 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
491 if(actuators
[i
]->is_selected()) {
492 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
493 last_milestone
[i
]= last_machine_position
[i
]= e
;
494 actuators
[i
]->change_last_milestone(e
);
505 } else if( gcode
->has_m
) {
507 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
508 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
511 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
512 if(!THEKERNEL
->is_grbl_mode()) break;
513 // fall through to M2
514 case 2: // M2 end of program
516 absolute_mode
= true;
519 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
522 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
523 if(gcode
->get_num_args() > 0) {
524 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
526 for (int i
= 0; i
< n_motors
; ++i
) {
527 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
528 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
530 // handle E parameter as currently selected extruder ABC
531 if(gcode
->has_letter('E')) {
532 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
533 // find first selected extruder
534 if(actuators
[i
]->is_selected()) {
535 bm
|= (0x02<<i
); // set appropriate bit
541 THEKERNEL
->conveyor
->wait_for_idle();
542 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
547 THEKERNEL
->conveyor
->wait_for_idle();
548 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
551 case 82: e_absolute_mode
= true; break;
552 case 83: e_absolute_mode
= false; break;
554 case 92: // M92 - set steps per mm
555 if (gcode
->has_letter('X'))
556 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
557 if (gcode
->has_letter('Y'))
558 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
559 if (gcode
->has_letter('Z'))
560 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
562 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());
563 gcode
->add_nl
= true;
564 check_max_actuator_speeds();
569 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
570 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
574 case 120: // push state
578 case 121: // pop state
582 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
583 if(gcode
->get_num_args() == 0) {
584 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
585 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
587 gcode
->add_nl
= true;
590 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
591 if (gcode
->has_letter('X' + i
)) {
592 float v
= gcode
->get_value('X'+i
);
593 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
594 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
598 // this format is deprecated
599 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
600 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
601 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
602 if (gcode
->has_letter('A' + i
)) {
603 float v
= gcode
->get_value('A'+i
);
604 actuators
[i
]->set_max_rate(v
);
609 if(gcode
->subcode
== 1) check_max_actuator_speeds();
613 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
614 if (gcode
->has_letter('S')) {
615 float acc
= gcode
->get_value('S'); // mm/s^2
617 if (acc
< 1.0F
) acc
= 1.0F
;
618 this->default_acceleration
= acc
;
620 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
621 if (gcode
->has_letter(i
+'X')) {
622 float acc
= gcode
->get_value(i
+'X'); // mm/s^2
624 if (acc
<= 0.0F
) acc
= NAN
;
625 actuators
[i
]->set_acceleration(acc
);
630 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
631 if (gcode
->has_letter('X')) {
632 float jd
= gcode
->get_value('X');
636 THEKERNEL
->planner
->junction_deviation
= jd
;
638 if (gcode
->has_letter('Z')) {
639 float jd
= gcode
->get_value('Z');
640 // enforce minimum, -1 disables it and uses regular junction deviation
643 THEKERNEL
->planner
->z_junction_deviation
= jd
;
645 if (gcode
->has_letter('S')) {
646 float mps
= gcode
->get_value('S');
650 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
654 case 220: // M220 - speed override percentage
655 if (gcode
->has_letter('S')) {
656 float factor
= gcode
->get_value('S');
657 // enforce minimum 10% speed
660 // enforce maximum 10x speed
661 if (factor
> 1000.0F
)
664 seconds_per_minute
= 6000.0F
/ factor
;
666 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
670 case 400: // wait until all moves are done up to this point
671 THEKERNEL
->conveyor
->wait_for_idle();
674 case 500: // M500 saves some volatile settings to config override file
675 case 503: { // M503 just prints the settings
676 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());
678 // only print XYZ if not NAN
679 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
680 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
681 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", 'X'+i
, actuators
[i
]->get_acceleration());
683 gcode
->stream
->printf("\n");
685 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
);
687 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
]);
688 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());
690 // get or save any arm solution specific optional values
691 BaseSolution::arm_options_t options
;
692 if(arm_solution
->get_optional(options
) && !options
.empty()) {
693 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
694 for(auto &i
: options
) {
695 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
697 gcode
->stream
->printf("\n");
700 // save wcs_offsets and current_wcs
701 // TODO this may need to be done whenever they change to be compliant
702 gcode
->stream
->printf(";WCS settings\n");
703 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
705 for(auto &i
: wcs_offsets
) {
706 if(i
!= wcs_t(0, 0, 0)) {
708 std::tie(x
, y
, z
) = i
;
709 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
714 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
715 // 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
716 if(g92_offset
!= wcs_t(0, 0, 0)) {
718 std::tie(x
, y
, z
) = g92_offset
;
719 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
725 case 665: { // M665 set optional arm solution variables based on arm solution.
726 // the parameter args could be any letter each arm solution only accepts certain ones
727 BaseSolution::arm_options_t options
= gcode
->get_args();
728 options
.erase('S'); // don't include the S
729 options
.erase('U'); // don't include the U
730 if(options
.size() > 0) {
731 // set the specified options
732 arm_solution
->set_optional(options
);
735 if(arm_solution
->get_optional(options
)) {
736 // foreach optional value
737 for(auto &i
: options
) {
738 // print all current values of supported options
739 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
740 gcode
->add_nl
= true;
744 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
745 this->delta_segments_per_second
= gcode
->get_value('S');
746 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
748 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
749 this->mm_per_line_segment
= gcode
->get_value('U');
750 this->delta_segments_per_second
= 0;
751 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
759 if( motion_mode
!= NONE
) {
760 is_g123
= motion_mode
!= SEEK
;
761 process_move(gcode
, motion_mode
);
767 next_command_is_MCS
= false; // must be on same line as G0 or G1
770 // process a G0/G1/G2/G3
771 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
773 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
774 // get XYZ and one E (which goes to the selected extruder)
775 float param
[4]{NAN
, NAN
, NAN
, NAN
};
777 // process primary axis
778 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
780 if( gcode
->has_letter(letter
) ) {
781 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
785 float offset
[3]{0,0,0};
786 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
787 if( gcode
->has_letter(letter
) ) {
788 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
792 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
793 float target
[n_motors
];
794 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
796 if(!next_command_is_MCS
) {
797 if(this->absolute_mode
) {
798 // apply wcs offsets and g92 offset and tool offset
799 if(!isnan(param
[X_AXIS
])) {
800 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
);
803 if(!isnan(param
[Y_AXIS
])) {
804 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
);
807 if(!isnan(param
[Z_AXIS
])) {
808 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
);
812 // they are deltas from the last_milestone if specified
813 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
814 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
819 // already in machine coordinates, we do not add tool offset for that
820 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
821 if(!isnan(param
[i
])) target
[i
] = param
[i
];
825 // process extruder parameters, for active extruder only (only one active extruder at a time)
826 selected_extruder
= 0;
827 if(gcode
->has_letter('E')) {
828 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
829 // find first selected extruder
830 if(actuators
[i
]->is_selected()) {
831 param
[E_AXIS
]= gcode
->get_value('E');
832 selected_extruder
= i
;
838 // do E for the selected extruder
840 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
841 if(this->e_absolute_mode
) {
842 target
[selected_extruder
]= param
[E_AXIS
];
843 delta_e
= target
[selected_extruder
] - last_milestone
[selected_extruder
];
845 delta_e
= param
[E_AXIS
];
846 target
[selected_extruder
] = delta_e
+ last_milestone
[selected_extruder
];
850 if( gcode
->has_letter('F') ) {
851 if( motion_mode
== SEEK
)
852 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
854 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
857 // S is modal When specified on a G0/1/2/3 command
858 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
862 // Perform any physical actions
863 switch(motion_mode
) {
867 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
871 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
876 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
877 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
882 // set last_milestone to the calculated target
883 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
887 // reset the machine position for all axis. Used for homing.
888 // During homing compensation is turned off
889 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
890 // so in those cases the final position is compensated.
891 void Robot::reset_axis_position(float x
, float y
, float z
)
893 // these are set to the same as compensation was not used to get to the current position
894 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
895 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
896 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
898 // now set the actuator positions to match
899 ActuatorCoordinates actuator_pos
;
900 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
901 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
902 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
905 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
906 void Robot::reset_axis_position(float position
, int axis
)
908 last_milestone
[axis
] = position
;
910 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
911 #if MAX_ROBOT_ACTUATORS > 3
913 // extruders need to be set not calculated
914 last_machine_position
[axis
]= position
;
919 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
920 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
921 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
923 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
924 actuators
[i
]->change_last_milestone(ac
[i
]);
926 // now correct axis positions then recorrect actuator to account for rounding
927 reset_position_from_current_actuator_position();
930 // Use FK to find out where actuator is and reset to match
931 void Robot::reset_position_from_current_actuator_position()
933 ActuatorCoordinates actuator_pos
;
934 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
935 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
936 actuator_pos
[i
] = actuators
[i
]->get_current_position();
939 // discover machine position from where actuators actually are
940 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
941 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
942 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
944 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
945 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
946 // to get everything in perfect sync.
947 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
948 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
949 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
952 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
953 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
954 // all transforms and is what we actually convert to actuator positions
955 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
957 float deltas
[n_motors
];
958 float transformed_target
[n_motors
]; // adjust target for bed compensation
959 float unit_vec
[N_PRIMARY_AXIS
];
961 // unity transform by default
962 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
964 // check function pointer and call if set to transform the target to compensate for bed
965 if(compensationTransform
) {
966 // some compensation strategies can transform XYZ, some just change Z
967 compensationTransform(transformed_target
);
971 float sos
= 0; // sun of squares for just XYZ
973 // find distance moved by each axis, use transformed target from the current machine position
974 for (size_t i
= 0; i
< n_motors
; i
++) {
975 deltas
[i
] = transformed_target
[i
] - last_machine_position
[i
];
976 if(deltas
[i
] == 0) continue;
977 // at least one non zero delta
980 sos
+= powf(deltas
[i
], 2);
985 if(!move
) return false;
987 // see if this is a primary axis move or not
988 bool auxilliary_move
= deltas
[X_AXIS
] == 0 && deltas
[Y_AXIS
] == 0 && deltas
[Z_AXIS
] == 0;
990 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
991 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
993 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
994 // 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
995 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
998 if(!auxilliary_move
) {
999 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1000 // find distance unit vector for primary axis only
1001 unit_vec
[i
] = deltas
[i
] / distance
;
1003 // Do not move faster than the configured cartesian limits for XYZ
1004 if ( max_speeds
[i
] > 0 ) {
1005 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1007 if (axis_speed
> max_speeds
[i
])
1008 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1013 // find actuator position given the machine position, use actual adjusted target
1014 ActuatorCoordinates actuator_pos
;
1015 if(!disable_arm_solution
) {
1016 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1019 // basically the same as cartesian, would be used for special homing situations like for scara
1020 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1021 actuator_pos
[i
] = transformed_target
[i
];
1025 #if MAX_ROBOT_ACTUATORS > 3
1027 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1028 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1029 actuator_pos
[i
]= transformed_target
[i
];
1030 if(get_e_scale_fnc
) {
1031 // NOTE this relies on the fact only one extruder is active at a time
1032 // scale for volumetric or flow rate
1033 // TODO is this correct? scaling the absolute target? what if the scale changes?
1034 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1035 actuator_pos
[i
] *= get_e_scale_fnc();
1037 if(auxilliary_move
) {
1038 // for E only moves we need to use the scaled E to calculate the distance
1039 sos
+= pow(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1042 if(auxilliary_move
) {
1043 distance
= sqrtf(sos
); // distance in mm of the e move
1044 if(distance
< 0.00001F
) return false;
1048 // use default acceleration to start with
1049 float acceleration
= default_acceleration
;
1051 float isecs
= rate_mm_s
/ distance
;
1053 // check per-actuator speed limits
1054 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1055 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1056 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1058 float actuator_rate
= d
* isecs
;
1059 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1060 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1061 isecs
= rate_mm_s
/ distance
;
1064 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1065 // 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.
1066 if(auxilliary_move
|| actuator
<= Z_AXIS
) {
1067 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1068 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1069 float ca
= fabsf((d
/distance
) * acceleration
);
1071 acceleration
*= ( ma
/ ca
);
1077 // Append the block to the planner
1078 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1079 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1080 // this is the machine position
1081 memcpy(this->last_machine_position
, transformed_target
, n_motors
*sizeof(float));
1089 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1090 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1092 if(THEKERNEL
->is_halted()) return false;
1094 // catch negative or zero feed rates
1095 if(rate_mm_s
<= 0.0F
) {
1099 // get the absolute target position, default is current last_milestone
1100 float target
[n_motors
];
1101 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
1103 // add in the deltas to get new target
1104 for (int i
= 0; i
< naxis
; i
++) {
1105 target
[i
] += delta
[i
];
1108 // submit for planning and if moved update last_milestone
1109 if(append_milestone(target
, rate_mm_s
)) {
1110 memcpy(last_milestone
, target
, n_motors
*sizeof(float));
1117 // Append a move to the queue ( cutting it into segments if needed )
1118 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1120 // catch negative or zero feed rates and return the same error as GRBL does
1121 if(rate_mm_s
<= 0.0F
) {
1122 gcode
->is_error
= true;
1123 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1127 // Find out the distance for this move in XYZ in MCS
1128 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 ));
1130 if(millimeters_of_travel
< 0.00001F
) {
1131 // we have no movement in XYZ, probably E only extrude or retract
1132 return this->append_milestone(target
, rate_mm_s
);
1136 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1137 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1138 We ask Extruder to do all the work but we need to pass in the relevant data.
1139 NOTE we need to do this before we segment the line (for deltas)
1141 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1142 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1143 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1144 rate_mm_s
*= data
[1]; // adjust the feedrate
1148 // 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.
1149 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1150 // 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
1153 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1156 } else if(this->delta_segments_per_second
> 1.0F
) {
1157 // enabled if set to something > 1, it is set to 0.0 by default
1158 // segment based on current speed and requested segments per second
1159 // the faster the travel speed the fewer segments needed
1160 // NOTE rate is mm/sec and we take into account any speed override
1161 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1162 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1163 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1166 if(this->mm_per_line_segment
== 0.0F
) {
1167 segments
= 1; // don't split it up
1169 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1175 // A vector to keep track of the endpoint of each segment
1176 float segment_delta
[n_motors
];
1177 float segment_end
[n_motors
];
1178 memcpy(segment_end
, last_milestone
, n_motors
*sizeof(float));
1180 // How far do we move each segment?
1181 for (int i
= 0; i
< n_motors
; i
++)
1182 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
1184 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1185 // We always add another point after this loop so we stop at segments-1, ie i < segments
1186 for (int i
= 1; i
< segments
; i
++) {
1187 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1188 for (int i
= 0; i
< n_motors
; i
++)
1189 segment_end
[i
] += segment_delta
[i
];
1191 // Append the end of this segment to the queue
1192 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1197 // Append the end of this full move to the queue
1198 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1200 this->next_command_is_MCS
= false; // always reset this
1206 // Append an arc to the queue ( cutting it into segments as needed )
1207 // TODO does not support any E parameters so cannot be used for 3D printing.
1208 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1210 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1211 // catch negative or zero feed rates and return the same error as GRBL does
1212 if(rate_mm_s
<= 0.0F
) {
1213 gcode
->is_error
= true;
1214 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1219 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1220 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1221 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
1222 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1223 float r_axis1
= -offset
[this->plane_axis_1
];
1224 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1225 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1227 // Patch from GRBL Firmware - Christoph Baumann 04072015
1228 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1229 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1230 if (is_clockwise
) { // Correct atan2 output per direction
1231 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1233 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1236 // Find the distance for this gcode
1237 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1239 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1240 if( millimeters_of_travel
< 0.00001F
) {
1244 // limit segments by maximum arc error
1245 float arc_segment
= this->mm_per_arc_segment
;
1246 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1247 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1248 if (this->mm_per_arc_segment
< min_err_segment
) {
1249 arc_segment
= min_err_segment
;
1252 // Figure out how many segments for this gcode
1253 // 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
1254 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1256 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1257 float theta_per_segment
= angular_travel
/ segments
;
1258 float linear_per_segment
= linear_travel
/ segments
;
1260 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1261 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1262 r_T = [cos(phi) -sin(phi);
1263 sin(phi) cos(phi] * r ;
1264 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1265 defined from the circle center to the initial position. Each line segment is formed by successive
1266 vector rotations. This requires only two cos() and sin() computations to form the rotation
1267 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1268 all float numbers are single precision on the Arduino. (True float precision will not have
1269 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1270 tool precision in some cases. Therefore, arc path correction is implemented.
1272 Small angle approximation may be used to reduce computation overhead further. This approximation
1273 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1274 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1275 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1276 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1277 issue for CNC machines with the single precision Arduino calculations.
1278 This approximation also allows mc_arc to immediately insert a line segment into the planner
1279 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1280 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1281 This is important when there are successive arc motions.
1283 // Vector rotation matrix values
1284 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1285 float sin_T
= theta_per_segment
;
1287 float arc_target
[3];
1294 // Initialize the linear axis
1295 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1298 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1299 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1301 if (count
< this->arc_correction
) {
1302 // Apply vector rotation matrix
1303 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1304 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1308 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1309 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1310 cos_Ti
= cosf(i
* theta_per_segment
);
1311 sin_Ti
= sinf(i
* theta_per_segment
);
1312 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1313 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1317 // Update arc_target location
1318 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1319 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1320 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1322 // Append this segment to the queue
1323 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1327 // Ensure last segment arrives at target location.
1328 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1333 // Do the math for an arc and add it to the queue
1334 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1338 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1340 // Set clockwise/counter-clockwise sign for mc_arc computations
1341 bool is_clockwise
= false;
1342 if( motion_mode
== CW_ARC
) {
1343 is_clockwise
= true;
1347 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1351 float Robot::theta(float x
, float y
)
1353 float t
= atanf(x
/ fabs(y
));
1365 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1367 this->plane_axis_0
= axis_0
;
1368 this->plane_axis_1
= axis_1
;
1369 this->plane_axis_2
= axis_2
;
1372 void Robot::clearToolOffset()
1374 this->tool_offset
= wcs_t(0,0,0);
1377 void Robot::setToolOffset(const float offset
[3])
1379 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1382 float Robot::get_feed_rate() const
1384 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;