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->machine_position
, 0, sizeof machine_position
);
104 memset(this->compensated_machine_position
, 0, sizeof compensated_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, and potentially A B C
198 uint16_t const checksums
[][6] = {
199 ACTUATOR_CHECKSUMS("alpha"), // X
200 ACTUATOR_CHECKSUMS("beta"), // Y
201 ACTUATOR_CHECKSUMS("gamma"), // Z
202 #if MAX_ROBOT_ACTUATORS > 3
203 ACTUATOR_CHECKSUMS("delta"), // A
204 #if MAX_ROBOT_ACTUATORS > 4
205 ACTUATOR_CHECKSUMS("epsilon"), // B
206 #if MAX_ROBOT_ACTUATORS > 5
207 ACTUATOR_CHECKSUMS("zeta") // C
213 // default acceleration setting, can be overriden with newer per axis settings
214 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
217 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
218 Pin pins
[3]; //step, dir, enable
219 for (size_t i
= 0; i
< 3; i
++) {
220 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
223 if(!pins
[0].connected() || !pins
[1].connected() || !pins
[2].connected()) {
224 if(a
<= Z_AXIS
) THEKERNEL
->streams
->printf("FATAL: motor %d is not defined in config\n", 'X'+a
);
225 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
228 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
229 // register this motor (NB This must be 0,1,2) of the actuators array
230 uint8_t n
= register_motor(sm
);
232 // this is a fatal error
233 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
237 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
238 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
239 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
242 check_max_actuator_speeds(); // check the configs are sane
244 // if we have not specified a z acceleration see if the legacy config was set
245 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
246 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
248 actuators
[Z_AXIS
]->set_acceleration(acc
);
252 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
253 // so the first move can be correct if homing is not performed
254 ActuatorCoordinates actuator_pos
;
255 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
256 for (size_t i
= 0; i
< n_motors
; i
++)
257 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
259 //this->clearToolOffset();
262 uint8_t Robot::register_motor(StepperMotor
*motor
)
264 // register this motor with the step ticker
265 THEKERNEL
->step_ticker
->register_motor(motor
);
266 if(n_motors
>= k_max_actuators
) {
267 // this is a fatal error
268 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
271 actuators
.push_back(motor
);
272 motor
->set_motor_id(n_motors
);
276 void Robot::push_state()
278 bool am
= this->absolute_mode
;
279 bool em
= this->e_absolute_mode
;
280 bool im
= this->inch_mode
;
281 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
285 void Robot::pop_state()
287 if(!state_stack
.empty()) {
288 auto s
= state_stack
.top();
290 this->feed_rate
= std::get
<0>(s
);
291 this->seek_rate
= std::get
<1>(s
);
292 this->absolute_mode
= std::get
<2>(s
);
293 this->e_absolute_mode
= std::get
<3>(s
);
294 this->inch_mode
= std::get
<4>(s
);
295 this->current_wcs
= std::get
<5>(s
);
299 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
301 std::vector
<wcs_t
> v
;
302 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
303 for(auto& i
: wcs_offsets
) {
306 v
.push_back(g92_offset
);
307 v
.push_back(tool_offset
);
311 void Robot::get_current_machine_position(float *pos
) const
313 // get real time current actuator position in mm
314 ActuatorCoordinates current_position
{
315 actuators
[X_AXIS
]->get_current_position(),
316 actuators
[Y_AXIS
]->get_current_position(),
317 actuators
[Z_AXIS
]->get_current_position()
320 // get machine position from the actuator position using FK
321 arm_solution
->actuator_to_cartesian(current_position
, pos
);
324 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
326 // M114.1 is a new way to do this (similar to how GRBL does it).
327 // it returns the realtime position based on the current step position of the actuators.
328 // this does require a FK to get a machine position from the actuator position
329 // and then invert all the transforms to get a workspace position from machine position
330 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
332 if(subcode
== 0) { // M114 print WCS
333 wcs_t pos
= mcs2wcs(machine_position
);
334 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
)));
336 } else if(subcode
== 4) {
337 // M114.4 print last milestone
338 n
= snprintf(buf
, bufsize
, "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position
[X_AXIS
], machine_position
[Y_AXIS
], machine_position
[Z_AXIS
]);
340 } else if(subcode
== 5) {
341 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
342 // will differ from LMS by the compensation at the current position otherwise
343 n
= snprintf(buf
, bufsize
, "CMP: X:%1.4f Y:%1.4f Z:%1.4f", compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
346 // get real time positions
348 get_current_machine_position(mpos
);
350 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
351 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
353 if(subcode
== 1) { // M114.1 print realtime WCS
354 wcs_t pos
= mcs2wcs(mpos
);
355 n
= snprintf(buf
, bufsize
, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get
<X_AXIS
>(pos
)), from_millimeters(std::get
<Y_AXIS
>(pos
)), from_millimeters(std::get
<Z_AXIS
>(pos
)));
357 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
358 n
= snprintf(buf
, bufsize
, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
360 } else if(subcode
== 3) { // M114.3 print realtime actuator position
361 // get real time current actuator position in mm
362 ActuatorCoordinates current_position
{
363 actuators
[X_AXIS
]->get_current_position(),
364 actuators
[Y_AXIS
]->get_current_position(),
365 actuators
[Z_AXIS
]->get_current_position()
367 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
]);
371 #if MAX_ROBOT_ACTUATORS > 3
372 // deal with the ABC axis
373 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
374 if(actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
375 if(subcode
== 4) { // M114.4 print last milestone
376 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
378 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
379 // current actuator position
380 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
388 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
389 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
391 return std::make_tuple(
392 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
),
393 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
),
394 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
)
398 // this does a sanity check that actuator speeds do not exceed steps rate capability
399 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
400 void Robot::check_max_actuator_speeds()
402 for (size_t i
= 0; i
< n_motors
; i
++) {
403 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
404 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
405 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
406 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());
411 //A GCode has been received
412 //See if the current Gcode line has some orders for us
413 void Robot::on_gcode_received(void *argument
)
415 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
417 enum MOTION_MODE_T motion_mode
= NONE
;
421 case 0: motion_mode
= SEEK
; break;
422 case 1: motion_mode
= LINEAR
; break;
423 case 2: motion_mode
= CW_ARC
; break;
424 case 3: motion_mode
= CCW_ARC
; break;
425 case 4: { // G4 pause
426 uint32_t delay_ms
= 0;
427 if (gcode
->has_letter('P')) {
428 delay_ms
= gcode
->get_int('P');
430 if (gcode
->has_letter('S')) {
431 delay_ms
+= gcode
->get_int('S') * 1000;
435 THEKERNEL
->conveyor
->wait_for_idle();
436 // wait for specified time
437 uint32_t start
= us_ticker_read(); // mbed call
438 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
439 THEKERNEL
->call_event(ON_IDLE
, this);
440 if(THEKERNEL
->is_halted()) return;
446 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
447 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
448 size_t n
= gcode
->get_uint('P');
449 if(n
== 0) n
= current_wcs
; // set current coordinate system
453 std::tie(x
, y
, z
) = wcs_offsets
[n
];
454 if(gcode
->get_int('L') == 20) {
455 // this makes the current machine position (less compensation transform) the offset
456 // get current position in WCS
457 wcs_t pos
= mcs2wcs(machine_position
);
459 if(gcode
->has_letter('X')){
460 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
463 if(gcode
->has_letter('Y')){
464 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
466 if(gcode
->has_letter('Z')) {
467 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
472 // the value is the offset from machine zero
473 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
474 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
475 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
477 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
478 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
479 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
482 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
487 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
488 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
489 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
490 case 20: this->inch_mode
= true; break;
491 case 21: this->inch_mode
= false; break;
493 case 54: case 55: case 56: case 57: case 58: case 59:
494 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
495 current_wcs
= gcode
->g
- 54;
496 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
497 current_wcs
+= gcode
->subcode
;
498 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
502 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
503 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
506 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
507 // reset G92 offsets to 0
508 g92_offset
= wcs_t(0, 0, 0);
510 } else if(gcode
->subcode
== 3) {
511 // initialize G92 to the specified values, only used for saving it with M500
512 float x
= 0, y
= 0, z
= 0;
513 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
514 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
515 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
516 g92_offset
= wcs_t(x
, y
, z
);
519 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
521 std::tie(x
, y
, z
) = g92_offset
;
522 // get current position in WCS
523 wcs_t pos
= mcs2wcs(machine_position
);
525 // adjust g92 offset to make the current wpos == the value requested
526 if(gcode
->has_letter('X')){
527 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
529 if(gcode
->has_letter('Y')){
530 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
532 if(gcode
->has_letter('Z')) {
533 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
535 g92_offset
= wcs_t(x
, y
, z
);
538 #if MAX_ROBOT_ACTUATORS > 3
539 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
540 // reset the E position, legacy for 3d Printers to be reprap compatible
541 // find the selected extruder
542 int selected_extruder
= get_active_extruder();
543 if(selected_extruder
> 0) {
544 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
545 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
546 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
555 } else if( gcode
->has_m
) {
557 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
558 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
561 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
562 if(!THEKERNEL
->is_grbl_mode()) break;
563 // fall through to M2
564 case 2: // M2 end of program
566 absolute_mode
= true;
569 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
572 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
573 if(gcode
->get_num_args() > 0) {
574 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
576 for (int i
= 0; i
< n_motors
; ++i
) {
577 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
578 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
580 // handle E parameter as currently selected extruder ABC
581 if(gcode
->has_letter('E')) {
582 // find first selected extruder
583 int i
= get_active_extruder();
585 bm
|= (0x02<<i
); // set appropriate bit
589 THEKERNEL
->conveyor
->wait_for_idle();
590 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
595 THEKERNEL
->conveyor
->wait_for_idle();
596 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
599 case 82: e_absolute_mode
= true; break;
600 case 83: e_absolute_mode
= false; break;
602 case 92: // M92 - set steps per mm
603 for (int i
= 0; i
< n_motors
; ++i
) {
604 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
605 if(gcode
->has_letter(axis
)) {
606 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
608 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
610 gcode
->add_nl
= true;
611 check_max_actuator_speeds();
616 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
617 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
621 case 120: // push state
625 case 121: // pop state
629 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
630 if(gcode
->get_num_args() == 0) {
631 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
632 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
634 if(gcode
->subcode
== 1) {
635 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
636 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
640 gcode
->add_nl
= true;
643 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
644 if (gcode
->has_letter('X' + i
)) {
645 float v
= gcode
->get_value('X'+i
);
646 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
647 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
651 if(gcode
->subcode
== 1) {
652 // ABC axis only handle actuator max speeds
653 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
654 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
655 int c
= 'A' + i
- A_AXIS
;
656 if(gcode
->has_letter(c
)) {
657 float v
= gcode
->get_value(c
);
658 actuators
[i
]->set_max_rate(v
);
664 // this format is deprecated
665 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
666 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
667 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
668 if (gcode
->has_letter('A' + i
)) {
669 float v
= gcode
->get_value('A'+i
);
670 actuators
[i
]->set_max_rate(v
);
675 if(gcode
->subcode
== 1) check_max_actuator_speeds();
679 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
680 if (gcode
->has_letter('S')) {
681 float acc
= gcode
->get_value('S'); // mm/s^2
683 if (acc
< 1.0F
) acc
= 1.0F
;
684 this->default_acceleration
= acc
;
686 for (int i
= 0; i
< n_motors
; ++i
) {
687 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
688 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
689 if(gcode
->has_letter(axis
)) {
690 float acc
= gcode
->get_value(axis
); // mm/s^2
692 if (acc
<= 0.0F
) acc
= NAN
;
693 actuators
[i
]->set_acceleration(acc
);
698 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
699 if (gcode
->has_letter('X')) {
700 float jd
= gcode
->get_value('X');
704 THEKERNEL
->planner
->junction_deviation
= jd
;
706 if (gcode
->has_letter('Z')) {
707 float jd
= gcode
->get_value('Z');
708 // enforce minimum, -1 disables it and uses regular junction deviation
711 THEKERNEL
->planner
->z_junction_deviation
= jd
;
713 if (gcode
->has_letter('S')) {
714 float mps
= gcode
->get_value('S');
718 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
722 case 220: // M220 - speed override percentage
723 if (gcode
->has_letter('S')) {
724 float factor
= gcode
->get_value('S');
725 // enforce minimum 10% speed
728 // enforce maximum 10x speed
729 if (factor
> 1000.0F
)
732 seconds_per_minute
= 6000.0F
/ factor
;
734 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
738 case 400: // wait until all moves are done up to this point
739 THEKERNEL
->conveyor
->wait_for_idle();
742 case 500: // M500 saves some volatile settings to config override file
743 case 503: { // M503 just prints the settings
744 gcode
->stream
->printf(";Steps per unit:\nM92 ");
745 for (int i
= 0; i
< n_motors
; ++i
) {
746 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
747 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
748 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
750 gcode
->stream
->printf("\n");
752 // only print if not NAN
753 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
754 for (int i
= 0; i
< n_motors
; ++i
) {
755 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
756 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
757 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
759 gcode
->stream
->printf("\n");
761 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
);
763 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
]);
765 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
766 for (int i
= 0; i
< n_motors
; ++i
) {
767 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
768 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
769 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
771 gcode
->stream
->printf("\n");
773 // get or save any arm solution specific optional values
774 BaseSolution::arm_options_t options
;
775 if(arm_solution
->get_optional(options
) && !options
.empty()) {
776 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
777 for(auto &i
: options
) {
778 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
780 gcode
->stream
->printf("\n");
783 // save wcs_offsets and current_wcs
784 // TODO this may need to be done whenever they change to be compliant
785 gcode
->stream
->printf(";WCS settings\n");
786 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
788 for(auto &i
: wcs_offsets
) {
789 if(i
!= wcs_t(0, 0, 0)) {
791 std::tie(x
, y
, z
) = i
;
792 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
797 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
798 // 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
799 if(g92_offset
!= wcs_t(0, 0, 0)) {
801 std::tie(x
, y
, z
) = g92_offset
;
802 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
808 case 665: { // M665 set optional arm solution variables based on arm solution.
809 // the parameter args could be any letter each arm solution only accepts certain ones
810 BaseSolution::arm_options_t options
= gcode
->get_args();
811 options
.erase('S'); // don't include the S
812 options
.erase('U'); // don't include the U
813 if(options
.size() > 0) {
814 // set the specified options
815 arm_solution
->set_optional(options
);
818 if(arm_solution
->get_optional(options
)) {
819 // foreach optional value
820 for(auto &i
: options
) {
821 // print all current values of supported options
822 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
823 gcode
->add_nl
= true;
827 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
828 this->delta_segments_per_second
= gcode
->get_value('S');
829 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
831 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
832 this->mm_per_line_segment
= gcode
->get_value('U');
833 this->delta_segments_per_second
= 0;
834 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
842 if( motion_mode
!= NONE
) {
843 is_g123
= motion_mode
!= SEEK
;
844 process_move(gcode
, motion_mode
);
850 next_command_is_MCS
= false; // must be on same line as G0 or G1
853 int Robot::get_active_extruder() const
855 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
856 // find first selected extruder
857 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
862 // process a G0/G1/G2/G3
863 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
865 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
866 // get XYZ and one E (which goes to the selected extruder)
867 float param
[4]{NAN
, NAN
, NAN
, NAN
};
869 // process primary axis
870 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
872 if( gcode
->has_letter(letter
) ) {
873 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
877 float offset
[3]{0,0,0};
878 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
879 if( gcode
->has_letter(letter
) ) {
880 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
884 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
885 float target
[n_motors
];
886 memcpy(target
, machine_position
, n_motors
*sizeof(float));
888 if(!next_command_is_MCS
) {
889 if(this->absolute_mode
) {
890 // apply wcs offsets and g92 offset and tool offset
891 if(!isnan(param
[X_AXIS
])) {
892 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
);
895 if(!isnan(param
[Y_AXIS
])) {
896 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
);
899 if(!isnan(param
[Z_AXIS
])) {
900 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
);
904 // they are deltas from the machine_position if specified
905 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
906 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
911 // already in machine coordinates, we do not add tool offset for that
912 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
913 if(!isnan(param
[i
])) target
[i
] = param
[i
];
919 #if MAX_ROBOT_ACTUATORS > 3
920 // process extruder parameters, for active extruder only (only one active extruder at a time)
921 int selected_extruder
= 0;
922 if(gcode
->has_letter('E')) {
923 selected_extruder
= get_active_extruder();
924 param
[E_AXIS
]= gcode
->get_value('E');
927 // do E for the selected extruder
928 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
929 if(this->e_absolute_mode
) {
930 target
[selected_extruder
]= param
[E_AXIS
];
931 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
933 delta_e
= param
[E_AXIS
];
934 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
938 // process ABC axis, this is mutually exclusive to using E for an extruder, so if E is used and A then the results are undefined
939 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
940 char letter
= 'A'+i
-A_AXIS
;
941 if(gcode
->has_letter(letter
)) {
942 float p
= gcode
->get_value(letter
);
943 if(this->absolute_mode
) {
946 target
[i
]= p
+ machine_position
[i
];
952 if( gcode
->has_letter('F') ) {
953 if( motion_mode
== SEEK
)
954 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
956 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
959 // S is modal When specified on a G0/1/2/3 command
960 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
964 // Perform any physical actions
965 switch(motion_mode
) {
969 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
973 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
978 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
979 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
984 // set machine_position to the calculated target
985 memcpy(machine_position
, target
, n_motors
*sizeof(float));
989 // reset the machine position for all axis. Used for homing.
990 // after homing we supply the cartesian coordinates that the head is at when homed,
991 // however for Z this is the compensated machine position (if enabled)
992 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
993 // this will make the results from M114 and ? consistent after homing.
994 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
995 void Robot::reset_axis_position(float x
, float y
, float z
)
997 // set both the same initially
998 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
999 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1000 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1002 if(compensationTransform
) {
1003 // apply inverse transform to get machine_position
1004 compensationTransform(machine_position
, true);
1007 // now set the actuator positions based on the supplied compensated position
1008 ActuatorCoordinates actuator_pos
;
1009 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1010 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1011 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1014 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1015 void Robot::reset_axis_position(float position
, int axis
)
1017 compensated_machine_position
[axis
] = position
;
1018 if(axis
<= Z_AXIS
) {
1019 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1021 #if MAX_ROBOT_ACTUATORS > 3
1022 }else if(axis
< n_motors
) {
1023 // ABC and/or extruders need to be set as there is no arm solution for them
1024 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1025 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1030 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1031 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1032 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1034 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1035 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1038 // now correct axis positions then recorrect actuator to account for rounding
1039 reset_position_from_current_actuator_position();
1042 // Use FK to find out where actuator is and reset to match
1043 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1044 void Robot::reset_position_from_current_actuator_position()
1046 ActuatorCoordinates actuator_pos
;
1047 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1048 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1049 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1052 // discover machine position from where actuators actually are
1053 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1054 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1056 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1057 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1059 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1060 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1061 // to get everything in perfect sync.
1062 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1063 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1064 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1067 // Handle extruders and/or ABC axis
1068 #if MAX_ROBOT_ACTUATORS > 3
1069 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1070 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1071 float ap
= actuator_pos
[i
];
1072 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) ap
/= get_e_scale_fnc(); // inverse E scale if there is one and this is an extruder
1073 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1074 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1079 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1080 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1081 // all transforms and is what we actually convert to actuator positions
1082 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1084 float deltas
[n_motors
];
1085 float transformed_target
[n_motors
]; // adjust target for bed compensation
1086 float unit_vec
[N_PRIMARY_AXIS
];
1088 // unity transform by default
1089 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1091 // check function pointer and call if set to transform the target to compensate for bed
1092 if(compensationTransform
) {
1093 // some compensation strategies can transform XYZ, some just change Z
1094 compensationTransform(transformed_target
, false);
1098 float sos
= 0; // sum of squares for just XYZ
1100 // find distance moved by each axis, use transformed target from the current compensated machine position
1101 for (size_t i
= 0; i
< n_motors
; i
++) {
1102 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1103 if(deltas
[i
] == 0) continue;
1104 // at least one non zero delta
1107 sos
+= powf(deltas
[i
], 2);
1112 if(!move
) return false;
1114 // see if this is a primary axis move or not
1115 bool auxilliary_move
= deltas
[X_AXIS
] == 0 && deltas
[Y_AXIS
] == 0 && deltas
[Z_AXIS
] == 0;
1117 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1118 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1120 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1121 // 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
1122 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1125 if(!auxilliary_move
) {
1126 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1127 // find distance unit vector for primary axis only
1128 unit_vec
[i
] = deltas
[i
] / distance
;
1130 // Do not move faster than the configured cartesian limits for XYZ
1131 if ( max_speeds
[i
] > 0 ) {
1132 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1134 if (axis_speed
> max_speeds
[i
])
1135 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1140 // find actuator position given the machine position, use actual adjusted target
1141 ActuatorCoordinates actuator_pos
;
1142 if(!disable_arm_solution
) {
1143 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1146 // basically the same as cartesian, would be used for special homing situations like for scara
1147 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1148 actuator_pos
[i
] = transformed_target
[i
];
1152 #if MAX_ROBOT_ACTUATORS > 3
1154 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1155 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1156 actuator_pos
[i
]= transformed_target
[i
];
1157 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1158 // NOTE this relies on the fact only one extruder is active at a time
1159 // scale for volumetric or flow rate
1160 // TODO is this correct? scaling the absolute target? what if the scale changes?
1161 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1162 actuator_pos
[i
] *= get_e_scale_fnc();
1164 if(auxilliary_move
) {
1165 // for E only moves we need to use the scaled E to calculate the distance
1166 sos
+= pow(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1169 if(auxilliary_move
) {
1170 distance
= sqrtf(sos
); // distance in mm of the e move
1171 if(distance
< 0.00001F
) return false;
1175 // use default acceleration to start with
1176 float acceleration
= default_acceleration
;
1178 float isecs
= rate_mm_s
/ distance
;
1180 // check per-actuator speed limits
1181 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1182 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1183 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1185 float actuator_rate
= d
* isecs
;
1186 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1187 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1188 isecs
= rate_mm_s
/ distance
;
1191 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1192 // 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.
1193 if(auxilliary_move
|| actuator
<= Z_AXIS
) {
1194 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1195 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1196 float ca
= fabsf((d
/distance
) * acceleration
);
1198 acceleration
*= ( ma
/ ca
);
1204 // Append the block to the planner
1205 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1206 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1207 // this is the new compensated machine position
1208 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1216 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1217 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1219 if(THEKERNEL
->is_halted()) return false;
1221 // catch negative or zero feed rates
1222 if(rate_mm_s
<= 0.0F
) {
1226 // get the absolute target position, default is current machine_position
1227 float target
[n_motors
];
1228 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1230 // add in the deltas to get new target
1231 for (int i
= 0; i
< naxis
; i
++) {
1232 target
[i
] += delta
[i
];
1235 // submit for planning and if moved update machine_position
1236 if(append_milestone(target
, rate_mm_s
)) {
1237 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1244 // Append a move to the queue ( cutting it into segments if needed )
1245 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1247 // catch negative or zero feed rates and return the same error as GRBL does
1248 if(rate_mm_s
<= 0.0F
) {
1249 gcode
->is_error
= true;
1250 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1254 // Find out the distance for this move in XYZ in MCS
1255 float millimeters_of_travel
= sqrtf(powf( target
[X_AXIS
] - machine_position
[X_AXIS
], 2 ) + powf( target
[Y_AXIS
] - machine_position
[Y_AXIS
], 2 ) + powf( target
[Z_AXIS
] - machine_position
[Z_AXIS
], 2 ));
1257 if(millimeters_of_travel
< 0.00001F
) {
1258 // we have no movement in XYZ, probably E only extrude or retract
1259 return this->append_milestone(target
, rate_mm_s
);
1263 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1264 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1265 We ask Extruder to do all the work but we need to pass in the relevant data.
1266 NOTE we need to do this before we segment the line (for deltas)
1268 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1269 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1270 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1271 rate_mm_s
*= data
[1]; // adjust the feedrate
1275 // 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.
1276 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1277 // 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
1280 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1283 } else if(this->delta_segments_per_second
> 1.0F
) {
1284 // enabled if set to something > 1, it is set to 0.0 by default
1285 // segment based on current speed and requested segments per second
1286 // the faster the travel speed the fewer segments needed
1287 // NOTE rate is mm/sec and we take into account any speed override
1288 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1289 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1290 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1293 if(this->mm_per_line_segment
== 0.0F
) {
1294 segments
= 1; // don't split it up
1296 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1302 // A vector to keep track of the endpoint of each segment
1303 float segment_delta
[n_motors
];
1304 float segment_end
[n_motors
];
1305 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1307 // How far do we move each segment?
1308 for (int i
= 0; i
< n_motors
; i
++)
1309 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1311 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1312 // We always add another point after this loop so we stop at segments-1, ie i < segments
1313 for (int i
= 1; i
< segments
; i
++) {
1314 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1315 for (int i
= 0; i
< n_motors
; i
++)
1316 segment_end
[i
] += segment_delta
[i
];
1318 // Append the end of this segment to the queue
1319 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1324 // Append the end of this full move to the queue
1325 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1327 this->next_command_is_MCS
= false; // always reset this
1333 // Append an arc to the queue ( cutting it into segments as needed )
1334 // TODO does not support any E parameters so cannot be used for 3D printing.
1335 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1337 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1338 // catch negative or zero feed rates and return the same error as GRBL does
1339 if(rate_mm_s
<= 0.0F
) {
1340 gcode
->is_error
= true;
1341 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1346 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1347 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1348 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1349 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1350 float r_axis1
= -offset
[this->plane_axis_1
];
1351 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1352 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1354 // Patch from GRBL Firmware - Christoph Baumann 04072015
1355 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1356 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1357 if (is_clockwise
) { // Correct atan2 output per direction
1358 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1360 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1363 // Find the distance for this gcode
1364 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1366 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1367 if( millimeters_of_travel
< 0.00001F
) {
1371 // limit segments by maximum arc error
1372 float arc_segment
= this->mm_per_arc_segment
;
1373 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1374 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1375 if (this->mm_per_arc_segment
< min_err_segment
) {
1376 arc_segment
= min_err_segment
;
1379 // Figure out how many segments for this gcode
1380 // 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
1381 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1383 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1384 float theta_per_segment
= angular_travel
/ segments
;
1385 float linear_per_segment
= linear_travel
/ segments
;
1387 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1388 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1389 r_T = [cos(phi) -sin(phi);
1390 sin(phi) cos(phi] * r ;
1391 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1392 defined from the circle center to the initial position. Each line segment is formed by successive
1393 vector rotations. This requires only two cos() and sin() computations to form the rotation
1394 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1395 all float numbers are single precision on the Arduino. (True float precision will not have
1396 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1397 tool precision in some cases. Therefore, arc path correction is implemented.
1399 Small angle approximation may be used to reduce computation overhead further. This approximation
1400 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1401 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1402 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1403 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1404 issue for CNC machines with the single precision Arduino calculations.
1405 This approximation also allows mc_arc to immediately insert a line segment into the planner
1406 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1407 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1408 This is important when there are successive arc motions.
1410 // Vector rotation matrix values
1411 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1412 float sin_T
= theta_per_segment
;
1414 // TODO we need to handle the ABC axis here by segmenting them
1415 float arc_target
[3];
1422 // Initialize the linear axis
1423 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1426 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1427 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1429 if (count
< this->arc_correction
) {
1430 // Apply vector rotation matrix
1431 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1432 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1436 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1437 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1438 cos_Ti
= cosf(i
* theta_per_segment
);
1439 sin_Ti
= sinf(i
* theta_per_segment
);
1440 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1441 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1445 // Update arc_target location
1446 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1447 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1448 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1450 // Append this segment to the queue
1451 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1455 // Ensure last segment arrives at target location.
1456 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1461 // Do the math for an arc and add it to the queue
1462 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1466 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1468 // Set clockwise/counter-clockwise sign for mc_arc computations
1469 bool is_clockwise
= false;
1470 if( motion_mode
== CW_ARC
) {
1471 is_clockwise
= true;
1475 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1479 float Robot::theta(float x
, float y
)
1481 float t
= atanf(x
/ fabs(y
));
1493 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1495 this->plane_axis_0
= axis_0
;
1496 this->plane_axis_1
= axis_1
;
1497 this->plane_axis_2
= axis_2
;
1500 void Robot::clearToolOffset()
1502 this->tool_offset
= wcs_t(0,0,0);
1505 void Robot::setToolOffset(const float offset
[3])
1507 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1510 float Robot::get_feed_rate() const
1512 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;