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(subcode
== 4) { // M114.4 print last milestone
375 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, last_milestone
[i
]);
377 }else if(subcode
== 3) { // M114.3 print actuator position
378 // current actuator position
379 float current_position
= actuators
[i
]->get_current_position();
380 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, 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 if (gcode
->has_letter('X'))
604 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
605 if (gcode
->has_letter('Y'))
606 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
607 if (gcode
->has_letter('Z'))
608 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
610 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());
611 gcode
->add_nl
= true;
612 check_max_actuator_speeds();
617 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
618 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
622 case 120: // push state
626 case 121: // pop state
630 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
631 if(gcode
->get_num_args() == 0) {
632 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
633 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
635 gcode
->add_nl
= true;
638 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
639 if (gcode
->has_letter('X' + i
)) {
640 float v
= gcode
->get_value('X'+i
);
641 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
642 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
646 // this format is deprecated
647 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
648 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
649 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
650 if (gcode
->has_letter('A' + i
)) {
651 float v
= gcode
->get_value('A'+i
);
652 actuators
[i
]->set_max_rate(v
);
657 if(gcode
->subcode
== 1) check_max_actuator_speeds();
661 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
662 if (gcode
->has_letter('S')) {
663 float acc
= gcode
->get_value('S'); // mm/s^2
665 if (acc
< 1.0F
) acc
= 1.0F
;
666 this->default_acceleration
= acc
;
668 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
669 if (gcode
->has_letter(i
+'X')) {
670 float acc
= gcode
->get_value(i
+'X'); // mm/s^2
672 if (acc
<= 0.0F
) acc
= NAN
;
673 actuators
[i
]->set_acceleration(acc
);
678 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
679 if (gcode
->has_letter('X')) {
680 float jd
= gcode
->get_value('X');
684 THEKERNEL
->planner
->junction_deviation
= jd
;
686 if (gcode
->has_letter('Z')) {
687 float jd
= gcode
->get_value('Z');
688 // enforce minimum, -1 disables it and uses regular junction deviation
691 THEKERNEL
->planner
->z_junction_deviation
= jd
;
693 if (gcode
->has_letter('S')) {
694 float mps
= gcode
->get_value('S');
698 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
702 case 220: // M220 - speed override percentage
703 if (gcode
->has_letter('S')) {
704 float factor
= gcode
->get_value('S');
705 // enforce minimum 10% speed
708 // enforce maximum 10x speed
709 if (factor
> 1000.0F
)
712 seconds_per_minute
= 6000.0F
/ factor
;
714 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
718 case 400: // wait until all moves are done up to this point
719 THEKERNEL
->conveyor
->wait_for_idle();
722 case 500: // M500 saves some volatile settings to config override file
723 case 503: { // M503 just prints the settings
724 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());
726 // only print XYZ if not NAN
727 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
728 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
729 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", 'X'+i
, actuators
[i
]->get_acceleration());
731 gcode
->stream
->printf("\n");
733 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
);
735 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
]);
736 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());
738 // get or save any arm solution specific optional values
739 BaseSolution::arm_options_t options
;
740 if(arm_solution
->get_optional(options
) && !options
.empty()) {
741 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
742 for(auto &i
: options
) {
743 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
745 gcode
->stream
->printf("\n");
748 // save wcs_offsets and current_wcs
749 // TODO this may need to be done whenever they change to be compliant
750 gcode
->stream
->printf(";WCS settings\n");
751 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
753 for(auto &i
: wcs_offsets
) {
754 if(i
!= wcs_t(0, 0, 0)) {
756 std::tie(x
, y
, z
) = i
;
757 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
762 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
763 // 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
764 if(g92_offset
!= wcs_t(0, 0, 0)) {
766 std::tie(x
, y
, z
) = g92_offset
;
767 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
773 case 665: { // M665 set optional arm solution variables based on arm solution.
774 // the parameter args could be any letter each arm solution only accepts certain ones
775 BaseSolution::arm_options_t options
= gcode
->get_args();
776 options
.erase('S'); // don't include the S
777 options
.erase('U'); // don't include the U
778 if(options
.size() > 0) {
779 // set the specified options
780 arm_solution
->set_optional(options
);
783 if(arm_solution
->get_optional(options
)) {
784 // foreach optional value
785 for(auto &i
: options
) {
786 // print all current values of supported options
787 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
788 gcode
->add_nl
= true;
792 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
793 this->delta_segments_per_second
= gcode
->get_value('S');
794 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
796 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
797 this->mm_per_line_segment
= gcode
->get_value('U');
798 this->delta_segments_per_second
= 0;
799 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
807 if( motion_mode
!= NONE
) {
808 is_g123
= motion_mode
!= SEEK
;
809 process_move(gcode
, motion_mode
);
815 next_command_is_MCS
= false; // must be on same line as G0 or G1
818 int Robot::get_active_extruder() const
820 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
821 // find first selected extruder
822 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
827 // process a G0/G1/G2/G3
828 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
830 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
831 // get XYZ and one E (which goes to the selected extruder)
832 float param
[4]{NAN
, NAN
, NAN
, NAN
};
834 // process primary axis
835 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
837 if( gcode
->has_letter(letter
) ) {
838 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
842 float offset
[3]{0,0,0};
843 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
844 if( gcode
->has_letter(letter
) ) {
845 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
849 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
850 float target
[n_motors
];
851 memcpy(target
, machine_position
, n_motors
*sizeof(float));
853 if(!next_command_is_MCS
) {
854 if(this->absolute_mode
) {
855 // apply wcs offsets and g92 offset and tool offset
856 if(!isnan(param
[X_AXIS
])) {
857 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
);
860 if(!isnan(param
[Y_AXIS
])) {
861 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
);
864 if(!isnan(param
[Z_AXIS
])) {
865 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
);
869 // they are deltas from the machine_position if specified
870 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
871 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
876 // already in machine coordinates, we do not add tool offset for that
877 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
878 if(!isnan(param
[i
])) target
[i
] = param
[i
];
884 #if MAX_ROBOT_ACTUATORS > 3
885 // process extruder parameters, for active extruder only (only one active extruder at a time)
886 int selected_extruder
= 0;
887 if(gcode
->has_letter('E')) {
888 selected_extruder
= get_active_extruder();
889 param
[E_AXIS
]= gcode
->get_value('E');
892 // do E for the selected extruder
893 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
894 if(this->e_absolute_mode
) {
895 target
[selected_extruder
]= param
[E_AXIS
];
896 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
898 delta_e
= param
[E_AXIS
];
899 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
903 // 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
904 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
905 char letter
= 'A'+i
-A_AXIS
;
906 if(gcode
->has_letter(letter
)) {
907 float p
= gcode
->get_value(letter
);
908 if(this->absolute_mode
) {
911 target
[i
]= p
+ last_milestone
[i
];
917 if( gcode
->has_letter('F') ) {
918 if( motion_mode
== SEEK
)
919 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
921 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
924 // S is modal When specified on a G0/1/2/3 command
925 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
929 // Perform any physical actions
930 switch(motion_mode
) {
934 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
938 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
943 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
944 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
949 // set machine_position to the calculated target
950 memcpy(machine_position
, target
, n_motors
*sizeof(float));
954 // reset the machine position for all axis. Used for homing.
955 // after homing we supply the cartesian coordinates that the head is at when homed,
956 // however for Z this is the compensated machine position (if enabled)
957 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
958 // this will make the results from M114 and ? consistent after homing.
959 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
960 void Robot::reset_axis_position(float x
, float y
, float z
)
962 // set both the same initially
963 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
964 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
965 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
967 if(compensationTransform
) {
968 // apply inverse transform to get machine_position
969 compensationTransform(machine_position
, true);
972 // now set the actuator positions based on the supplied compensated position
973 ActuatorCoordinates actuator_pos
;
974 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
975 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
976 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
979 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
980 void Robot::reset_axis_position(float position
, int axis
)
982 compensated_machine_position
[axis
] = position
;
984 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
986 #if MAX_ROBOT_ACTUATORS > 3
987 }else if(axis
< n_motors
) {
988 // ABC and/or extruders need to be set as there is no arm solution for them
989 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
990 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
995 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
996 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
997 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
999 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1000 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1003 // now correct axis positions then recorrect actuator to account for rounding
1004 reset_position_from_current_actuator_position();
1007 // Use FK to find out where actuator is and reset to match
1008 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1009 void Robot::reset_position_from_current_actuator_position()
1011 ActuatorCoordinates actuator_pos
;
1012 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1013 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1014 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1017 // discover machine position from where actuators actually are
1018 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1019 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1021 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1022 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1024 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1025 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1026 // to get everything in perfect sync.
1027 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1028 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1029 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1032 // Handle extruders and/or ABC axis
1033 #if MAX_ROBOT_ACTUATORS > 3
1034 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1035 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1036 float ap
= actuator_pos
[i
];
1037 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
1038 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1039 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1044 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1045 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1046 // all transforms and is what we actually convert to actuator positions
1047 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1049 float deltas
[n_motors
];
1050 float transformed_target
[n_motors
]; // adjust target for bed compensation
1051 float unit_vec
[N_PRIMARY_AXIS
];
1053 // unity transform by default
1054 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1056 // check function pointer and call if set to transform the target to compensate for bed
1057 if(compensationTransform
) {
1058 // some compensation strategies can transform XYZ, some just change Z
1059 compensationTransform(transformed_target
, false);
1063 float sos
= 0; // sum of squares for just XYZ
1065 // find distance moved by each axis, use transformed target from the current compensated machine position
1066 for (size_t i
= 0; i
< n_motors
; i
++) {
1067 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1068 if(deltas
[i
] == 0) continue;
1069 // at least one non zero delta
1072 sos
+= powf(deltas
[i
], 2);
1077 if(!move
) return false;
1079 // see if this is a primary axis move or not
1080 bool auxilliary_move
= deltas
[X_AXIS
] == 0 && deltas
[Y_AXIS
] == 0 && deltas
[Z_AXIS
] == 0;
1082 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1083 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1085 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1086 // 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
1087 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1090 if(!auxilliary_move
) {
1091 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1092 // find distance unit vector for primary axis only
1093 unit_vec
[i
] = deltas
[i
] / distance
;
1095 // Do not move faster than the configured cartesian limits for XYZ
1096 if ( max_speeds
[i
] > 0 ) {
1097 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1099 if (axis_speed
> max_speeds
[i
])
1100 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1105 // find actuator position given the machine position, use actual adjusted target
1106 ActuatorCoordinates actuator_pos
;
1107 if(!disable_arm_solution
) {
1108 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1111 // basically the same as cartesian, would be used for special homing situations like for scara
1112 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1113 actuator_pos
[i
] = transformed_target
[i
];
1117 #if MAX_ROBOT_ACTUATORS > 3
1119 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1120 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1121 actuator_pos
[i
]= transformed_target
[i
];
1122 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1123 // NOTE this relies on the fact only one extruder is active at a time
1124 // scale for volumetric or flow rate
1125 // TODO is this correct? scaling the absolute target? what if the scale changes?
1126 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1127 actuator_pos
[i
] *= get_e_scale_fnc();
1129 if(auxilliary_move
) {
1130 // for E only moves we need to use the scaled E to calculate the distance
1131 sos
+= pow(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1134 if(auxilliary_move
) {
1135 distance
= sqrtf(sos
); // distance in mm of the e move
1136 if(distance
< 0.00001F
) return false;
1140 // use default acceleration to start with
1141 float acceleration
= default_acceleration
;
1143 float isecs
= rate_mm_s
/ distance
;
1145 // check per-actuator speed limits
1146 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1147 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1148 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1150 float actuator_rate
= d
* isecs
;
1151 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1152 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1153 isecs
= rate_mm_s
/ distance
;
1156 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1157 // 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.
1158 if(auxilliary_move
|| actuator
<= Z_AXIS
) {
1159 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1160 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1161 float ca
= fabsf((d
/distance
) * acceleration
);
1163 acceleration
*= ( ma
/ ca
);
1169 // Append the block to the planner
1170 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1171 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1172 // this is the new compensated machine position
1173 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1181 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1182 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1184 if(THEKERNEL
->is_halted()) return false;
1186 // catch negative or zero feed rates
1187 if(rate_mm_s
<= 0.0F
) {
1191 // get the absolute target position, default is current machine_position
1192 float target
[n_motors
];
1193 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1195 // add in the deltas to get new target
1196 for (int i
= 0; i
< naxis
; i
++) {
1197 target
[i
] += delta
[i
];
1200 // submit for planning and if moved update machine_position
1201 if(append_milestone(target
, rate_mm_s
)) {
1202 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1209 // Append a move to the queue ( cutting it into segments if needed )
1210 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1212 // catch negative or zero feed rates and return the same error as GRBL does
1213 if(rate_mm_s
<= 0.0F
) {
1214 gcode
->is_error
= true;
1215 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1219 // Find out the distance for this move in XYZ in MCS
1220 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 ));
1222 if(millimeters_of_travel
< 0.00001F
) {
1223 // we have no movement in XYZ, probably E only extrude or retract
1224 return this->append_milestone(target
, rate_mm_s
);
1228 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1229 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1230 We ask Extruder to do all the work but we need to pass in the relevant data.
1231 NOTE we need to do this before we segment the line (for deltas)
1233 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1234 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1235 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1236 rate_mm_s
*= data
[1]; // adjust the feedrate
1240 // 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.
1241 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1242 // 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
1245 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1248 } else if(this->delta_segments_per_second
> 1.0F
) {
1249 // enabled if set to something > 1, it is set to 0.0 by default
1250 // segment based on current speed and requested segments per second
1251 // the faster the travel speed the fewer segments needed
1252 // NOTE rate is mm/sec and we take into account any speed override
1253 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1254 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1255 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1258 if(this->mm_per_line_segment
== 0.0F
) {
1259 segments
= 1; // don't split it up
1261 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1267 // A vector to keep track of the endpoint of each segment
1268 float segment_delta
[n_motors
];
1269 float segment_end
[n_motors
];
1270 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1272 // How far do we move each segment?
1273 for (int i
= 0; i
< n_motors
; i
++)
1274 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1276 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1277 // We always add another point after this loop so we stop at segments-1, ie i < segments
1278 for (int i
= 1; i
< segments
; i
++) {
1279 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1280 for (int i
= 0; i
< n_motors
; i
++)
1281 segment_end
[i
] += segment_delta
[i
];
1283 // Append the end of this segment to the queue
1284 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1289 // Append the end of this full move to the queue
1290 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1292 this->next_command_is_MCS
= false; // always reset this
1298 // Append an arc to the queue ( cutting it into segments as needed )
1299 // TODO does not support any E parameters so cannot be used for 3D printing.
1300 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1302 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1303 // catch negative or zero feed rates and return the same error as GRBL does
1304 if(rate_mm_s
<= 0.0F
) {
1305 gcode
->is_error
= true;
1306 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1311 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1312 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1313 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1314 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1315 float r_axis1
= -offset
[this->plane_axis_1
];
1316 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1317 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1319 // Patch from GRBL Firmware - Christoph Baumann 04072015
1320 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1321 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1322 if (is_clockwise
) { // Correct atan2 output per direction
1323 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1325 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1328 // Find the distance for this gcode
1329 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1331 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1332 if( millimeters_of_travel
< 0.00001F
) {
1336 // limit segments by maximum arc error
1337 float arc_segment
= this->mm_per_arc_segment
;
1338 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1339 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1340 if (this->mm_per_arc_segment
< min_err_segment
) {
1341 arc_segment
= min_err_segment
;
1344 // Figure out how many segments for this gcode
1345 // 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
1346 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1348 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1349 float theta_per_segment
= angular_travel
/ segments
;
1350 float linear_per_segment
= linear_travel
/ segments
;
1352 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1353 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1354 r_T = [cos(phi) -sin(phi);
1355 sin(phi) cos(phi] * r ;
1356 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1357 defined from the circle center to the initial position. Each line segment is formed by successive
1358 vector rotations. This requires only two cos() and sin() computations to form the rotation
1359 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1360 all float numbers are single precision on the Arduino. (True float precision will not have
1361 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1362 tool precision in some cases. Therefore, arc path correction is implemented.
1364 Small angle approximation may be used to reduce computation overhead further. This approximation
1365 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1366 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1367 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1368 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1369 issue for CNC machines with the single precision Arduino calculations.
1370 This approximation also allows mc_arc to immediately insert a line segment into the planner
1371 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1372 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1373 This is important when there are successive arc motions.
1375 // Vector rotation matrix values
1376 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1377 float sin_T
= theta_per_segment
;
1379 float arc_target
[3];
1386 // Initialize the linear axis
1387 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1390 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1391 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1393 if (count
< this->arc_correction
) {
1394 // Apply vector rotation matrix
1395 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1396 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1400 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1401 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1402 cos_Ti
= cosf(i
* theta_per_segment
);
1403 sin_Ti
= sinf(i
* theta_per_segment
);
1404 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1405 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1409 // Update arc_target location
1410 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1411 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1412 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1414 // Append this segment to the queue
1415 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1419 // Ensure last segment arrives at target location.
1420 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1425 // Do the math for an arc and add it to the queue
1426 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1430 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1432 // Set clockwise/counter-clockwise sign for mc_arc computations
1433 bool is_clockwise
= false;
1434 if( motion_mode
== CW_ARC
) {
1435 is_clockwise
= true;
1439 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1443 float Robot::theta(float x
, float y
)
1445 float t
= atanf(x
/ fabs(y
));
1457 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1459 this->plane_axis_0
= axis_0
;
1460 this->plane_axis_1
= axis_1
;
1461 this->plane_axis_2
= axis_2
;
1464 void Robot::clearToolOffset()
1466 this->tool_offset
= wcs_t(0,0,0);
1469 void Robot::setToolOffset(const float offset
[3])
1471 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1474 float Robot::get_feed_rate() const
1476 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;