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() || !pins
[2].connected()) {
223 if(a
<= Z_AXIS
) THEKERNEL
->streams
->printf("FATAL: motor %d is not defined in config\n", 'X'+a
);
224 break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
227 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
228 // register this motor (NB This must be 0,1,2) of the actuators array
229 uint8_t n
= register_motor(sm
);
231 // this is a fatal error
232 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
236 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
237 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
238 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
241 check_max_actuator_speeds(); // check the configs are sane
243 // if we have not specified a z acceleration see if the legacy config was set
244 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
245 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
247 actuators
[Z_AXIS
]->set_acceleration(acc
);
251 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
252 // so the first move can be correct if homing is not performed
253 ActuatorCoordinates actuator_pos
;
254 arm_solution
->cartesian_to_actuator(machine_position
, actuator_pos
);
255 for (size_t i
= 0; i
< n_motors
; i
++)
256 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
258 //this->clearToolOffset();
261 uint8_t Robot::register_motor(StepperMotor
*motor
)
263 // register this motor with the step ticker
264 THEKERNEL
->step_ticker
->register_motor(motor
);
265 if(n_motors
>= k_max_actuators
) {
266 // this is a fatal error
267 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
270 actuators
.push_back(motor
);
271 motor
->set_motor_id(n_motors
);
275 void Robot::push_state()
277 bool am
= this->absolute_mode
;
278 bool em
= this->e_absolute_mode
;
279 bool im
= this->inch_mode
;
280 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
284 void Robot::pop_state()
286 if(!state_stack
.empty()) {
287 auto s
= state_stack
.top();
289 this->feed_rate
= std::get
<0>(s
);
290 this->seek_rate
= std::get
<1>(s
);
291 this->absolute_mode
= std::get
<2>(s
);
292 this->e_absolute_mode
= std::get
<3>(s
);
293 this->inch_mode
= std::get
<4>(s
);
294 this->current_wcs
= std::get
<5>(s
);
298 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
300 std::vector
<wcs_t
> v
;
301 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
302 for(auto& i
: wcs_offsets
) {
305 v
.push_back(g92_offset
);
306 v
.push_back(tool_offset
);
310 void Robot::get_current_machine_position(float *pos
) const
312 // get real time current actuator position in mm
313 ActuatorCoordinates current_position
{
314 actuators
[X_AXIS
]->get_current_position(),
315 actuators
[Y_AXIS
]->get_current_position(),
316 actuators
[Z_AXIS
]->get_current_position()
319 // get machine position from the actuator position using FK
320 arm_solution
->actuator_to_cartesian(current_position
, pos
);
323 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
325 // M114.1 is a new way to do this (similar to how GRBL does it).
326 // it returns the realtime position based on the current step position of the actuators.
327 // this does require a FK to get a machine position from the actuator position
328 // and then invert all the transforms to get a workspace position from machine position
329 // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
331 if(subcode
== 0) { // M114 print WCS
332 wcs_t pos
= mcs2wcs(machine_position
);
333 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
)));
335 } else if(subcode
== 4) {
336 // M114.4 print last milestone
337 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
]);
339 } else if(subcode
== 5) {
340 // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
341 // will differ from LMS by the compensation at the current position otherwise
342 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
]);
345 // get real time positions
347 get_current_machine_position(mpos
);
349 // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
350 if(compensationTransform
) compensationTransform(mpos
, true); // get inverse compensation transform
352 if(subcode
== 1) { // M114.1 print realtime WCS
353 wcs_t pos
= mcs2wcs(mpos
);
354 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
)));
356 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
357 n
= snprintf(buf
, bufsize
, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
359 } else if(subcode
== 3) { // M114.3 print realtime actuator position
360 // get real time current actuator position in mm
361 ActuatorCoordinates current_position
{
362 actuators
[X_AXIS
]->get_current_position(),
363 actuators
[Y_AXIS
]->get_current_position(),
364 actuators
[Z_AXIS
]->get_current_position()
366 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
]);
370 #if MAX_ROBOT_ACTUATORS > 3
371 // deal with the ABC axis
372 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
373 if(actuators
[i
]->is_extruder()) continue; // don't show an extruder as that will be E
374 if(subcode
== 4) { // M114.4 print last milestone
375 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, machine_position
[i
]);
377 }else if(subcode
== 2 || subcode
== 3) { // M114.2/M114.3 print actuator position which is the same as machine position for ABC
378 // current actuator position
379 n
+= snprintf(&buf
[n
], bufsize
-n
, " %c:%1.4f", 'A'+i
-A_AXIS
, actuators
[i
]->get_current_position());
387 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
388 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
390 return std::make_tuple(
391 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
),
392 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
),
393 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
)
397 // this does a sanity check that actuator speeds do not exceed steps rate capability
398 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
399 void Robot::check_max_actuator_speeds()
401 for (size_t i
= 0; i
< n_motors
; i
++) {
402 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
403 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
404 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
405 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());
410 //A GCode has been received
411 //See if the current Gcode line has some orders for us
412 void Robot::on_gcode_received(void *argument
)
414 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
416 enum MOTION_MODE_T motion_mode
= NONE
;
420 case 0: motion_mode
= SEEK
; break;
421 case 1: motion_mode
= LINEAR
; break;
422 case 2: motion_mode
= CW_ARC
; break;
423 case 3: motion_mode
= CCW_ARC
; break;
424 case 4: { // G4 Dwell
425 uint32_t delay_ms
= 0;
426 if (gcode
->has_letter('P')) {
427 if(THEKERNEL
->is_grbl_mode()) {
428 // in grbl mode (and linuxcnc) P is decimal seconds
429 float f
= gcode
->get_value('P');
430 delay_ms
= f
* 1000.0F
;
433 // in reprap P is milliseconds, they always have to be different!
434 delay_ms
= gcode
->get_int('P');
437 if (gcode
->has_letter('S')) {
438 delay_ms
+= gcode
->get_int('S') * 1000;
442 THEKERNEL
->conveyor
->wait_for_idle();
443 // wait for specified time
444 uint32_t start
= us_ticker_read(); // mbed call
445 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
446 THEKERNEL
->call_event(ON_IDLE
, this);
447 if(THEKERNEL
->is_halted()) return;
453 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
454 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
455 size_t n
= gcode
->get_uint('P');
456 if(n
== 0) n
= current_wcs
; // set current coordinate system
460 std::tie(x
, y
, z
) = wcs_offsets
[n
];
461 if(gcode
->get_int('L') == 20) {
462 // this makes the current machine position (less compensation transform) the offset
463 // get current position in WCS
464 wcs_t pos
= mcs2wcs(machine_position
);
466 if(gcode
->has_letter('X')){
467 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
470 if(gcode
->has_letter('Y')){
471 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
473 if(gcode
->has_letter('Z')) {
474 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
479 // the value is the offset from machine zero
480 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
481 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
482 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
484 if(gcode
->has_letter('X')) x
+= to_millimeters(gcode
->get_value('X'));
485 if(gcode
->has_letter('Y')) y
+= to_millimeters(gcode
->get_value('Y'));
486 if(gcode
->has_letter('Z')) z
+= to_millimeters(gcode
->get_value('Z'));
489 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
494 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
495 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
496 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
497 case 20: this->inch_mode
= true; break;
498 case 21: this->inch_mode
= false; break;
500 case 54: case 55: case 56: case 57: case 58: case 59:
501 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
502 current_wcs
= gcode
->g
- 54;
503 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
504 current_wcs
+= gcode
->subcode
;
505 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
509 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
510 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
513 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
514 // reset G92 offsets to 0
515 g92_offset
= wcs_t(0, 0, 0);
517 } else if(gcode
->subcode
== 3) {
518 // initialize G92 to the specified values, only used for saving it with M500
519 float x
= 0, y
= 0, z
= 0;
520 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
521 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
522 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
523 g92_offset
= wcs_t(x
, y
, z
);
526 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
528 std::tie(x
, y
, z
) = g92_offset
;
529 // get current position in WCS
530 wcs_t pos
= mcs2wcs(machine_position
);
532 // adjust g92 offset to make the current wpos == the value requested
533 if(gcode
->has_letter('X')){
534 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
536 if(gcode
->has_letter('Y')){
537 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
539 if(gcode
->has_letter('Z')) {
540 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
542 g92_offset
= wcs_t(x
, y
, z
);
545 #if MAX_ROBOT_ACTUATORS > 3
546 if(gcode
->subcode
== 0 && (gcode
->has_letter('E') || gcode
->get_num_args() == 0)){
547 // reset the E position, legacy for 3d Printers to be reprap compatible
548 // find the selected extruder
549 int selected_extruder
= get_active_extruder();
550 if(selected_extruder
> 0) {
551 float e
= gcode
->has_letter('E') ? gcode
->get_value('E') : 0;
552 machine_position
[selected_extruder
]= compensated_machine_position
[selected_extruder
]= e
;
553 actuators
[selected_extruder
]->change_last_milestone(get_e_scale_fnc
? e
*get_e_scale_fnc() : e
);
562 } else if( gcode
->has_m
) {
564 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
565 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
568 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
569 if(!THEKERNEL
->is_grbl_mode()) break;
570 // fall through to M2
571 case 2: // M2 end of program
573 absolute_mode
= true;
576 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
579 case 18: // this allows individual motors to be turned off, no parameters falls through to turn all off
580 if(gcode
->get_num_args() > 0) {
581 // bitmap of motors to turn off, where bit 1:X, 2:Y, 3:Z, 4:A, 5:B, 6:C
583 for (int i
= 0; i
< n_motors
; ++i
) {
584 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-3));
585 if(gcode
->has_letter(axis
)) bm
|= (0x02<<i
); // set appropriate bit
587 // handle E parameter as currently selected extruder ABC
588 if(gcode
->has_letter('E')) {
589 // find first selected extruder
590 int i
= get_active_extruder();
592 bm
|= (0x02<<i
); // set appropriate bit
596 THEKERNEL
->conveyor
->wait_for_idle();
597 THEKERNEL
->call_event(ON_ENABLE
, (void *)bm
);
602 THEKERNEL
->conveyor
->wait_for_idle();
603 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
606 case 82: e_absolute_mode
= true; break;
607 case 83: e_absolute_mode
= false; break;
609 case 92: // M92 - set steps per mm
610 for (int i
= 0; i
< n_motors
; ++i
) {
611 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
612 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
613 if(gcode
->has_letter(axis
)) {
614 actuators
[i
]->change_steps_per_mm(this->to_millimeters(gcode
->get_value(axis
)));
616 gcode
->stream
->printf("%c:%f ", axis
, actuators
[i
]->get_steps_per_mm());
618 gcode
->add_nl
= true;
619 check_max_actuator_speeds();
624 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
625 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
629 case 120: // push state
633 case 121: // pop state
637 case 203: // M203 Set maximum feedrates in mm/sec, M203.1 set maximum actuator feedrates
638 if(gcode
->get_num_args() == 0) {
639 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
640 gcode
->stream
->printf(" %c: %g ", 'X' + i
, gcode
->subcode
== 0 ? this->max_speeds
[i
] : actuators
[i
]->get_max_rate());
642 if(gcode
->subcode
== 1) {
643 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
644 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
645 gcode
->stream
->printf(" %c: %g ", 'A' + i
- A_AXIS
, actuators
[i
]->get_max_rate());
649 gcode
->add_nl
= true;
652 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
653 if (gcode
->has_letter('X' + i
)) {
654 float v
= gcode
->get_value('X'+i
);
655 if(gcode
->subcode
== 0) this->max_speeds
[i
]= v
;
656 else if(gcode
->subcode
== 1) actuators
[i
]->set_max_rate(v
);
660 if(gcode
->subcode
== 1) {
661 // ABC axis only handle actuator max speeds
662 for (size_t i
= A_AXIS
; i
< n_motors
; i
++) {
663 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
664 int c
= 'A' + i
- A_AXIS
;
665 if(gcode
->has_letter(c
)) {
666 float v
= gcode
->get_value(c
);
667 actuators
[i
]->set_max_rate(v
);
673 // this format is deprecated
674 if(gcode
->subcode
== 0 && (gcode
->has_letter('A') || gcode
->has_letter('B') || gcode
->has_letter('C'))) {
675 gcode
->stream
->printf("NOTE this format is deprecated, Use M203.1 instead\n");
676 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
677 if (gcode
->has_letter('A' + i
)) {
678 float v
= gcode
->get_value('A'+i
);
679 actuators
[i
]->set_max_rate(v
);
684 if(gcode
->subcode
== 1) check_max_actuator_speeds();
688 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
689 if (gcode
->has_letter('S')) {
690 float acc
= gcode
->get_value('S'); // mm/s^2
692 if (acc
< 1.0F
) acc
= 1.0F
;
693 this->default_acceleration
= acc
;
695 for (int i
= 0; i
< n_motors
; ++i
) {
696 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
697 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
698 if(gcode
->has_letter(axis
)) {
699 float acc
= gcode
->get_value(axis
); // mm/s^2
701 if (acc
<= 0.0F
) acc
= NAN
;
702 actuators
[i
]->set_acceleration(acc
);
707 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed
708 if (gcode
->has_letter('X')) {
709 float jd
= gcode
->get_value('X');
713 THEKERNEL
->planner
->junction_deviation
= jd
;
715 if (gcode
->has_letter('Z')) {
716 float jd
= gcode
->get_value('Z');
717 // enforce minimum, -1 disables it and uses regular junction deviation
720 THEKERNEL
->planner
->z_junction_deviation
= jd
;
722 if (gcode
->has_letter('S')) {
723 float mps
= gcode
->get_value('S');
727 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
731 case 220: // M220 - speed override percentage
732 if (gcode
->has_letter('S')) {
733 float factor
= gcode
->get_value('S');
734 // enforce minimum 10% speed
737 // enforce maximum 10x speed
738 if (factor
> 1000.0F
)
741 seconds_per_minute
= 6000.0F
/ factor
;
743 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
747 case 400: // wait until all moves are done up to this point
748 THEKERNEL
->conveyor
->wait_for_idle();
751 case 500: // M500 saves some volatile settings to config override file
752 case 503: { // M503 just prints the settings
753 gcode
->stream
->printf(";Steps per unit:\nM92 ");
754 for (int i
= 0; i
< n_motors
; ++i
) {
755 if(actuators
[i
]->is_extruder()) continue; //extruders handle this themselves
756 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
757 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_steps_per_mm());
759 gcode
->stream
->printf("\n");
761 // only print if not NAN
762 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f ", default_acceleration
);
763 for (int i
= 0; i
< n_motors
; ++i
) {
764 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
765 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
766 if(!isnan(actuators
[i
]->get_acceleration())) gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_acceleration());
768 gcode
->stream
->printf("\n");
770 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
);
772 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
]);
774 gcode
->stream
->printf(";Max actuator feedrates in mm/sec:\nM203.1 ");
775 for (int i
= 0; i
< n_motors
; ++i
) {
776 if(actuators
[i
]->is_extruder()) continue; // extruders handle this themselves
777 char axis
= (i
<= Z_AXIS
? 'X'+i
: 'A'+(i
-A_AXIS
));
778 gcode
->stream
->printf("%c%1.5f ", axis
, actuators
[i
]->get_max_rate());
780 gcode
->stream
->printf("\n");
782 // get or save any arm solution specific optional values
783 BaseSolution::arm_options_t options
;
784 if(arm_solution
->get_optional(options
) && !options
.empty()) {
785 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
786 for(auto &i
: options
) {
787 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
789 gcode
->stream
->printf("\n");
792 // save wcs_offsets and current_wcs
793 // TODO this may need to be done whenever they change to be compliant
794 gcode
->stream
->printf(";WCS settings\n");
795 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
797 for(auto &i
: wcs_offsets
) {
798 if(i
!= wcs_t(0, 0, 0)) {
800 std::tie(x
, y
, z
) = i
;
801 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
806 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
807 // 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
808 if(g92_offset
!= wcs_t(0, 0, 0)) {
810 std::tie(x
, y
, z
) = g92_offset
;
811 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
817 case 665: { // M665 set optional arm solution variables based on arm solution.
818 // the parameter args could be any letter each arm solution only accepts certain ones
819 BaseSolution::arm_options_t options
= gcode
->get_args();
820 options
.erase('S'); // don't include the S
821 options
.erase('U'); // don't include the U
822 if(options
.size() > 0) {
823 // set the specified options
824 arm_solution
->set_optional(options
);
827 if(arm_solution
->get_optional(options
)) {
828 // foreach optional value
829 for(auto &i
: options
) {
830 // print all current values of supported options
831 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
832 gcode
->add_nl
= true;
836 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
837 this->delta_segments_per_second
= gcode
->get_value('S');
838 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
840 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
841 this->mm_per_line_segment
= gcode
->get_value('U');
842 this->delta_segments_per_second
= 0;
843 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
851 if( motion_mode
!= NONE
) {
852 is_g123
= motion_mode
!= SEEK
;
853 process_move(gcode
, motion_mode
);
859 next_command_is_MCS
= false; // must be on same line as G0 or G1
862 int Robot::get_active_extruder() const
864 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
865 // find first selected extruder
866 if(actuators
[i
]->is_extruder() && actuators
[i
]->is_selected()) return i
;
871 // process a G0/G1/G2/G3
872 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
874 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
875 // get XYZ and one E (which goes to the selected extruder)
876 float param
[4]{NAN
, NAN
, NAN
, NAN
};
878 // process primary axis
879 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
881 if( gcode
->has_letter(letter
) ) {
882 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
886 float offset
[3]{0,0,0};
887 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
888 if( gcode
->has_letter(letter
) ) {
889 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
893 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
894 float target
[n_motors
];
895 memcpy(target
, machine_position
, n_motors
*sizeof(float));
897 if(!next_command_is_MCS
) {
898 if(this->absolute_mode
) {
899 // apply wcs offsets and g92 offset and tool offset
900 if(!isnan(param
[X_AXIS
])) {
901 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
);
904 if(!isnan(param
[Y_AXIS
])) {
905 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
);
908 if(!isnan(param
[Z_AXIS
])) {
909 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
);
913 // they are deltas from the machine_position if specified
914 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
915 if(!isnan(param
[i
])) target
[i
] = param
[i
] + machine_position
[i
];
920 // already in machine coordinates, we do not add tool offset for that
921 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
922 if(!isnan(param
[i
])) target
[i
] = param
[i
];
928 #if MAX_ROBOT_ACTUATORS > 3
929 // process extruder parameters, for active extruder only (only one active extruder at a time)
930 int selected_extruder
= 0;
931 if(gcode
->has_letter('E')) {
932 selected_extruder
= get_active_extruder();
933 param
[E_AXIS
]= gcode
->get_value('E');
936 // do E for the selected extruder
937 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
938 if(this->e_absolute_mode
) {
939 target
[selected_extruder
]= param
[E_AXIS
];
940 delta_e
= target
[selected_extruder
] - machine_position
[selected_extruder
];
942 delta_e
= param
[E_AXIS
];
943 target
[selected_extruder
] = delta_e
+ machine_position
[selected_extruder
];
947 // 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
948 for (int i
= A_AXIS
; i
< n_motors
; ++i
) {
949 char letter
= 'A'+i
-A_AXIS
;
950 if(gcode
->has_letter(letter
)) {
951 float p
= gcode
->get_value(letter
);
952 if(this->absolute_mode
) {
955 target
[i
]= p
+ machine_position
[i
];
961 if( gcode
->has_letter('F') ) {
962 if( motion_mode
== SEEK
)
963 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
965 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
968 // S is modal When specified on a G0/1/2/3 command
969 if(gcode
->has_letter('S')) s_value
= gcode
->get_value('S');
973 // Perform any physical actions
974 switch(motion_mode
) {
978 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
982 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
987 // Note arcs are not currently supported by extruder based machines, as 3D slicers do not use arcs (G2/G3)
988 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
993 // set machine_position to the calculated target
994 memcpy(machine_position
, target
, n_motors
*sizeof(float));
998 // reset the machine position for all axis. Used for homing.
999 // after homing we supply the cartesian coordinates that the head is at when homed,
1000 // however for Z this is the compensated machine position (if enabled)
1001 // So we need to apply the inverse compensation transform to the supplied coordinates to get the correct machine position
1002 // this will make the results from M114 and ? consistent after homing.
1003 // This works for cases where the Z endstop is fixed on the Z actuator and is the same regardless of where XY are.
1004 void Robot::reset_axis_position(float x
, float y
, float z
)
1006 // set both the same initially
1007 compensated_machine_position
[X_AXIS
]= machine_position
[X_AXIS
] = x
;
1008 compensated_machine_position
[Y_AXIS
]= machine_position
[Y_AXIS
] = y
;
1009 compensated_machine_position
[Z_AXIS
]= machine_position
[Z_AXIS
] = z
;
1011 if(compensationTransform
) {
1012 // apply inverse transform to get machine_position
1013 compensationTransform(machine_position
, true);
1016 // now set the actuator positions based on the supplied compensated position
1017 ActuatorCoordinates actuator_pos
;
1018 arm_solution
->cartesian_to_actuator(this->compensated_machine_position
, actuator_pos
);
1019 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
1020 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1023 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
1024 void Robot::reset_axis_position(float position
, int axis
)
1026 compensated_machine_position
[axis
] = position
;
1027 if(axis
<= Z_AXIS
) {
1028 reset_axis_position(compensated_machine_position
[X_AXIS
], compensated_machine_position
[Y_AXIS
], compensated_machine_position
[Z_AXIS
]);
1030 #if MAX_ROBOT_ACTUATORS > 3
1031 }else if(axis
< n_motors
) {
1032 // ABC and/or extruders need to be set as there is no arm solution for them
1033 machine_position
[axis
]= compensated_machine_position
[axis
]= position
;
1034 actuators
[axis
]->change_last_milestone(machine_position
[axis
]);
1039 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
1040 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
1041 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
1043 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1044 if(!isnan(ac
[i
])) actuators
[i
]->change_last_milestone(ac
[i
]);
1047 // now correct axis positions then recorrect actuator to account for rounding
1048 reset_position_from_current_actuator_position();
1051 // Use FK to find out where actuator is and reset to match
1052 // TODO maybe we should only reset axis that are being homed unless this is due to a ON_HALT
1053 void Robot::reset_position_from_current_actuator_position()
1055 ActuatorCoordinates actuator_pos
;
1056 for (size_t i
= X_AXIS
; i
< n_motors
; i
++) {
1057 // NOTE actuator::current_position is curently NOT the same as actuator::machine_position after an abrupt abort
1058 actuator_pos
[i
] = actuators
[i
]->get_current_position();
1061 // discover machine position from where actuators actually are
1062 arm_solution
->actuator_to_cartesian(actuator_pos
, compensated_machine_position
);
1063 memcpy(machine_position
, compensated_machine_position
, sizeof machine_position
);
1065 // compensated_machine_position includes the compensation transform so we need to get the inverse to get actual machine_position
1066 if(compensationTransform
) compensationTransform(machine_position
, true); // get inverse compensation transform
1068 // now reset actuator::machine_position, NOTE this may lose a little precision as FK is not always entirely accurate.
1069 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
1070 // to get everything in perfect sync.
1071 arm_solution
->cartesian_to_actuator(compensated_machine_position
, actuator_pos
);
1072 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1073 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
1076 // Handle extruders and/or ABC axis
1077 #if MAX_ROBOT_ACTUATORS > 3
1078 for (int i
= A_AXIS
; i
< n_motors
; i
++) {
1079 // ABC and/or extruders just need to set machine_position and compensated_machine_position
1080 float ap
= actuator_pos
[i
];
1081 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
1082 machine_position
[i
]= compensated_machine_position
[i
]= ap
;
1083 actuators
[i
]->change_last_milestone(actuator_pos
[i
]); // this updates the last_milestone in the actuator
1088 // Convert target (in machine coordinates) to machine_position, then convert to actuator position and append this to the planner
1089 // target is in machine coordinates without the compensation transform, however we save a compensated_machine_position that includes
1090 // all transforms and is what we actually convert to actuator positions
1091 bool Robot::append_milestone(const float target
[], float rate_mm_s
)
1093 float deltas
[n_motors
];
1094 float transformed_target
[n_motors
]; // adjust target for bed compensation
1095 float unit_vec
[N_PRIMARY_AXIS
];
1097 // unity transform by default
1098 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
1100 // check function pointer and call if set to transform the target to compensate for bed
1101 if(compensationTransform
) {
1102 // some compensation strategies can transform XYZ, some just change Z
1103 compensationTransform(transformed_target
, false);
1107 float sos
= 0; // sum of squares for just primary axis (XYZ usually)
1109 // find distance moved by each axis, use transformed target from the current compensated machine position
1110 for (size_t i
= 0; i
< n_motors
; i
++) {
1111 deltas
[i
] = transformed_target
[i
] - compensated_machine_position
[i
];
1112 if(deltas
[i
] == 0) continue;
1113 // at least one non zero delta
1115 if(i
< N_PRIMARY_AXIS
) {
1116 sos
+= powf(deltas
[i
], 2);
1121 if(!move
) return false;
1123 // see if this is a primary axis move or not
1124 bool auxilliary_move
= true;
1125 for (int i
= 0; i
< N_PRIMARY_AXIS
; ++i
) {
1126 if(deltas
[i
] != 0) {
1127 auxilliary_move
= false;
1132 // total movement, use XYZ if a primary axis otherwise we calculate distance for E after scaling to mm
1133 float distance
= auxilliary_move
? 0 : sqrtf(sos
);
1135 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1136 // 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
1137 if(!auxilliary_move
&& distance
< 0.00001F
) return false;
1140 if(!auxilliary_move
) {
1141 for (size_t i
= X_AXIS
; i
< N_PRIMARY_AXIS
; i
++) {
1142 // find distance unit vector for primary axis only
1143 unit_vec
[i
] = deltas
[i
] / distance
;
1145 // Do not move faster than the configured cartesian limits for XYZ
1146 if ( max_speeds
[i
] > 0 ) {
1147 float axis_speed
= fabsf(unit_vec
[i
] * rate_mm_s
);
1149 if (axis_speed
> max_speeds
[i
])
1150 rate_mm_s
*= ( max_speeds
[i
] / axis_speed
);
1155 // find actuator position given the machine position, use actual adjusted target
1156 ActuatorCoordinates actuator_pos
;
1157 if(!disable_arm_solution
) {
1158 arm_solution
->cartesian_to_actuator( transformed_target
, actuator_pos
);
1161 // basically the same as cartesian, would be used for special homing situations like for scara
1162 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
1163 actuator_pos
[i
] = transformed_target
[i
];
1167 #if MAX_ROBOT_ACTUATORS > 3
1169 // for the extruders just copy the position, and possibly scale it from mm³ to mm
1170 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
1171 actuator_pos
[i
]= transformed_target
[i
];
1172 if(actuators
[i
]->is_extruder() && get_e_scale_fnc
) {
1173 // NOTE this relies on the fact only one extruder is active at a time
1174 // scale for volumetric or flow rate
1175 // TODO is this correct? scaling the absolute target? what if the scale changes?
1176 // for volumetric it basically converts mm³ to mm, but what about flow rate?
1177 actuator_pos
[i
] *= get_e_scale_fnc();
1179 if(auxilliary_move
) {
1180 // for E only moves we need to use the scaled E to calculate the distance
1181 sos
+= powf(actuator_pos
[i
] - actuators
[i
]->get_last_milestone(), 2);
1184 if(auxilliary_move
) {
1185 distance
= sqrtf(sos
); // distance in mm of the e move
1186 if(distance
< 0.00001F
) return false;
1190 // use default acceleration to start with
1191 float acceleration
= default_acceleration
;
1193 float isecs
= rate_mm_s
/ distance
;
1195 // check per-actuator speed limits
1196 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1197 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1198 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
1200 float actuator_rate
= d
* isecs
;
1201 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1202 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1203 isecs
= rate_mm_s
/ distance
;
1206 // adjust acceleration to lowest found, for now just primary axis unless it is an auxiliary move
1207 // 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.
1208 if(auxilliary_move
|| actuator
< N_PRIMARY_AXIS
) {
1209 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1210 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1211 float ca
= fabsf((d
/distance
) * acceleration
);
1213 acceleration
*= ( ma
/ ca
);
1219 // Append the block to the planner
1220 // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
1221 if(THEKERNEL
->planner
->append_block( actuator_pos
, n_motors
, rate_mm_s
, distance
, auxilliary_move
? nullptr : unit_vec
, acceleration
, s_value
, is_g123
)) {
1222 // this is the new compensated machine position
1223 memcpy(this->compensated_machine_position
, transformed_target
, n_motors
*sizeof(float));
1231 // Used to plan a single move used by things like endstops when homing, zprobe, extruder firmware retracts etc.
1232 bool Robot::delta_move(const float *delta
, float rate_mm_s
, uint8_t naxis
)
1234 if(THEKERNEL
->is_halted()) return false;
1236 // catch negative or zero feed rates
1237 if(rate_mm_s
<= 0.0F
) {
1241 // get the absolute target position, default is current machine_position
1242 float target
[n_motors
];
1243 memcpy(target
, machine_position
, n_motors
*sizeof(float));
1245 // add in the deltas to get new target
1246 for (int i
= 0; i
< naxis
; i
++) {
1247 target
[i
] += delta
[i
];
1250 // submit for planning and if moved update machine_position
1251 if(append_milestone(target
, rate_mm_s
)) {
1252 memcpy(machine_position
, target
, n_motors
*sizeof(float));
1259 // Append a move to the queue ( cutting it into segments if needed )
1260 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1262 // catch negative or zero feed rates and return the same error as GRBL does
1263 if(rate_mm_s
<= 0.0F
) {
1264 gcode
->is_error
= true;
1265 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1269 // Find out the distance for this move in XYZ in MCS
1270 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 ));
1272 if(millimeters_of_travel
< 0.00001F
) {
1273 // we have no movement in XYZ, probably E only extrude or retract
1274 return this->append_milestone(target
, rate_mm_s
);
1278 For extruders, we need to do some extra work to limit the volumetric rate if specified...
1279 If using volumetric limts we need to be using volumetric extrusion for this to work as Ennn needs to be in mm³ not mm
1280 We ask Extruder to do all the work but we need to pass in the relevant data.
1281 NOTE we need to do this before we segment the line (for deltas)
1283 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1284 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1285 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1286 rate_mm_s
*= data
[1]; // adjust the feedrate
1290 // 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.
1291 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1292 // 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
1295 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1298 } else if(this->delta_segments_per_second
> 1.0F
) {
1299 // enabled if set to something > 1, it is set to 0.0 by default
1300 // segment based on current speed and requested segments per second
1301 // the faster the travel speed the fewer segments needed
1302 // NOTE rate is mm/sec and we take into account any speed override
1303 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1304 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1305 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1308 if(this->mm_per_line_segment
== 0.0F
) {
1309 segments
= 1; // don't split it up
1311 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1317 // A vector to keep track of the endpoint of each segment
1318 float segment_delta
[n_motors
];
1319 float segment_end
[n_motors
];
1320 memcpy(segment_end
, machine_position
, n_motors
*sizeof(float));
1322 // How far do we move each segment?
1323 for (int i
= 0; i
< n_motors
; i
++)
1324 segment_delta
[i
] = (target
[i
] - machine_position
[i
]) / segments
;
1326 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1327 // We always add another point after this loop so we stop at segments-1, ie i < segments
1328 for (int i
= 1; i
< segments
; i
++) {
1329 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1330 for (int i
= 0; i
< n_motors
; i
++)
1331 segment_end
[i
] += segment_delta
[i
];
1333 // Append the end of this segment to the queue
1334 bool b
= this->append_milestone(segment_end
, rate_mm_s
);
1339 // Append the end of this full move to the queue
1340 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1342 this->next_command_is_MCS
= false; // always reset this
1348 // Append an arc to the queue ( cutting it into segments as needed )
1349 // TODO does not support any E parameters so cannot be used for 3D printing.
1350 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1352 float rate_mm_s
= this->feed_rate
/ seconds_per_minute
;
1353 // catch negative or zero feed rates and return the same error as GRBL does
1354 if(rate_mm_s
<= 0.0F
) {
1355 gcode
->is_error
= true;
1356 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
1361 float center_axis0
= this->machine_position
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1362 float center_axis1
= this->machine_position
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1363 float linear_travel
= target
[this->plane_axis_2
] - this->machine_position
[this->plane_axis_2
];
1364 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1365 float r_axis1
= -offset
[this->plane_axis_1
];
1366 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1367 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1369 // Patch from GRBL Firmware - Christoph Baumann 04072015
1370 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1371 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1372 if (is_clockwise
) { // Correct atan2 output per direction
1373 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1375 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1378 // Find the distance for this gcode
1379 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1381 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1382 if( millimeters_of_travel
< 0.00001F
) {
1386 // limit segments by maximum arc error
1387 float arc_segment
= this->mm_per_arc_segment
;
1388 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1389 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1390 if (this->mm_per_arc_segment
< min_err_segment
) {
1391 arc_segment
= min_err_segment
;
1394 // Figure out how many segments for this gcode
1395 // 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
1396 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1398 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1399 float theta_per_segment
= angular_travel
/ segments
;
1400 float linear_per_segment
= linear_travel
/ segments
;
1402 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1403 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1404 r_T = [cos(phi) -sin(phi);
1405 sin(phi) cos(phi] * r ;
1406 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1407 defined from the circle center to the initial position. Each line segment is formed by successive
1408 vector rotations. This requires only two cos() and sin() computations to form the rotation
1409 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1410 all float numbers are single precision on the Arduino. (True float precision will not have
1411 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1412 tool precision in some cases. Therefore, arc path correction is implemented.
1414 Small angle approximation may be used to reduce computation overhead further. This approximation
1415 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1416 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1417 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1418 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1419 issue for CNC machines with the single precision Arduino calculations.
1420 This approximation also allows mc_arc to immediately insert a line segment into the planner
1421 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1422 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1423 This is important when there are successive arc motions.
1425 // Vector rotation matrix values
1426 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1427 float sin_T
= theta_per_segment
;
1429 // TODO we need to handle the ABC axis here by segmenting them
1430 float arc_target
[3];
1437 // Initialize the linear axis
1438 arc_target
[this->plane_axis_2
] = this->machine_position
[this->plane_axis_2
];
1441 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1442 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1444 if (count
< this->arc_correction
) {
1445 // Apply vector rotation matrix
1446 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1447 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1451 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1452 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1453 cos_Ti
= cosf(i
* theta_per_segment
);
1454 sin_Ti
= sinf(i
* theta_per_segment
);
1455 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1456 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1460 // Update arc_target location
1461 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1462 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1463 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1465 // Append this segment to the queue
1466 bool b
= this->append_milestone(arc_target
, rate_mm_s
);
1470 // Ensure last segment arrives at target location.
1471 if(this->append_milestone(target
, rate_mm_s
)) moved
= true;
1476 // Do the math for an arc and add it to the queue
1477 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1481 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1483 // Set clockwise/counter-clockwise sign for mc_arc computations
1484 bool is_clockwise
= false;
1485 if( motion_mode
== CW_ARC
) {
1486 is_clockwise
= true;
1490 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1494 float Robot::theta(float x
, float y
)
1496 float t
= atanf(x
/ fabs(y
));
1508 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1510 this->plane_axis_0
= axis_0
;
1511 this->plane_axis_1
= axis_1
;
1512 this->plane_axis_2
= axis_2
;
1515 void Robot::clearToolOffset()
1517 this->tool_offset
= wcs_t(0,0,0);
1520 void Robot::setToolOffset(const float offset
[3])
1522 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1525 float Robot::get_feed_rate() const
1527 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;