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"
36 #include "EndstopsPublicAccess.h"
38 #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 enable_checksum CHECKSUM("enable")
92 #define halt_checksum CHECKSUM("halt")
93 #define soft_endstop_checksum CHECKSUM("soft_endstop")
94 #define xmin_checksum CHECKSUM("x_min")
95 #define ymin_checksum CHECKSUM("y_min")
96 #define zmin_checksum CHECKSUM("z_min")
97 #define xmax_checksum CHECKSUM("x_max")
98 #define ymax_checksum CHECKSUM("y_max")
99 #define zmax_checksum CHECKSUM("z_max")
102 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
103 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
105 // 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
106 // It takes care of cutting arcs into segments, same thing for line that are too long
110 this->inch_mode
= false;
111 this->absolute_mode
= true;
112 this->e_absolute_mode
= true;
113 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
114 memset(this->machine_position
, 0, sizeof machine_position
);
115 memset(this->compensated_machine_position
, 0, sizeof compensated_machine_position
);
116 this->arm_solution
= NULL
;
117 seconds_per_minute
= 60.0F
;
118 this->clearToolOffset();
119 this->compensationTransform
= nullptr;
120 this->get_e_scale_fnc
= nullptr;
121 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
122 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
123 this->next_command_is_MCS
= false;
124 this->disable_segmentation
= false;
125 this->disable_arm_solution
= false;
129 //Called when the module has just been loaded
130 void Robot::on_module_loaded()
132 this->register_for_event(ON_GCODE_RECEIVED
);
138 #define ACTUATOR_CHECKSUMS(X) { \
139 CHECKSUM(X "_step_pin"), \
140 CHECKSUM(X "_dir_pin"), \
141 CHECKSUM(X "_en_pin"), \
142 CHECKSUM(X "_steps_per_mm"), \
143 CHECKSUM(X "_max_rate"), \
144 CHECKSUM(X "_acceleration") \
147 void Robot::load_config()
149 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
150 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
151 // To make adding those solution easier, they have their own, separate object.
152 // Here we read the config to find out which arm solution to use
153 if (this->arm_solution
) delete this->arm_solution
;
154 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
155 // Note checksums are not const expressions when in debug mode, so don't use switch
156 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
157 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
159 } else if(solution_checksum
== corexz_checksum
) {
160 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
162 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
163 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
165 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
166 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
168 } else if(solution_checksum
== rotary_delta_checksum
) {
169 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
171 } else if(solution_checksum
== morgan_checksum
) {
172 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
174 } else if(solution_checksum
== cartesian_checksum
) {
175 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
178 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
181 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
182 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
183 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
184 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
185 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
186 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
187 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
189 // in mm/sec but specified in config as mm/min
190 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
191 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
192 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
194 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
195 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
196 string g92
= THEKERNEL
->config
->value(set_g92_checksum
)->by_default("")->as_string();
198 // optional setting for a fixed G92 offset
199 std::vector
<float> t
= parse_number_list(g92
.c_str());
201 g92_offset
= wcs_t(t
[0], t
[1], t
[2]);
205 // default s value for laser
206 this->s_value
= THEKERNEL
->config
->value(laser_module_default_power_checksum
)->by_default(0.8F
)->as_number();
208 // Make our Primary XYZ StepperMotors, and potentially A B C
209 uint16_t const motor_checksums
[][6] = {
210 ACTUATOR_CHECKSUMS("alpha"), // X
211 ACTUATOR_CHECKSUMS("beta"), // Y
212 ACTUATOR_CHECKSUMS("gamma"), // Z
213 #if MAX_ROBOT_ACTUATORS > 3
214 ACTUATOR_CHECKSUMS("delta"), // A
215 #if MAX_ROBOT_ACTUATORS > 4
216 ACTUATOR_CHECKSUMS("epsilon"), // B
217 #if MAX_ROBOT_ACTUATORS > 5
218 ACTUATOR_CHECKSUMS("zeta") // C
224 // default acceleration setting, can be overriden with newer per axis settings
225 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
228 for (size_t a
= 0; a
< MAX_ROBOT_ACTUATORS
; a
++) {
229 Pin pins
[3]; //step, dir, enable
230 for (size_t i
= 0; i
< 3; i
++) {
231 pins
[i
].from_string(THEKERNEL
->config
->value(motor_checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
234 if(!pins
[0].connected() || !pins
[1].connected()) { // step and dir must be defined, but enable is optional
236 THEKERNEL
->streams
->printf("FATAL: motor %c is not defined in config\n", 'X'+a
);
237 n_motors
= a
; // we only have this number of motors
240 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
243 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
244 // register this motor (NB This must be 0,1,2) of the actuators array
245 uint8_t n
= register_motor(sm
);
247 // this is a fatal error
248 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
252 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(motor_checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
253 actuators
[a
]->set_max_rate(THEKERNEL
->config
->value(motor_checksums
[a
][4])->by_default(30000.0F
)->as_number()/60.0F
); // it is in mm/min and converted to mm/sec
254 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(motor_checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
257 check_max_actuator_speeds(); // check the configs are sane
259 // if we have not specified a z acceleration see if the legacy config was set
260 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
261 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
263 actuators
[Z_AXIS
]->set_acceleration(acc
);
267 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
268 // so the first move can be correct if homing is not performed
269 ActuatorCoordinates actuator_pos
;
270 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
271 for (size_t i
= 0; i
< n_motors
; i
++)
272 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
274 //this->clearToolOffset();
276 soft_endstop_enabled
= THEKERNEL
->config
->value(soft_endstop_checksum
, enable_checksum
)->by_default(false)->as_bool();
277 soft_endstop_halt
= THEKERNEL
->config
->value(soft_endstop_checksum
, halt_checksum
)->by_default(true)->as_bool();
279 soft_endstop_min
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmin_checksum
)->by_default(NAN
)->as_number();
280 soft_endstop_min
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymin_checksum
)->by_default(NAN
)->as_number();
281 soft_endstop_min
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmin_checksum
)->by_default(NAN
)->as_number();
282 soft_endstop_max
[X_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, xmax_checksum
)->by_default(NAN
)->as_number();
283 soft_endstop_max
[Y_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, ymax_checksum
)->by_default(NAN
)->as_number();
284 soft_endstop_max
[Z_AXIS
]= THEKERNEL
->config
->value(soft_endstop_checksum
, zmax_checksum
)->by_default(NAN
)->as_number();
287 uint8_t Robot::register_motor(StepperMotor
*motor
)
289 // register this motor with the step ticker
290 THEKERNEL
->step_ticker
->register_motor(motor
);
291 if(n_motors
>= k_max_actuators
) {
292 // this is a fatal error
293 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
296 actuators
.push_back(motor
);
297 motor
->set_motor_id(n_motors
);
301 void Robot::push_state()
303 bool am
= this->absolute_mode
;
304 bool em
= this->e_absolute_mode
;
305 bool im
= this->inch_mode
;
306 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
310 void Robot::pop_state()
312 if(!state_stack
.empty()) {
313 auto s
= state_stack
.top();
315 this->feed_rate
= std::get
<0>(s
);
316 this->seek_rate
= std::get
<1>(s
);
317 this->absolute_mode
= std::get
<2>(s
);
318 this->e_absolute_mode
= std::get
<3>(s
);
319 this->inch_mode
= std::get
<4>(s
);
320 this->current_wcs
= std::get
<5>(s
);
324 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
326 std::vector
<wcs_t
> v
;
327 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
328 for(auto& i
: wcs_offsets
) {
331 v
.push_back(g92_offset
);
332 v
.push_back(tool_offset
);
336 void Robot::get_current_machine_position(float *pos
) const
338 // get real time current actuator position in mm
339 ActuatorCoordinates current_position
{
340 actuators
[X_AXIS
]->get_current_position(),
341 actuators
[Y_AXIS
]->get_current_position(),
342 actuators
[Z_AXIS
]->get_current_position()
345 // get machine position from the actuator position using FK
346 arm_solution
->actuator_to_cartesian(current_position
, pos
);
349 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
351 // M114.1 is a new way to do this (similar to how GRBL does it).
352 // it returns the realtime position based on the current step position of the actuators.
353 // this does require a FK to get a machine position from the actuator position
354 // and then invert all the transforms to get a workspace position from machine position
355 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
357 if(subcode
== 0) { // M114 print WCS
358 wcs_t pos
= mcs2wcs(machine_position
);
359 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
)));
361 } else if(subcode
== 4) {
362 // M114.4 print last milestone
363 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
]);
365 } else if(subcode
== 5) {
366 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
367 // will differ from LMS by the compensation at the current position otherwise
368 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
]);
371 // get real time positions
373 get_current_machine_position(mpos
);
375 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
376 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
378 if(subcode
== 1) { // M114.1 print realtime WCS
379 wcs_t pos
= mcs2wcs(mpos
);
380 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
)));
382 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
383 n
= snprintf(buf
, bufsize
, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
385 } else if(subcode
== 3) { // M114.3 print realtime actuator position
386 // get real time current actuator position in mm
387 ActuatorCoordinates current_position
{
388 actuators
[X_AXIS
]->get_current_position(),
389 actuators
[Y_AXIS
]->get_current_position(),
390 actuators
[Z_AXIS
]->get_current_position()
392 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
]);
396 #if MAX_ROBOT_ACTUATORS > 3
397 // deal with the ABC axis
398 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
399 if(actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
400 if(subcode
== 4) { // M114.4 print last milestone
401 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
403 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
404 // current actuator position
405 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
413 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
414 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
416 return std::make_tuple(
417 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
),
418 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
),
419 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
)
423 // this does a sanity check that actuator speeds do not exceed steps rate capability
424 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
425 void Robot::check_max_actuator_speeds()
427 for (size_t i
= 0; i
< n_motors
; i
++) {
428 if(actuators
[i
]->is_extruder()) continue; //extruders are not included in this check
430 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
431 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
432 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
433 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());
438 //A GCode has been received
439 //See if the current Gcode line has some orders for us
440 void Robot::on_gcode_received(void *argument
)
442 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
444 enum MOTION_MODE_T motion_mode
= NONE
;
448 case 0: motion_mode
= SEEK
; break;
449 case 1: motion_mode
= LINEAR
; break;
450 case 2: motion_mode
= CW_ARC
; break;
451 case 3: motion_mode
= CCW_ARC
; break;
452 case 4: { // G4 Dwell
453 uint32_t delay_ms
= 0;
454 if (gcode
->has_letter('P')) {
455 if(THEKERNEL
->is_grbl_mode()) {
456 // in grbl mode (and linuxcnc) P is decimal seconds
457 float f
= gcode
->get_value('P');
458 delay_ms
= f
* 1000.0F
;
461 // in reprap P is milliseconds, they always have to be different!
462 delay_ms
= gcode
->get_int('P');
465 if (gcode
->has_letter('S')) {
466 delay_ms
+= gcode
->get_int('S') * 1000;
470 THEKERNEL
->conveyor
->wait_for_idle();
471 // wait for specified time
472 uint32_t start
= us_ticker_read(); // mbed call
473 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
474 THEKERNEL
->call_event(ON_IDLE
, this);
475 if(THEKERNEL
->is_halted()) return;
481 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
482 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
483 size_t n
= gcode
->get_uint('P');
484 if(n
== 0) n
= current_wcs
; // set current coordinate system
488 std::tie(x
, y
, z
) = wcs_offsets
[n
];
489 if(gcode
->get_int('L') == 20) {
490 // this makes the current machine position (less compensation transform) the offset
491 // get current position in WCS
492 wcs_t pos
= mcs2wcs(machine_position
);
494 if(gcode
->has_letter('X')){
495 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
498 if(gcode
->has_letter('Y')){
499 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
501 if(gcode
->has_letter('Z')) {
502 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
507 // the value is the offset from machine zero
508 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
509 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
510 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
512 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
513 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
514 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
517 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
522 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
523 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
524 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
525 case 20: this->inch_mode
= true; break;
526 case 21: this->inch_mode
= false; break;
528 case 54: case 55: case 56: case 57: case 58: case 59:
529 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
530 current_wcs
= gcode
->g
- 54;
531 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
532 current_wcs
+= gcode
->subcode
;
533 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
537 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
538 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
541 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
542 // reset G92 offsets to 0
543 g92_offset
= wcs_t(0, 0, 0);
545 } else if(gcode
->subcode
== 3) {
546 // initialize G92 to the specified values, only used for saving it with M500
547 float x
= 0, y
= 0, z
= 0;
548 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
549 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
550 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
551 g92_offset
= wcs_t(x
, y
, z
);
554 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
556 std::tie(x
, y
, z
) = g92_offset
;
557 // get current position in WCS
558 wcs_t pos
= mcs2wcs(machine_position
);
560 // adjust g92 offset to make the current wpos == the value requested
561 if(gcode
->has_letter('X')){
562 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
564 if(gcode
->has_letter('Y')){
565 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
567 if(gcode
->has_letter('Z')) {
568 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
570 g92_offset
= wcs_t(x
, y
, z
);
573 #if MAX_ROBOT_ACTUATORS > 3
574 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
575 // reset the E position, legacy for 3d Printers to be reprap compatible
576 // find the selected extruder
577 int selected_extruder
= get_active_extruder();
578 if(selected_extruder
> 0) {
579 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
580 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
581 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
590 } else if( gcode
->has_m
) {
592 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
593 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
596 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
597 if(!THEKERNEL
->is_grbl_mode()) break;
598 // fall through to M2
599 case 2: // M2 end of program
601 absolute_mode
= true;
604 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
607 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
608 if(gcode
->get_num_args() > 0) {
609 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
611 for (int i
= 0; i
< n_motors
; ++i
) {
612 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
613 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
615 // handle E parameter as currently selected extruder ABC
616 if(gcode
->has_letter('E')) {
617 // find first selected extruder
618 int i
= get_active_extruder();
620 bm
|= (0x02<<i
); // set appropriate bit
624 THEKERNEL
->conveyor
->wait_for_idle();
625 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
630 THEKERNEL
->conveyor
->wait_for_idle();
631 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
634 case 82: e_absolute_mode
= true; break;
635 case 83: e_absolute_mode
= false; break;
637 case 92: // M92 - set steps per mm
638 for (int i
= 0; i
< n_motors
; ++i
) {
639 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
640 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
641 if(gcode
->has_letter(axis
)) {
642 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
644 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
646 gcode
->add_nl
= true;
647 check_max_actuator_speeds();
652 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
653 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
657 case 120: // push state
661 case 121: // pop state
665 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
666 if(gcode
->get_num_args() == 0) {
667 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
668 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
670 if(gcode
->subcode
== 1) {
671 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
672 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
673 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
677 gcode
->add_nl
= true;
680 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
681 if (gcode
->has_letter('X' + i
)) {
682 float v
= gcode
->get_value('X'+i
);
683 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
684 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
688 if(gcode
->subcode
== 1) {
689 // ABC axis only handle actuator max speeds
690 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
691 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
692 int c
= 'A' + i
- A_AXIS
;
693 if(gcode
->has_letter(c
)) {
694 float v
= gcode
->get_value(c
);
695 actuators
[i
]->set_max_rate(v
);
701 // this format is deprecated
702 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
703 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
704 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
705 if (gcode
->has_letter('A' + i
)) {
706 float v
= gcode
->get_value('A'+i
);
707 actuators
[i
]->set_max_rate(v
);
712 if(gcode
->subcode
== 1) check_max_actuator_speeds();
716 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
717 if (gcode
->has_letter('S')) {
718 float acc
= gcode
->get_value('S'); // mm/s^2
720 if (acc
< 1.0F
) acc
= 1.0F
;
721 this->default_acceleration
= acc
;
723 for (int i
= 0; i
< n_motors
; ++i
) {
724 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
725 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
726 if(gcode
->has_letter(axis
)) {
727 float acc
= gcode
->get_value(axis
); // mm/s^2
729 if (acc
<= 0.0F
) acc
= NAN
;
730 actuators
[i
]->set_acceleration(acc
);
735 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
736 if (gcode
->has_letter('X')) {
737 float jd
= gcode
->get_value('X');
741 THEKERNEL
->planner
->junction_deviation
= jd
;
743 if (gcode
->has_letter('Z')) {
744 float jd
= gcode
->get_value('Z');
745 // enforce minimum, -1 disables it and uses regular junction deviation
748 THEKERNEL
->planner
->z_junction_deviation
= jd
;
750 if (gcode
->has_letter('S')) {
751 float mps
= gcode
->get_value('S');
755 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
759 case 211: // M211 Sn turns soft endstops on/off
760 if(gcode
->has_letter('S')) {
761 soft_endstop_enabled
= gcode
->get_uint('S') == 1;
763 gcode
->stream
->printf("Soft endstops are %s", soft_endstop_enabled
? "Enabled" : "Disabled");
764 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
765 if(isnan(soft_endstop_min
[i
])) {
766 gcode
->stream
->printf(",%c min is disabled", 'X'+i
);
768 if(isnan(soft_endstop_max
[i
])) {
769 gcode
->stream
->printf(",%c max is disabled", 'X'+i
);
772 gcode
->stream
->printf(",%c axis is not homed", 'X'+i
);
775 gcode
->stream
->printf("\n");
779 case 220: // M220 - speed override percentage
780 if (gcode
->has_letter('S')) {
781 float factor
= gcode
->get_value('S');
782 // enforce minimum 10% speed
785 // enforce maximum 10x speed
786 if (factor
> 1000.0F
)
789 seconds_per_minute
= 6000.0F
/ factor
;
791 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
795 case 400: // wait until all moves are done up to this point
796 THEKERNEL
->conveyor
->wait_for_idle();
799 case 500: // M500 saves some volatile settings to config override file
800 case 503: { // M503 just prints the settings
801 gcode
->stream
->printf(";Steps per unit:\nM92 ");
802 for (int i
= 0; i
< n_motors
; ++i
) {
803 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
804 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
805 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
807 gcode
->stream
->printf("\n");
809 // only print if not NAN
810 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
811 for (int i
= 0; i
< n_motors
; ++i
) {
812 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
813 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
814 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
816 gcode
->stream
->printf("\n");
818 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
);
820 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
]);
822 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
823 for (int i
= 0; i
< n_motors
; ++i
) {
824 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
825 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
826 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
828 gcode
->stream
->printf("\n");
830 // get or save any arm solution specific optional values
831 BaseSolution::arm_options_t options
;
832 if(arm_solution
->get_optional(options
) && !options
.empty()) {
833 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
834 for(auto &i
: options
) {
835 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
837 gcode
->stream
->printf("\n");
840 // save wcs_offsets and current_wcs
841 // TODO this may need to be done whenever they change to be compliant
842 gcode
->stream
->printf(";WCS settings\n");
843 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
845 for(auto &i
: wcs_offsets
) {
846 if(i
!= wcs_t(0, 0, 0)) {
848 std::tie(x
, y
, z
) = i
;
849 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
854 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
855 // 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
856 if(g92_offset
!= wcs_t(0, 0, 0)) {
858 std::tie(x
, y
, z
) = g92_offset
;
859 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
865 case 665: { // M665 set optional arm solution variables based on arm solution.
866 // the parameter args could be any letter each arm solution only accepts certain ones
867 BaseSolution::arm_options_t options
= gcode
->get_args();
868 options
.erase('S'); // don't include the S
869 options
.erase('U'); // don't include the U
870 if(options
.size() > 0) {
871 // set the specified options
872 arm_solution
->set_optional(options
);
875 if(arm_solution
->get_optional(options
)) {
876 // foreach optional value
877 for(auto &i
: options
) {
878 // print all current values of supported options
879 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
880 gcode
->add_nl
= true;
884 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
885 this->delta_segments_per_second
= gcode
->get_value('S');
886 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
888 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
889 this->mm_per_line_segment
= gcode
->get_value('U');
890 this->delta_segments_per_second
= 0;
891 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
899 if( motion_mode
!= NONE
) {
900 is_g123
= motion_mode
!= SEEK
;
901 process_move(gcode
, motion_mode
);
907 next_command_is_MCS
= false; // must be on same line as G0 or G1
910 int Robot::get_active_extruder() const
912 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
913 // find first selected extruder
914 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
919 // process a G0/G1/G2/G3
920 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
922 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
923 // get XYZ and one E (which goes to the selected extruder)
924 float param
[4]{NAN
, NAN
, NAN
, NAN
};
926 // process primary axis
927 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
929 if( gcode
->has_letter(letter
) ) {
930 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
934 float offset
[3]{0,0,0};
935 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
936 if( gcode
->has_letter(letter
) ) {
937 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
941 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
942 float target
[n_motors
];
943 memcpy(target
, machine_position
, n_motors
*sizeof(float));
945 if(!next_command_is_MCS
) {
946 if(this->absolute_mode
) {
947 // apply wcs offsets and g92 offset and tool offset
948 if(!isnan(param
[X_AXIS
])) {
949 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
);
952 if(!isnan(param
[Y_AXIS
])) {
953 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
);
956 if(!isnan(param
[Z_AXIS
])) {
957 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
);
961 // they are deltas from the machine_position if specified
962 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
963 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
968 // already in machine coordinates, we do not add tool offset for that
969 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
970 if(!isnan(param
[i
])) target
[i
] = param
[i
];
976 #if MAX_ROBOT_ACTUATORS > 3
977 // process extruder parameters, for active extruder only (only one active extruder at a time)
978 int selected_extruder
= 0;
979 if(gcode
->has_letter('E')) {
980 selected_extruder
= get_active_extruder();
981 param
[E_AXIS
]= gcode
->get_value('E');
984 // do E for the selected extruder
985 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
986 if(this->e_absolute_mode
) {
987 target
[selected_extruder
]= param
[E_AXIS
];
988 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
990 delta_e
= param
[E_AXIS
];
991 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
995 // 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
996 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
997 char letter
= 'A'+i
-A_AXIS
;
998 if(gcode
->has_letter(letter
)) {
999 float p
= gcode
->get_value(letter
);
1000 if(this->absolute_mode
) {
1003 target
[i
]= p
+ machine_position
[i
];
1009 if( gcode
->has_letter('F') ) {
1010 if( motion_mode
== SEEK
)
1011 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
1013 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
1016 // S is modal When specified on a G0/1/2/3 command
1017 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
1021 // Perform any physical actions
1022 switch(motion_mode
) {
1026 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
1030 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
1035 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
1036 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
1041 // set machine_position to the calculated target
1042 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1046 // reset the machine position for all axis. Used for homing.
1047 // after homing we supply the cartesian coordinates that the head is at when homed,
1048 // however for Z this is the compensated machine position (if enabled)
1049 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1050 // this will make the results from M114 and ? consistent after homing.
1051 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1052 void Robot::reset_axis_position(float x
, float y
, float z
)
1054 // set both the same initially
1055 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1056 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1057 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1059 if(compensationTransform
) {
1060 // apply inverse transform to get machine_position
1061 compensationTransform(machine_position
, true);
1064 // now set the actuator positions based on the supplied compensated position
1065 ActuatorCoordinates actuator_pos
;
1066 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1067 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1068 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1071 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1072 void Robot::reset_axis_position(float position
, int axis
)
1074 compensated_machine_position
[axis
] = position
;
1075 if(axis
<= Z_AXIS
) {
1076 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1078 #if MAX_ROBOT_ACTUATORS > 3
1079 }else if(axis
< n_motors
) {
1080 // ABC and/or extruders need to be set as there is no arm solution for them
1081 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1082 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1087 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1088 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1089 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1091 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1092 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1095 // now correct axis positions then recorrect actuator to account for rounding
1096 reset_position_from_current_actuator_position();
1099 // Use FK to find out where actuator is and reset to match
1100 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1101 void Robot::reset_position_from_current_actuator_position()
1103 ActuatorCoordinates actuator_pos
;
1104 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1105 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1106 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1109 // discover machine position from where actuators actually are
1110 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1111 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1113 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1114 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1116 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1117 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1118 // to get everything in perfect sync.
1119 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1120 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1121 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1124 // Handle extruders and/or ABC axis
1125 #if MAX_ROBOT_ACTUATORS > 3
1126 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1127 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1128 float ap
= actuator_pos
[i
];
1129 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
1130 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1131 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1136 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1137 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1138 // all transforms and is what we actually convert to actuator positions
1139 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1141 float deltas
[n_motors
];
1142 float transformed_target
[n_motors
]; // adjust target for bed compensation
1143 float unit_vec
[N_PRIMARY_AXIS
];
1145 // unity transform by default
1146 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1148 // check function pointer and call if set to transform the target to compensate for bed
1149 if(compensationTransform
) {
1150 // some compensation strategies can transform XYZ, some just change Z
1151 compensationTransform(transformed_target
, false);
1154 // check soft endstops only for homed axis that are enabled
1155 if(soft_endstop_enabled
) {
1156 for (int i
= 0; i
<= Z_AXIS
; ++i
) {
1157 if(!is_homed(i
)) continue;
1158 if( (!isnan(soft_endstop_min
[i
]) && transformed_target
[i
] < soft_endstop_min
[i
]) || (!isnan(soft_endstop_max
[i
]) && transformed_target
[i
] > soft_endstop_max
[i
]) ) {
1159 if(soft_endstop_halt
) {
1160 THEKERNEL
->streams
->printf("Soft Endstop %c was exceeded - reset or M999 required\n", i
+'X');
1161 THEKERNEL
->call_event(ON_HALT
, nullptr);
1164 //} else if(soft_endstop_truncate) {
1165 // TODO VERY hard to do need to go back and change the target, and calculate intercept with the edge
1166 // and store all preceding vectors that have on eor more points ourtside of bounds so we can create a propper clip against the boundaries
1170 THEKERNEL
->streams
->printf("WARNING Soft Endstop %c was exceeded - entire move ignored\n", i
+'X');
1179 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1181 // find distance moved by each axis, use transformed target from the current compensated machine position
1182 for (size_t i
= 0; i
< n_motors
; i
++) {
1183 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1184 if(deltas
[i
] == 0) continue;
1185 // at least one non zero delta
1187 if(i
< N_PRIMARY_AXIS
) {
1188 sos
+= powf(deltas
[i
], 2);
1193 if(!move
) return false;
1195 // see if this is a primary axis move or not
1196 bool auxilliary_move
= true;
1197 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1198 if(deltas
[i
] != 0) {
1199 auxilliary_move
= false;
1204 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1205 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1207 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1208 // 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
1209 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1212 if(!auxilliary_move
) {
1213 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1214 // find distance unit vector for primary axis only
1215 unit_vec
[i
] = deltas
[i
] / distance
;
1217 // Do not move faster than the configured cartesian limits for XYZ
1218 if ( max_speeds
[i
] > 0 ) {
1219 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1221 if (axis_speed
> max_speeds
[i
])
1222 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1227 // find actuator position given the machine position, use actual adjusted target
1228 ActuatorCoordinates actuator_pos
;
1229 if(!disable_arm_solution
) {
1230 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1233 // basically the same as cartesian, would be used for special homing situations like for scara
1234 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1235 actuator_pos
[i
] = transformed_target
[i
];
1239 #if MAX_ROBOT_ACTUATORS > 3
1241 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1242 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1243 actuator_pos
[i
]= transformed_target
[i
];
1244 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1245 // NOTE this relies on the fact only one extruder is active at a time
1246 // scale for volumetric or flow rate
1247 // TODO is this correct? scaling the absolute target? what if the scale changes?
1248 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1249 actuator_pos
[i
] *= get_e_scale_fnc();
1251 if(auxilliary_move
) {
1252 // for E only moves we need to use the scaled E to calculate the distance
1253 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1256 if(auxilliary_move
) {
1257 distance
= sqrtf(sos
); // distance in mm of the e move
1258 if(distance
< 0.00001F
) return false;
1262 // use default acceleration to start with
1263 float acceleration
= default_acceleration
;
1265 float isecs
= rate_mm_s
/ distance
;
1267 // check per-actuator speed limits
1268 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1269 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1270 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1272 float actuator_rate
= d
* isecs
;
1273 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1274 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1275 isecs
= rate_mm_s
/ distance
;
1278 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1279 // 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.
1280 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1281 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1282 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1283 float ca
= fabsf((d
/distance
) * acceleration
);
1285 acceleration
*= ( ma
/ ca
);
1291 // Append the block to the planner
1292 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1293 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1294 // this is the new compensated machine position
1295 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1303 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1304 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1306 if(THEKERNEL
->is_halted()) return false;
1308 // catch negative or zero feed rates
1309 if(rate_mm_s
<= 0.0F
) {
1313 // get the absolute target position, default is current machine_position
1314 float target
[n_motors
];
1315 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1317 // add in the deltas to get new target
1318 for (int i
= 0; i
< naxis
; i
++) {
1319 target
[i
] += delta
[i
];
1322 // submit for planning and if moved update machine_position
1323 if(append_milestone(target
, rate_mm_s
)) {
1324 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1331 // Append a move to the queue ( cutting it into segments if needed )
1332 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1334 // catch negative or zero feed rates and return the same error as GRBL does
1335 if(rate_mm_s
<= 0.0F
) {
1336 gcode
->is_error
= true;
1337 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1341 // Find out the distance for this move in XYZ in MCS
1342 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 ));
1344 if(millimeters_of_travel
< 0.00001F
) {
1345 // we have no movement in XYZ, probably E only extrude or retract
1346 return this->append_milestone(target
, rate_mm_s
);
1350 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1351 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1352 We ask Extruder to do all the work but we need to pass in the relevant data.
1353 NOTE we need to do this before we segment the line (for deltas)
1355 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1356 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1357 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1358 rate_mm_s
*= data
[1]; // adjust the feedrate
1362 // 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.
1363 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1364 // 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
1367 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1370 } else if(this->delta_segments_per_second
> 1.0F
) {
1371 // enabled if set to something > 1, it is set to 0.0 by default
1372 // segment based on current speed and requested segments per second
1373 // the faster the travel speed the fewer segments needed
1374 // NOTE rate is mm/sec and we take into account any speed override
1375 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1376 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1377 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1380 if(this->mm_per_line_segment
== 0.0F
) {
1381 segments
= 1; // don't split it up
1383 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1389 // A vector to keep track of the endpoint of each segment
1390 float segment_delta
[n_motors
];
1391 float segment_end
[n_motors
];
1392 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1394 // How far do we move each segment?
1395 for (int i
= 0; i
< n_motors
; i
++)
1396 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1398 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1399 // We always add another point after this loop so we stop at segments-1, ie i < segments
1400 for (int i
= 1; i
< segments
; i
++) {
1401 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1402 for (int i
= 0; i
< n_motors
; i
++)
1403 segment_end
[i
] += segment_delta
[i
];
1405 // Append the end of this segment to the queue
1406 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1411 // Append the end of this full move to the queue
1412 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1414 this->next_command_is_MCS
= false; // always reset this
1420 // Append an arc to the queue ( cutting it into segments as needed )
1421 // TODO does not support any E parameters so cannot be used for 3D printing.
1422 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1424 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1425 // catch negative or zero feed rates and return the same error as GRBL does
1426 if(rate_mm_s
<= 0.0F
) {
1427 gcode
->is_error
= true;
1428 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1433 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1434 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1435 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1436 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1437 float r_axis1
= -offset
[this->plane_axis_1
];
1438 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1439 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1441 // Patch from GRBL Firmware - Christoph Baumann 04072015
1442 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1443 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1444 if (is_clockwise
) { // Correct atan2 output per direction
1445 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1447 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1450 // Find the distance for this gcode
1451 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1453 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1454 if( millimeters_of_travel
< 0.00001F
) {
1458 // limit segments by maximum arc error
1459 float arc_segment
= this->mm_per_arc_segment
;
1460 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1461 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1462 if (this->mm_per_arc_segment
< min_err_segment
) {
1463 arc_segment
= min_err_segment
;
1466 // Figure out how many segments for this gcode
1467 // 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
1468 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1470 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1471 float theta_per_segment
= angular_travel
/ segments
;
1472 float linear_per_segment
= linear_travel
/ segments
;
1474 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1475 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1476 r_T = [cos(phi) -sin(phi);
1477 sin(phi) cos(phi] * r ;
1478 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1479 defined from the circle center to the initial position. Each line segment is formed by successive
1480 vector rotations. This requires only two cos() and sin() computations to form the rotation
1481 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1482 all float numbers are single precision on the Arduino. (True float precision will not have
1483 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1484 tool precision in some cases. Therefore, arc path correction is implemented.
1486 Small angle approximation may be used to reduce computation overhead further. This approximation
1487 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1488 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1489 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1490 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1491 issue for CNC machines with the single precision Arduino calculations.
1492 This approximation also allows mc_arc to immediately insert a line segment into the planner
1493 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1494 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1495 This is important when there are successive arc motions.
1497 // Vector rotation matrix values
1498 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1499 float sin_T
= theta_per_segment
;
1501 // TODO we need to handle the ABC axis here by segmenting them
1502 float arc_target
[3];
1509 // Initialize the linear axis
1510 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1513 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1514 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1516 if (count
< this->arc_correction
) {
1517 // Apply vector rotation matrix
1518 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1519 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1523 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1524 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1525 cos_Ti
= cosf(i
* theta_per_segment
);
1526 sin_Ti
= sinf(i
* theta_per_segment
);
1527 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1528 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1532 // Update arc_target location
1533 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1534 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1535 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1537 // Append this segment to the queue
1538 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1542 // Ensure last segment arrives at target location.
1543 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1548 // Do the math for an arc and add it to the queue
1549 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1553 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1555 // Set clockwise/counter-clockwise sign for mc_arc computations
1556 bool is_clockwise
= false;
1557 if( motion_mode
== CW_ARC
) {
1558 is_clockwise
= true;
1562 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1566 float Robot::theta(float x
, float y
)
1568 float t
= atanf(x
/ fabs(y
));
1580 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1582 this->plane_axis_0
= axis_0
;
1583 this->plane_axis_1
= axis_1
;
1584 this->plane_axis_2
= axis_2
;
1587 void Robot::clearToolOffset()
1589 this->tool_offset
= wcs_t(0,0,0);
1592 void Robot::setToolOffset(const float offset
[3])
1594 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1597 float Robot::get_feed_rate() const
1599 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;
1602 bool Robot::is_homed(uint8_t i
) const
1604 if(i
>= 3) return false; // safety
1606 // if we are homing we ignore soft endstops so return false
1608 bool ok
= PublicData::get_value(endstops_checksum
, get_homing_status_checksum
, 0, &homing
);
1609 if(!ok
|| homing
) return false;
1611 // check individual axis homing status
1613 ok
= PublicData::get_value(endstops_checksum
, get_homed_status_checksum
, 0, homed
);
1614 if(!ok
) return false;