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()
44 #define default_seek_rate_checksum CHECKSUM("default_seek_rate")
45 #define default_feed_rate_checksum CHECKSUM("default_feed_rate")
46 #define mm_per_line_segment_checksum CHECKSUM("mm_per_line_segment")
47 #define delta_segments_per_second_checksum CHECKSUM("delta_segments_per_second")
48 #define mm_per_arc_segment_checksum CHECKSUM("mm_per_arc_segment")
49 #define mm_max_arc_error_checksum CHECKSUM("mm_max_arc_error")
50 #define arc_correction_checksum CHECKSUM("arc_correction")
51 #define x_axis_max_speed_checksum CHECKSUM("x_axis_max_speed")
52 #define y_axis_max_speed_checksum CHECKSUM("y_axis_max_speed")
53 #define z_axis_max_speed_checksum CHECKSUM("z_axis_max_speed")
54 #define segment_z_moves_checksum CHECKSUM("segment_z_moves")
55 #define save_g92_checksum CHECKSUM("save_g92")
56 #define set_g92_checksum CHECKSUM("set_g92")
59 #define arm_solution_checksum CHECKSUM("arm_solution")
60 #define cartesian_checksum CHECKSUM("cartesian")
61 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
62 #define rostock_checksum CHECKSUM("rostock")
63 #define linear_delta_checksum CHECKSUM("linear_delta")
64 #define rotary_delta_checksum CHECKSUM("rotary_delta")
65 #define delta_checksum CHECKSUM("delta")
66 #define hbot_checksum CHECKSUM("hbot")
67 #define corexy_checksum CHECKSUM("corexy")
68 #define corexz_checksum CHECKSUM("corexz")
69 #define kossel_checksum CHECKSUM("kossel")
70 #define morgan_checksum CHECKSUM("morgan")
72 // new-style actuator stuff
73 #define actuator_checksum CHEKCSUM("actuator")
75 #define step_pin_checksum CHECKSUM("step_pin")
76 #define dir_pin_checksum CHEKCSUM("dir_pin")
77 #define en_pin_checksum CHECKSUM("en_pin")
79 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
80 #define max_rate_checksum CHECKSUM("max_rate")
81 #define acceleration_checksum CHECKSUM("acceleration")
82 #define z_acceleration_checksum CHECKSUM("z_acceleration")
84 #define alpha_checksum CHECKSUM("alpha")
85 #define beta_checksum CHECKSUM("beta")
86 #define gamma_checksum CHECKSUM("gamma")
88 #define laser_module_default_power_checksum CHECKSUM("laser_module_default_power")
90 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
91 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
93 // The Robot converts GCodes into actual movements, and then adds them to the Planner, which passes them to the Conveyor so they can be added to the queue
94 // It takes care of cutting arcs into segments, same thing for line that are too long
98 this->inch_mode
= false;
99 this->absolute_mode
= true;
100 this->e_absolute_mode
= true;
101 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
102 memset(this->machine_position
, 0, sizeof machine_position
);
103 memset(this->compensated_machine_position
, 0, sizeof compensated_machine_position
);
104 this->arm_solution
= NULL
;
105 seconds_per_minute
= 60.0F
;
106 this->clearToolOffset();
107 this->compensationTransform
= nullptr;
108 this->get_e_scale_fnc
= nullptr;
109 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
110 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
111 this->next_command_is_MCS
= false;
112 this->disable_segmentation
= false;
113 this->disable_arm_solution
= false;
117 //Called when the module has just been loaded
118 void Robot::on_module_loaded()
120 this->register_for_event(ON_GCODE_RECEIVED
);
126 #define ACTUATOR_CHECKSUMS(X) { \
127 CHECKSUM(X "_step_pin"), \
128 CHECKSUM(X "_dir_pin"), \
129 CHECKSUM(X "_en_pin"), \
130 CHECKSUM(X "_steps_per_mm"), \
131 CHECKSUM(X "_max_rate"), \
132 CHECKSUM(X "_acceleration") \
135 void Robot::load_config()
137 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
138 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
139 // To make adding those solution easier, they have their own, separate object.
140 // Here we read the config to find out which arm solution to use
141 if (this->arm_solution
) delete this->arm_solution
;
142 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
143 // Note checksums are not const expressions when in debug mode, so don't use switch
144 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
145 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
147 } else if(solution_checksum
== corexz_checksum
) {
148 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
150 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
151 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
153 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
154 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
156 } else if(solution_checksum
== rotary_delta_checksum
) {
157 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== morgan_checksum
) {
160 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== cartesian_checksum
) {
163 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
166 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
169 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
170 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
171 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
172 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
173 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
174 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
175 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
177 // in mm/sec but specified in config as mm/min
178 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
179 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
180 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
182 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
183 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
184 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
186 // optional setting for a fixed G92 offset
187 std::vector
<float> t
= parse_number_list(g92
.c_str());
189 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
193 // default s value for laser
194 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
196 // Make our Primary XYZ StepperMotors, and potentially A B C
197 uint16_t const checksums
[][6] = {
198 ACTUATOR_CHECKSUMS("alpha"), // X
199 ACTUATOR_CHECKSUMS("beta"), // Y
200 ACTUATOR_CHECKSUMS("gamma"), // Z
201 #if MAX_ROBOT_ACTUATORS > 3
202 ACTUATOR_CHECKSUMS("delta"), // A
203 #if MAX_ROBOT_ACTUATORS > 4
204 ACTUATOR_CHECKSUMS("epsilon"), // B
205 #if MAX_ROBOT_ACTUATORS > 5
206 ACTUATOR_CHECKSUMS("zeta") // C
212 // default acceleration setting, can be overriden with newer per axis settings
213 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
216 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
217 Pin pins
[3]; //step, dir, enable
218 for (size_t i
= 0; i
< 3; i
++) {
219 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
222 if(!pins
[0].connected() || !pins
[1].connected()) { // step and dir must be defined, but enable is optional
224 THEKERNEL
->streams
->printf("FATAL: motor %c is not defined in config\n", 'X'+a
);
225 n_motors
= a
; // we only have this number of motors
228 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
231 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
232 // register this motor (NB This must be 0,1,2) of the actuators array
233 uint8_t n
= register_motor(sm
);
235 // this is a fatal error
236 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
240 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
241 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
242 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
245 check_max_actuator_speeds(); // check the configs are sane
247 // if we have not specified a z acceleration see if the legacy config was set
248 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
249 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
251 actuators
[Z_AXIS
]->set_acceleration(acc
);
255 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
256 // so the first move can be correct if homing is not performed
257 ActuatorCoordinates actuator_pos
;
258 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
259 for (size_t i
= 0; i
< n_motors
; i
++)
260 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
262 //this->clearToolOffset();
265 uint8_t Robot::register_motor(StepperMotor
*motor
)
267 // register this motor with the step ticker
268 THEKERNEL
->step_ticker
->register_motor(motor
);
269 if(n_motors
>= k_max_actuators
) {
270 // this is a fatal error
271 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
274 actuators
.push_back(motor
);
275 motor
->set_motor_id(n_motors
);
279 void Robot::push_state()
281 bool am
= this->absolute_mode
;
282 bool em
= this->e_absolute_mode
;
283 bool im
= this->inch_mode
;
284 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
288 void Robot::pop_state()
290 if(!state_stack
.empty()) {
291 auto s
= state_stack
.top();
293 this->feed_rate
= std::get
<0>(s
);
294 this->seek_rate
= std::get
<1>(s
);
295 this->absolute_mode
= std::get
<2>(s
);
296 this->e_absolute_mode
= std::get
<3>(s
);
297 this->inch_mode
= std::get
<4>(s
);
298 this->current_wcs
= std::get
<5>(s
);
302 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
304 std::vector
<wcs_t
> v
;
305 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
306 for(auto& i
: wcs_offsets
) {
309 v
.push_back(g92_offset
);
310 v
.push_back(tool_offset
);
314 void Robot::get_current_machine_position(float *pos
) const
316 // get real time current actuator position in mm
317 ActuatorCoordinates current_position
{
318 actuators
[X_AXIS
]->get_current_position(),
319 actuators
[Y_AXIS
]->get_current_position(),
320 actuators
[Z_AXIS
]->get_current_position()
323 // get machine position from the actuator position using FK
324 arm_solution
->actuator_to_cartesian(current_position
, pos
);
327 void Robot::print_position(uint8_t subcode
, std::string
& res
, bool ignore_extruders
) const
329 // M114.1 is a new way to do this (similar to how GRBL does it).
330 // it returns the realtime position based on the current step position of the actuators.
331 // this does require a FK to get a machine position from the actuator position
332 // and then invert all the transforms to get a workspace position from machine position
333 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
336 if(subcode
== 0) { // M114 print WCS
337 wcs_t pos
= mcs2wcs(machine_position
);
338 n
= snprintf(buf
, sizeof(buf
), "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
)));
340 } else if(subcode
== 4) {
341 // M114.4 print last milestone
342 n
= snprintf(buf
, sizeof(buf
), "MP: X:%1.4f Y:%1.4f Z:%1.4f", machine_position
[X_AXIS
], machine_position
[Y_AXIS
], machine_position
[Z_AXIS
]);
344 } else if(subcode
== 5) {
345 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
346 // will differ from LMS by the compensation at the current position otherwise
347 n
= snprintf(buf
, sizeof(buf
), "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
]);
350 // get real time positions
352 get_current_machine_position(mpos
);
354 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
355 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
357 if(subcode
== 1) { // M114.1 print realtime WCS
358 wcs_t pos
= mcs2wcs(mpos
);
359 n
= snprintf(buf
, sizeof(buf
), "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
)));
361 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
362 n
= snprintf(buf
, sizeof(buf
), "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
364 } else if(subcode
== 3) { // M114.3 print realtime actuator position
365 // get real time current actuator position in mm
366 ActuatorCoordinates current_position
{
367 actuators
[X_AXIS
]->get_current_position(),
368 actuators
[Y_AXIS
]->get_current_position(),
369 actuators
[Z_AXIS
]->get_current_position()
371 n
= snprintf(buf
, sizeof(buf
), "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position
[X_AXIS
], current_position
[Y_AXIS
], current_position
[Z_AXIS
]);
375 if(n
> sizeof(buf
)) n
= sizeof(buf
);
378 #if MAX_ROBOT_ACTUATORS > 3
379 // deal with the ABC axis
380 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
382 if(ignore_extruders
&& actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
383 if(subcode
== 4) { // M114.4 print last milestone
384 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
386 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
387 // current actuator position
388 n
= snprintf(buf
, sizeof(buf
), " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
390 if(n
> sizeof(buf
)) n
= sizeof(buf
);
391 if(n
> 0) res
.append(buf
, n
);
396 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
397 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
399 return std::make_tuple(
400 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
),
401 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
),
402 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
)
406 // this does a sanity check that actuator speeds do not exceed steps rate capability
407 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
408 void Robot::check_max_actuator_speeds()
410 for (size_t i
= 0; i
< n_motors
; i
++) {
411 if(actuators
[i
]->is_extruder()) continue; //extruders are not included in this check
413 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
414 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
415 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
416 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());
421 //A GCode has been received
422 //See if the current Gcode line has some orders for us
423 void Robot::on_gcode_received(void *argument
)
425 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
427 enum MOTION_MODE_T motion_mode
= NONE
;
431 case 0: motion_mode
= SEEK
; break;
432 case 1: motion_mode
= LINEAR
; break;
433 case 2: motion_mode
= CW_ARC
; break;
434 case 3: motion_mode
= CCW_ARC
; break;
435 case 4: { // G4 Dwell
436 uint32_t delay_ms
= 0;
437 if (gcode
->has_letter('P')) {
438 if(THEKERNEL
->is_grbl_mode()) {
439 // in grbl mode (and linuxcnc) P is decimal seconds
440 float f
= gcode
->get_value('P');
441 delay_ms
= f
* 1000.0F
;
444 // in reprap P is milliseconds, they always have to be different!
445 delay_ms
= gcode
->get_int('P');
448 if (gcode
->has_letter('S')) {
449 delay_ms
+= gcode
->get_int('S') * 1000;
453 THEKERNEL
->conveyor
->wait_for_idle();
454 // wait for specified time
455 uint32_t start
= us_ticker_read(); // mbed call
456 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
457 THEKERNEL
->call_event(ON_IDLE
, this);
458 if(THEKERNEL
->is_halted()) return;
464 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
465 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
466 size_t n
= gcode
->get_uint('P');
467 if(n
== 0) n
= current_wcs
; // set current coordinate system
471 std::tie(x
, y
, z
) = wcs_offsets
[n
];
472 if(gcode
->get_int('L') == 20) {
473 // this makes the current machine position (less compensation transform) the offset
474 // get current position in WCS
475 wcs_t pos
= mcs2wcs(machine_position
);
477 if(gcode
->has_letter('X')){
478 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
481 if(gcode
->has_letter('Y')){
482 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
484 if(gcode
->has_letter('Z')) {
485 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
490 // the value is the offset from machine zero
491 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
492 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
493 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
495 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
496 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
497 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
500 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
505 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
506 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
507 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
508 case 20: this->inch_mode
= true; break;
509 case 21: this->inch_mode
= false; break;
511 case 54: case 55: case 56: case 57: case 58: case 59:
512 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
513 current_wcs
= gcode
->g
- 54;
514 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
515 current_wcs
+= gcode
->subcode
;
516 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
520 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
521 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
524 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
525 // reset G92 offsets to 0
526 g92_offset
= wcs_t(0, 0, 0);
528 } else if(gcode
->subcode
== 3) {
529 // initialize G92 to the specified values, only used for saving it with M500
530 float x
= 0, y
= 0, z
= 0;
531 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
532 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
533 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
534 g92_offset
= wcs_t(x
, y
, z
);
537 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
539 std::tie(x
, y
, z
) = g92_offset
;
540 // get current position in WCS
541 wcs_t pos
= mcs2wcs(machine_position
);
543 // adjust g92 offset to make the current wpos == the value requested
544 if(gcode
->has_letter('X')){
545 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
547 if(gcode
->has_letter('Y')){
548 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
550 if(gcode
->has_letter('Z')) {
551 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
553 g92_offset
= wcs_t(x
, y
, z
);
556 #if MAX_ROBOT_ACTUATORS > 3
557 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
558 // reset the E position, legacy for 3d Printers to be reprap compatible
559 // find the selected extruder
560 int selected_extruder
= get_active_extruder();
561 if(selected_extruder
> 0) {
562 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
563 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
564 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
573 } else if( gcode
->has_m
) {
575 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
576 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
579 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
580 if(!THEKERNEL
->is_grbl_mode()) break;
581 // fall through to M2
582 case 2: // M2 end of program
584 absolute_mode
= true;
587 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
590 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
591 if(gcode
->get_num_args() > 0) {
592 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
594 for (int i
= 0; i
< n_motors
; ++i
) {
595 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
596 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
598 // handle E parameter as currently selected extruder ABC
599 if(gcode
->has_letter('E')) {
600 // find first selected extruder
601 int i
= get_active_extruder();
603 bm
|= (0x02<<i
); // set appropriate bit
607 THEKERNEL
->conveyor
->wait_for_idle();
608 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
613 THEKERNEL
->conveyor
->wait_for_idle();
614 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
617 case 82: e_absolute_mode
= true; break;
618 case 83: e_absolute_mode
= false; break;
620 case 92: // M92 - set steps per mm
621 for (int i
= 0; i
< n_motors
; ++i
) {
622 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
623 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
624 if(gcode
->has_letter(axis
)) {
625 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
627 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
629 gcode
->add_nl
= true;
630 check_max_actuator_speeds();
635 print_position(gcode
->subcode
, buf
, true); // ignore extruders as they will print E themselves
636 gcode
->txt_after_ok
.append(buf
);
640 case 120: // push state
644 case 121: // pop state
648 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
649 if(gcode
->get_num_args() == 0) {
650 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
651 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
653 if(gcode
->subcode
== 1) {
654 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
655 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
656 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
660 gcode
->add_nl
= true;
663 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
664 if (gcode
->has_letter('X' + i
)) {
665 float v
= gcode
->get_value('X'+i
);
666 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
667 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
671 if(gcode
->subcode
== 1) {
672 // ABC axis only handle actuator max speeds
673 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
674 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
675 int c
= 'A' + i
- A_AXIS
;
676 if(gcode
->has_letter(c
)) {
677 float v
= gcode
->get_value(c
);
678 actuators
[i
]->set_max_rate(v
);
684 // this format is deprecated
685 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
686 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
687 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
688 if (gcode
->has_letter('A' + i
)) {
689 float v
= gcode
->get_value('A'+i
);
690 actuators
[i
]->set_max_rate(v
);
695 if(gcode
->subcode
== 1) check_max_actuator_speeds();
699 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
700 if (gcode
->has_letter('S')) {
701 float acc
= gcode
->get_value('S'); // mm/s^2
703 if (acc
< 1.0F
) acc
= 1.0F
;
704 this->default_acceleration
= acc
;
706 for (int i
= 0; i
< n_motors
; ++i
) {
707 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
708 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
709 if(gcode
->has_letter(axis
)) {
710 float acc
= gcode
->get_value(axis
); // mm/s^2
712 if (acc
<= 0.0F
) acc
= NAN
;
713 actuators
[i
]->set_acceleration(acc
);
718 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
719 if (gcode
->has_letter('X')) {
720 float jd
= gcode
->get_value('X');
724 THEKERNEL
->planner
->junction_deviation
= jd
;
726 if (gcode
->has_letter('Z')) {
727 float jd
= gcode
->get_value('Z');
728 // enforce minimum, -1 disables it and uses regular junction deviation
731 THEKERNEL
->planner
->z_junction_deviation
= jd
;
733 if (gcode
->has_letter('S')) {
734 float mps
= gcode
->get_value('S');
738 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
742 case 220: // M220 - speed override percentage
743 if (gcode
->has_letter('S')) {
744 float factor
= gcode
->get_value('S');
745 // enforce minimum 10% speed
748 // enforce maximum 10x speed
749 if (factor
> 1000.0F
)
752 seconds_per_minute
= 6000.0F
/ factor
;
754 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
758 case 400: // wait until all moves are done up to this point
759 THEKERNEL
->conveyor
->wait_for_idle();
762 case 500: // M500 saves some volatile settings to config override file
763 case 503: { // M503 just prints the settings
764 gcode
->stream
->printf(";Steps per unit:\nM92 ");
765 for (int i
= 0; i
< n_motors
; ++i
) {
766 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
767 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
768 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
770 gcode
->stream
->printf("\n");
772 // only print if not NAN
773 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
774 for (int i
= 0; i
< n_motors
; ++i
) {
775 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
776 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
777 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
779 gcode
->stream
->printf("\n");
781 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
);
783 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
]);
785 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
786 for (int i
= 0; i
< n_motors
; ++i
) {
787 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
788 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
789 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
791 gcode
->stream
->printf("\n");
793 // get or save any arm solution specific optional values
794 BaseSolution::arm_options_t options
;
795 if(arm_solution
->get_optional(options
) && !options
.empty()) {
796 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
797 for(auto &i
: options
) {
798 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
800 gcode
->stream
->printf("\n");
803 // save wcs_offsets and current_wcs
804 // TODO this may need to be done whenever they change to be compliant
805 gcode
->stream
->printf(";WCS settings\n");
806 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
808 for(auto &i
: wcs_offsets
) {
809 if(i
!= wcs_t(0, 0, 0)) {
811 std::tie(x
, y
, z
) = i
;
812 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
817 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
818 // 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
819 if(g92_offset
!= wcs_t(0, 0, 0)) {
821 std::tie(x
, y
, z
) = g92_offset
;
822 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
828 case 665: { // M665 set optional arm solution variables based on arm solution.
829 // the parameter args could be any letter each arm solution only accepts certain ones
830 BaseSolution::arm_options_t options
= gcode
->get_args();
831 options
.erase('S'); // don't include the S
832 options
.erase('U'); // don't include the U
833 if(options
.size() > 0) {
834 // set the specified options
835 arm_solution
->set_optional(options
);
838 if(arm_solution
->get_optional(options
)) {
839 // foreach optional value
840 for(auto &i
: options
) {
841 // print all current values of supported options
842 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
843 gcode
->add_nl
= true;
847 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
848 this->delta_segments_per_second
= gcode
->get_value('S');
849 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
851 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
852 this->mm_per_line_segment
= gcode
->get_value('U');
853 this->delta_segments_per_second
= 0;
854 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
862 if( motion_mode
!= NONE
) {
863 is_g123
= motion_mode
!= SEEK
;
864 process_move(gcode
, motion_mode
);
870 next_command_is_MCS
= false; // must be on same line as G0 or G1
873 int Robot::get_active_extruder() const
875 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
876 // find first selected extruder
877 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
882 // process a G0/G1/G2/G3
883 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
885 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
886 // get XYZ and one E (which goes to the selected extruder)
887 float param
[4]{NAN
, NAN
, NAN
, NAN
};
889 // process primary axis
890 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
892 if( gcode
->has_letter(letter
) ) {
893 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
897 float offset
[3]{0,0,0};
898 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
899 if( gcode
->has_letter(letter
) ) {
900 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
904 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
905 float target
[n_motors
];
906 memcpy(target
, machine_position
, n_motors
*sizeof(float));
908 if(!next_command_is_MCS
) {
909 if(this->absolute_mode
) {
910 // apply wcs offsets and g92 offset and tool offset
911 if(!isnan(param
[X_AXIS
])) {
912 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
);
915 if(!isnan(param
[Y_AXIS
])) {
916 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
);
919 if(!isnan(param
[Z_AXIS
])) {
920 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
);
924 // they are deltas from the machine_position if specified
925 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
926 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
931 // already in machine coordinates, we do not add wcs or tool offset for that
932 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
933 if(!isnan(param
[i
])) target
[i
] = param
[i
];
939 #if MAX_ROBOT_ACTUATORS > 3
940 // process extruder parameters, for active extruder only (only one active extruder at a time)
941 int selected_extruder
= 0;
942 if(gcode
->has_letter('E')) {
943 selected_extruder
= get_active_extruder();
944 param
[E_AXIS
]= gcode
->get_value('E');
947 // do E for the selected extruder
948 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
949 if(this->e_absolute_mode
) {
950 target
[selected_extruder
]= param
[E_AXIS
];
951 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
953 delta_e
= param
[E_AXIS
];
954 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
958 // 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
959 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
960 char letter
= 'A'+i
-A_AXIS
;
961 if(gcode
->has_letter(letter
)) {
962 float p
= gcode
->get_value(letter
);
963 if(this->absolute_mode
) {
966 target
[i
]= p
+ machine_position
[i
];
972 if( gcode
->has_letter('F') ) {
973 if( motion_mode
== SEEK
)
974 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
976 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
979 // S is modal When specified on a G0/1/2/3 command
980 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
984 // Perform any physical actions
985 switch(motion_mode
) {
989 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
993 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
998 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
999 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1004 // set machine_position to the calculated target
1005 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1009 // reset the machine position for all axis. Used for homing.
1010 // after homing we supply the cartesian coordinates that the head is at when homed,
1011 // however for Z this is the compensated machine position (if enabled)
1012 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1013 // this will make the results from M114 and ? consistent after homing.
1014 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1015 void Robot::reset_axis_position(float x
, float y
, float z
)
1017 // set both the same initially
1018 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1019 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1020 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1022 if(compensationTransform
) {
1023 // apply inverse transform to get machine_position
1024 compensationTransform(machine_position
, true);
1027 // now set the actuator positions based on the supplied compensated position
1028 ActuatorCoordinates actuator_pos
;
1029 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1030 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1031 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1034 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1035 void Robot::reset_axis_position(float position
, int axis
)
1037 compensated_machine_position
[axis
] = position
;
1038 if(axis
<= Z_AXIS
) {
1039 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1041 #if MAX_ROBOT_ACTUATORS > 3
1042 }else if(axis
< n_motors
) {
1043 // ABC and/or extruders need to be set as there is no arm solution for them
1044 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1045 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1050 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1051 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1052 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1054 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1055 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1058 // now correct axis positions then recorrect actuator to account for rounding
1059 reset_position_from_current_actuator_position();
1062 // Use FK to find out where actuator is and reset to match
1063 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1064 void Robot::reset_position_from_current_actuator_position()
1066 ActuatorCoordinates actuator_pos
;
1067 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1068 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1069 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1072 // discover machine position from where actuators actually are
1073 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1074 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1076 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1077 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1079 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1080 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1081 // to get everything in perfect sync.
1082 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1083 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1084 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1087 // Handle extruders and/or ABC axis
1088 #if MAX_ROBOT_ACTUATORS > 3
1089 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1090 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1091 float ap
= actuator_pos
[i
];
1092 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
1093 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1094 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1099 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1100 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1101 // all transforms and is what we actually convert to actuator positions
1102 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1104 float deltas
[n_motors
];
1105 float transformed_target
[n_motors
]; // adjust target for bed compensation
1106 float unit_vec
[N_PRIMARY_AXIS
];
1108 // unity transform by default
1109 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1111 // check function pointer and call if set to transform the target to compensate for bed
1112 if(compensationTransform
) {
1113 // some compensation strategies can transform XYZ, some just change Z
1114 compensationTransform(transformed_target
, false);
1118 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1120 // find distance moved by each axis, use transformed target from the current compensated machine position
1121 for (size_t i
= 0; i
< n_motors
; i
++) {
1122 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1123 if(deltas
[i
] == 0) continue;
1124 // at least one non zero delta
1126 if(i
< N_PRIMARY_AXIS
) {
1127 sos
+= powf(deltas
[i
], 2);
1132 if(!move
) return false;
1134 // see if this is a primary axis move or not
1135 bool auxilliary_move
= true;
1136 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1137 if(deltas
[i
] != 0) {
1138 auxilliary_move
= false;
1143 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1144 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1146 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1147 // 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
1148 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1151 if(!auxilliary_move
) {
1152 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1153 // find distance unit vector for primary axis only
1154 unit_vec
[i
] = deltas
[i
] / distance
;
1156 // Do not move faster than the configured cartesian limits for XYZ
1157 if ( i
<= Z_AXIS
&& max_speeds
[i
] > 0 ) {
1158 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1160 if (axis_speed
> max_speeds
[i
])
1161 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1166 // find actuator position given the machine position, use actual adjusted target
1167 ActuatorCoordinates actuator_pos
;
1168 if(!disable_arm_solution
) {
1169 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1172 // basically the same as cartesian, would be used for special homing situations like for scara
1173 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1174 actuator_pos
[i
] = transformed_target
[i
];
1178 #if MAX_ROBOT_ACTUATORS > 3
1180 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1181 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1182 actuator_pos
[i
]= transformed_target
[i
];
1183 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1184 // NOTE this relies on the fact only one extruder is active at a time
1185 // scale for volumetric or flow rate
1186 // TODO is this correct? scaling the absolute target? what if the scale changes?
1187 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1188 actuator_pos
[i
] *= get_e_scale_fnc();
1190 if(auxilliary_move
) {
1191 // for E only moves we need to use the scaled E to calculate the distance
1192 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1195 if(auxilliary_move
) {
1196 distance
= sqrtf(sos
); // distance in mm of the e move
1197 if(distance
< 0.00001F
) return false;
1201 // use default acceleration to start with
1202 float acceleration
= default_acceleration
;
1204 float isecs
= rate_mm_s
/ distance
;
1206 // check per-actuator speed limits
1207 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1208 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1209 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1211 float actuator_rate
= d
* isecs
;
1212 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1213 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1214 isecs
= rate_mm_s
/ distance
;
1217 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1218 // 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.
1219 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1220 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1221 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1222 float ca
= fabsf((d
/distance
) * acceleration
);
1224 acceleration
*= ( ma
/ ca
);
1230 // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
1231 while(THEKERNEL
->get_feed_hold()) {
1232 THEKERNEL
->call_event(ON_IDLE
, this);
1233 // if we also got a HALT then break out of this
1234 if(THEKERNEL
->is_halted()) return false;
1237 // Append the block to the planner
1238 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1239 // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
1240 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1241 // this is the new compensated machine position
1242 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1250 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1251 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1253 if(THEKERNEL
->is_halted()) return false;
1255 // catch negative or zero feed rates
1256 if(rate_mm_s
<= 0.0F
) {
1260 // get the absolute target position, default is current machine_position
1261 float target
[n_motors
];
1262 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1264 // add in the deltas to get new target
1265 for (int i
= 0; i
< naxis
; i
++) {
1266 target
[i
] += delta
[i
];
1269 // submit for planning and if moved update machine_position
1270 if(append_milestone(target
, rate_mm_s
)) {
1271 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1278 // Append a move to the queue ( cutting it into segments if needed )
1279 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1281 // catch negative or zero feed rates and return the same error as GRBL does
1282 if(rate_mm_s
<= 0.0F
) {
1283 gcode
->is_error
= true;
1284 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1288 // Find out the distance for this move in XYZ in MCS
1289 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 ));
1291 if(millimeters_of_travel
< 0.00001F
) {
1292 // we have no movement in XYZ, probably E only extrude or retract
1293 return this->append_milestone(target
, rate_mm_s
);
1297 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1298 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1299 We ask Extruder to do all the work but we need to pass in the relevant data.
1300 NOTE we need to do this before we segment the line (for deltas)
1302 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1303 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1304 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1305 rate_mm_s
*= data
[1]; // adjust the feedrate
1309 // 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.
1310 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1311 // 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
1314 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1317 } else if(this->delta_segments_per_second
> 1.0F
) {
1318 // enabled if set to something > 1, it is set to 0.0 by default
1319 // segment based on current speed and requested segments per second
1320 // the faster the travel speed the fewer segments needed
1321 // NOTE rate is mm/sec and we take into account any speed override
1322 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1323 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1324 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1327 if(this->mm_per_line_segment
== 0.0F
) {
1328 segments
= 1; // don't split it up
1330 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1336 // A vector to keep track of the endpoint of each segment
1337 float segment_delta
[n_motors
];
1338 float segment_end
[n_motors
];
1339 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1341 // How far do we move each segment?
1342 for (int i
= 0; i
< n_motors
; i
++)
1343 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1345 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1346 // We always add another point after this loop so we stop at segments-1, ie i < segments
1347 for (int i
= 1; i
< segments
; i
++) {
1348 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1349 for (int j
= 0; j
< n_motors
; j
++)
1350 segment_end
[j
] += segment_delta
[j
];
1352 // Append the end of this segment to the queue
1353 // this can block waiting for free block queue or if in feed hold
1354 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1359 // Append the end of this full move to the queue
1360 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1362 this->next_command_is_MCS
= false; // always reset this
1368 // Append an arc to the queue ( cutting it into segments as needed )
1369 // TODO does not support any E parameters so cannot be used for 3D printing.
1370 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1372 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1373 // catch negative or zero feed rates and return the same error as GRBL does
1374 if(rate_mm_s
<= 0.0F
) {
1375 gcode
->is_error
= true;
1376 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1381 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1382 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1383 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1384 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1385 float r_axis1
= -offset
[this->plane_axis_1
];
1386 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1387 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1389 // Patch from GRBL Firmware - Christoph Baumann 04072015
1390 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1391 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1392 if (plane_axis_2
== Y_AXIS
) { is_clockwise
= !is_clockwise
; } //Math for XZ plane is revere of other 2 planes
1393 if (is_clockwise
) { // Correct atan2 output per direction
1394 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1396 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1399 // Find the distance for this gcode
1400 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1402 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1403 if( millimeters_of_travel
< 0.00001F
) {
1407 // limit segments by maximum arc error
1408 float arc_segment
= this->mm_per_arc_segment
;
1409 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1410 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1411 if (this->mm_per_arc_segment
< min_err_segment
) {
1412 arc_segment
= min_err_segment
;
1415 // Figure out how many segments for this gcode
1416 // 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
1417 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1419 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1420 float theta_per_segment
= angular_travel
/ segments
;
1421 float linear_per_segment
= linear_travel
/ segments
;
1423 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1424 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1425 r_T = [cos(phi) -sin(phi);
1426 sin(phi) cos(phi] * r ;
1427 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1428 defined from the circle center to the initial position. Each line segment is formed by successive
1429 vector rotations. This requires only two cos() and sin() computations to form the rotation
1430 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1431 all float numbers are single precision on the Arduino. (True float precision will not have
1432 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1433 tool precision in some cases. Therefore, arc path correction is implemented.
1435 Small angle approximation may be used to reduce computation overhead further. This approximation
1436 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1437 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1438 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1439 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1440 issue for CNC machines with the single precision Arduino calculations.
1441 This approximation also allows mc_arc to immediately insert a line segment into the planner
1442 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1443 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1444 This is important when there are successive arc motions.
1446 // Vector rotation matrix values
1447 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1448 float sin_T
= theta_per_segment
;
1450 // TODO we need to handle the ABC axis here by segmenting them
1451 float arc_target
[n_motors
];
1458 // init array for all axis
1459 memcpy(arc_target
, machine_position
, n_motors
*sizeof(float));
1461 // Initialize the linear axis
1462 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1465 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1466 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1468 if (count
< this->arc_correction
) {
1469 // Apply vector rotation matrix
1470 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1471 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1475 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1476 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1477 cos_Ti
= cosf(i
* theta_per_segment
);
1478 sin_Ti
= sinf(i
* theta_per_segment
);
1479 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1480 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1484 // Update arc_target location
1485 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1486 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1487 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1489 // Append this segment to the queue
1490 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1494 // Ensure last segment arrives at target location.
1495 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1500 // Do the math for an arc and add it to the queue
1501 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1505 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1507 // Set clockwise/counter-clockwise sign for mc_arc computations
1508 bool is_clockwise
= false;
1509 if( motion_mode
== CW_ARC
) {
1510 is_clockwise
= true;
1514 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1518 float Robot::theta(float x
, float y
)
1520 float t
= atanf(x
/ fabs(y
));
1532 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1534 this->plane_axis_0
= axis_0
;
1535 this->plane_axis_1
= axis_1
;
1536 this->plane_axis_2
= axis_2
;
1539 void Robot::clearToolOffset()
1541 this->tool_offset
= wcs_t(0,0,0);
1544 void Robot::setToolOffset(const float offset
[3])
1546 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1549 float Robot::get_feed_rate() const
1551 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;