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"
36 #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")
58 #define arm_solution_checksum CHECKSUM("arm_solution")
59 #define cartesian_checksum CHECKSUM("cartesian")
60 #define rotatable_cartesian_checksum CHECKSUM("rotatable_cartesian")
61 #define rostock_checksum CHECKSUM("rostock")
62 #define linear_delta_checksum CHECKSUM("linear_delta")
63 #define rotary_delta_checksum CHECKSUM("rotary_delta")
64 #define delta_checksum CHECKSUM("delta")
65 #define hbot_checksum CHECKSUM("hbot")
66 #define corexy_checksum CHECKSUM("corexy")
67 #define corexz_checksum CHECKSUM("corexz")
68 #define kossel_checksum CHECKSUM("kossel")
69 #define morgan_checksum CHECKSUM("morgan")
71 // new-style actuator stuff
72 #define actuator_checksum CHEKCSUM("actuator")
74 #define step_pin_checksum CHECKSUM("step_pin")
75 #define dir_pin_checksum CHEKCSUM("dir_pin")
76 #define en_pin_checksum CHECKSUM("en_pin")
78 #define steps_per_mm_checksum CHECKSUM("steps_per_mm")
79 #define max_rate_checksum CHECKSUM("max_rate")
80 #define acceleration_checksum CHECKSUM("acceleration")
81 #define z_acceleration_checksum CHECKSUM("z_acceleration")
83 #define alpha_checksum CHECKSUM("alpha")
84 #define beta_checksum CHECKSUM("beta")
85 #define gamma_checksum CHECKSUM("gamma")
87 #define ARC_ANGULAR_TRAVEL_EPSILON 5E-7F // Float (radians)
88 #define PI 3.14159265358979323846F // force to be float, do not use M_PI
90 // 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
91 // It takes care of cutting arcs into segments, same thing for line that are too long
95 this->inch_mode
= false;
96 this->absolute_mode
= true;
97 this->e_absolute_mode
= true;
98 this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
);
99 memset(this->last_milestone
, 0, sizeof last_milestone
);
100 memset(this->last_machine_position
, 0, sizeof last_machine_position
);
101 this->arm_solution
= NULL
;
102 seconds_per_minute
= 60.0F
;
103 this->clearToolOffset();
104 this->compensationTransform
= nullptr;
105 this->wcs_offsets
.fill(wcs_t(0.0F
, 0.0F
, 0.0F
));
106 this->g92_offset
= wcs_t(0.0F
, 0.0F
, 0.0F
);
107 this->next_command_is_MCS
= false;
108 this->disable_segmentation
= false;
110 this->actuators
.fill(nullptr);
113 //Called when the module has just been loaded
114 void Robot::on_module_loaded()
116 this->register_for_event(ON_GCODE_RECEIVED
);
122 #define ACTUATOR_CHECKSUMS(X) { \
123 CHECKSUM(X "_step_pin"), \
124 CHECKSUM(X "_dir_pin"), \
125 CHECKSUM(X "_en_pin"), \
126 CHECKSUM(X "_steps_per_mm"), \
127 CHECKSUM(X "_max_rate"), \
128 CHECKSUM(X "_acceleration") \
131 void Robot::load_config()
133 // Arm solutions are used to convert positions in millimeters into position in steps for each stepper motor.
134 // While for a cartesian arm solution, this is a simple multiplication, in other, less simple cases, there is some serious math to be done.
135 // To make adding those solution easier, they have their own, separate object.
136 // Here we read the config to find out which arm solution to use
137 if (this->arm_solution
) delete this->arm_solution
;
138 int solution_checksum
= get_checksum(THEKERNEL
->config
->value(arm_solution_checksum
)->by_default("cartesian")->as_string());
139 // Note checksums are not const expressions when in debug mode, so don't use switch
140 if(solution_checksum
== hbot_checksum
|| solution_checksum
== corexy_checksum
) {
141 this->arm_solution
= new HBotSolution(THEKERNEL
->config
);
143 } else if(solution_checksum
== corexz_checksum
) {
144 this->arm_solution
= new CoreXZSolution(THEKERNEL
->config
);
146 } else if(solution_checksum
== rostock_checksum
|| solution_checksum
== kossel_checksum
|| solution_checksum
== delta_checksum
|| solution_checksum
== linear_delta_checksum
) {
147 this->arm_solution
= new LinearDeltaSolution(THEKERNEL
->config
);
149 } else if(solution_checksum
== rotatable_cartesian_checksum
) {
150 this->arm_solution
= new RotatableCartesianSolution(THEKERNEL
->config
);
152 } else if(solution_checksum
== rotary_delta_checksum
) {
153 this->arm_solution
= new RotaryDeltaSolution(THEKERNEL
->config
);
155 } else if(solution_checksum
== morgan_checksum
) {
156 this->arm_solution
= new MorganSCARASolution(THEKERNEL
->config
);
158 } else if(solution_checksum
== cartesian_checksum
) {
159 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
162 this->arm_solution
= new CartesianSolution(THEKERNEL
->config
);
165 this->feed_rate
= THEKERNEL
->config
->value(default_feed_rate_checksum
)->by_default( 100.0F
)->as_number();
166 this->seek_rate
= THEKERNEL
->config
->value(default_seek_rate_checksum
)->by_default( 100.0F
)->as_number();
167 this->mm_per_line_segment
= THEKERNEL
->config
->value(mm_per_line_segment_checksum
)->by_default( 0.0F
)->as_number();
168 this->delta_segments_per_second
= THEKERNEL
->config
->value(delta_segments_per_second_checksum
)->by_default(0.0f
)->as_number();
169 this->mm_per_arc_segment
= THEKERNEL
->config
->value(mm_per_arc_segment_checksum
)->by_default( 0.0f
)->as_number();
170 this->mm_max_arc_error
= THEKERNEL
->config
->value(mm_max_arc_error_checksum
)->by_default( 0.01f
)->as_number();
171 this->arc_correction
= THEKERNEL
->config
->value(arc_correction_checksum
)->by_default( 5 )->as_number();
173 // in mm/sec but specified in config as mm/min
174 this->max_speeds
[X_AXIS
] = THEKERNEL
->config
->value(x_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
175 this->max_speeds
[Y_AXIS
] = THEKERNEL
->config
->value(y_axis_max_speed_checksum
)->by_default(60000.0F
)->as_number() / 60.0F
;
176 this->max_speeds
[Z_AXIS
] = THEKERNEL
->config
->value(z_axis_max_speed_checksum
)->by_default( 300.0F
)->as_number() / 60.0F
;
178 this->segment_z_moves
= THEKERNEL
->config
->value(segment_z_moves_checksum
)->by_default(true)->as_bool();
179 this->save_g92
= THEKERNEL
->config
->value(save_g92_checksum
)->by_default(false)->as_bool();
181 // Make our Primary XYZ StepperMotors
182 uint16_t const checksums
[][6] = {
183 ACTUATOR_CHECKSUMS("alpha"), // X
184 ACTUATOR_CHECKSUMS("beta"), // Y
185 ACTUATOR_CHECKSUMS("gamma"), // Z
188 // default acceleration setting, can be overriden with newer per axis settings
189 this->default_acceleration
= THEKERNEL
->config
->value(acceleration_checksum
)->by_default(100.0F
)->as_number(); // Acceleration is in mm/s^2
192 for (size_t a
= X_AXIS
; a
<= Z_AXIS
; a
++) {
193 Pin pins
[3]; //step, dir, enable
194 for (size_t i
= 0; i
< 3; i
++) {
195 pins
[i
].from_string(THEKERNEL
->config
->value(checksums
[a
][i
])->by_default("nc")->as_string())->as_output();
197 StepperMotor
*sm
= new StepperMotor(pins
[0], pins
[1], pins
[2]);
198 // register this motor (NB This must be 0,1,2) of the actuators array
199 uint8_t n
= register_motor(sm
);
201 // this is a fatal error
202 THEKERNEL
->streams
->printf("FATAL: motor %d does not match index %d\n", n
, a
);
206 actuators
[a
]->change_steps_per_mm(THEKERNEL
->config
->value(checksums
[a
][3])->by_default(a
== 2 ? 2560.0F
: 80.0F
)->as_number());
207 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
208 actuators
[a
]->set_acceleration(THEKERNEL
->config
->value(checksums
[a
][5])->by_default(NAN
)->as_number()); // mm/secs²
211 check_max_actuator_speeds(); // check the configs are sane
213 // if we have not specified a z acceleration see if the legacy config was set
214 if(isnan(actuators
[Z_AXIS
]->get_acceleration())) {
215 float acc
= THEKERNEL
->config
->value(z_acceleration_checksum
)->by_default(NAN
)->as_number(); // disabled by default
217 actuators
[Z_AXIS
]->set_acceleration(acc
);
221 // initialise actuator positions to current cartesian position (X0 Y0 Z0)
222 // so the first move can be correct if homing is not performed
223 ActuatorCoordinates actuator_pos
;
224 arm_solution
->cartesian_to_actuator(last_milestone
, actuator_pos
);
225 for (size_t i
= 0; i
< n_motors
; i
++)
226 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
228 //this->clearToolOffset();
231 uint8_t Robot::register_motor(StepperMotor
*motor
)
233 // register this motor with the step ticker
234 THEKERNEL
->step_ticker
->register_motor(motor
);
235 if(n_motors
>= k_max_actuators
) {
236 // this is a fatal error
237 THEKERNEL
->streams
->printf("FATAL: too many motors, increase k_max_actuators\n");
240 actuators
[n_motors
++]= motor
;
244 void Robot::push_state()
246 bool am
= this->absolute_mode
;
247 bool em
= this->e_absolute_mode
;
248 bool im
= this->inch_mode
;
249 saved_state_t
s(this->feed_rate
, this->seek_rate
, am
, em
, im
, current_wcs
);
253 void Robot::pop_state()
255 if(!state_stack
.empty()) {
256 auto s
= state_stack
.top();
258 this->feed_rate
= std::get
<0>(s
);
259 this->seek_rate
= std::get
<1>(s
);
260 this->absolute_mode
= std::get
<2>(s
);
261 this->e_absolute_mode
= std::get
<3>(s
);
262 this->inch_mode
= std::get
<4>(s
);
263 this->current_wcs
= std::get
<5>(s
);
267 std::vector
<Robot::wcs_t
> Robot::get_wcs_state() const
269 std::vector
<wcs_t
> v
;
270 v
.push_back(wcs_t(current_wcs
, MAX_WCS
, 0));
271 for(auto& i
: wcs_offsets
) {
274 v
.push_back(g92_offset
);
275 v
.push_back(tool_offset
);
279 int Robot::print_position(uint8_t subcode
, char *buf
, size_t bufsize
) const
281 // M114.1 is a new way to do this (similar to how GRBL does it).
282 // it returns the realtime position based on the current step position of the actuators.
283 // this does require a FK to get a machine position from the actuator position
284 // and then invert all the transforms to get a workspace position from machine position
285 // M114 just does it the old way uses last_milestone and does inversse transforms to get the requested position
287 if(subcode
== 0) { // M114 print WCS
288 wcs_t pos
= mcs2wcs(last_milestone
);
289 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
)));
291 } else if(subcode
== 4) { // M114.4 print last milestone (which should be the same as machine position if axis are not moving and no level compensation)
292 n
= snprintf(buf
, bufsize
, "LMS: X:%1.4f Y:%1.4f Z:%1.4f", last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
294 } else if(subcode
== 5) { // M114.5 print last machine position (which should be the same as M114.1 if axis are not moving and no level compensation)
295 n
= snprintf(buf
, bufsize
, "LMP: X:%1.4f Y:%1.4f Z:%1.4f", last_machine_position
[X_AXIS
], last_machine_position
[Y_AXIS
], last_machine_position
[Z_AXIS
]);
298 // get real time positions
299 // current actuator position in mm
300 ActuatorCoordinates current_position
{
301 actuators
[X_AXIS
]->get_current_position(),
302 actuators
[Y_AXIS
]->get_current_position(),
303 actuators
[Z_AXIS
]->get_current_position()
306 // get machine position from the actuator position using FK
308 arm_solution
->actuator_to_cartesian(current_position
, mpos
);
310 if(subcode
== 1) { // M114.1 print realtime WCS
311 // FIXME this currently includes the compensation transform which is incorrect so will be slightly off if it is in effect (but by very little)
312 wcs_t pos
= mcs2wcs(mpos
);
313 n
= snprintf(buf
, bufsize
, "WPOS: 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
)));
315 } else if(subcode
== 2) { // M114.2 print realtime Machine coordinate system
316 n
= snprintf(buf
, bufsize
, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos
[X_AXIS
], mpos
[Y_AXIS
], mpos
[Z_AXIS
]);
318 } else if(subcode
== 3) { // M114.3 print realtime actuator position
319 n
= snprintf(buf
, bufsize
, "APOS: A:%1.4f B:%1.4f C:%1.4f", current_position
[X_AXIS
], current_position
[Y_AXIS
], current_position
[Z_AXIS
]);
325 // converts current last milestone (machine position without compensation transform) to work coordinate system (inverse transform)
326 Robot::wcs_t
Robot::mcs2wcs(const Robot::wcs_t
& pos
) const
328 return std::make_tuple(
329 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
),
330 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
),
331 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
)
335 // this does a sanity check that actuator speeds do not exceed steps rate capability
336 // we will override the actuator max_rate if the combination of max_rate and steps/sec exceeds base_stepping_frequency
337 void Robot::check_max_actuator_speeds()
339 for (size_t i
= 0; i
< n_motors
; i
++) {
340 float step_freq
= actuators
[i
]->get_max_rate() * actuators
[i
]->get_steps_per_mm();
341 if (step_freq
> THEKERNEL
->base_stepping_frequency
) {
342 actuators
[i
]->set_max_rate(floorf(THEKERNEL
->base_stepping_frequency
/ actuators
[i
]->get_steps_per_mm()));
343 THEKERNEL
->streams
->printf("WARNING: actuator %d rate exceeds base_stepping_frequency * ..._steps_per_mm: %f, setting to %f\n", i
, step_freq
, actuators
[i
]->max_rate
);
348 //A GCode has been received
349 //See if the current Gcode line has some orders for us
350 void Robot::on_gcode_received(void *argument
)
352 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
354 enum MOTION_MODE_T motion_mode
= NONE
;
358 case 0: motion_mode
= SEEK
; break;
359 case 1: motion_mode
= LINEAR
; break;
360 case 2: motion_mode
= CW_ARC
; break;
361 case 3: motion_mode
= CCW_ARC
; break;
362 case 4: { // G4 pause
363 uint32_t delay_ms
= 0;
364 if (gcode
->has_letter('P')) {
365 delay_ms
= gcode
->get_int('P');
367 if (gcode
->has_letter('S')) {
368 delay_ms
+= gcode
->get_int('S') * 1000;
372 THEKERNEL
->conveyor
->wait_for_empty_queue();
373 // wait for specified time
374 uint32_t start
= us_ticker_read(); // mbed call
375 while ((us_ticker_read() - start
) < delay_ms
* 1000) {
376 THEKERNEL
->call_event(ON_IDLE
, this);
377 if(THEKERNEL
->is_halted()) return;
383 case 10: // G10 L2 [L20] Pn Xn Yn Zn set WCS
384 if(gcode
->has_letter('L') && (gcode
->get_int('L') == 2 || gcode
->get_int('L') == 20) && gcode
->has_letter('P')) {
385 size_t n
= gcode
->get_uint('P');
386 if(n
== 0) n
= current_wcs
; // set current coordinate system
390 std::tie(x
, y
, z
) = wcs_offsets
[n
];
391 if(gcode
->get_int('L') == 20) {
392 // this makes the current machine position (less compensation transform) the offset
393 // get current position in WCS
394 wcs_t pos
= mcs2wcs(last_milestone
);
396 if(gcode
->has_letter('X')){
397 x
-= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
400 if(gcode
->has_letter('Y')){
401 y
-= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
403 if(gcode
->has_letter('Z')) {
404 z
-= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
408 // the value is the offset from machine zero
409 if(gcode
->has_letter('X')) x
= to_millimeters(gcode
->get_value('X'));
410 if(gcode
->has_letter('Y')) y
= to_millimeters(gcode
->get_value('Y'));
411 if(gcode
->has_letter('Z')) z
= to_millimeters(gcode
->get_value('Z'));
413 wcs_offsets
[n
] = wcs_t(x
, y
, z
);
418 case 17: this->select_plane(X_AXIS
, Y_AXIS
, Z_AXIS
); break;
419 case 18: this->select_plane(X_AXIS
, Z_AXIS
, Y_AXIS
); break;
420 case 19: this->select_plane(Y_AXIS
, Z_AXIS
, X_AXIS
); break;
421 case 20: this->inch_mode
= true; break;
422 case 21: this->inch_mode
= false; break;
424 case 54: case 55: case 56: case 57: case 58: case 59:
425 // select WCS 0-8: G54..G59, G59.1, G59.2, G59.3
426 current_wcs
= gcode
->g
- 54;
427 if(gcode
->g
== 59 && gcode
->subcode
> 0) {
428 current_wcs
+= gcode
->subcode
;
429 if(current_wcs
>= MAX_WCS
) current_wcs
= MAX_WCS
- 1;
433 case 90: this->absolute_mode
= true; this->e_absolute_mode
= true; break;
434 case 91: this->absolute_mode
= false; this->e_absolute_mode
= false; break;
437 if(gcode
->subcode
== 1 || gcode
->subcode
== 2 || gcode
->get_num_args() == 0) {
438 // reset G92 offsets to 0
439 g92_offset
= wcs_t(0, 0, 0);
441 } else if(gcode
->subcode
== 3) {
442 // initialize G92 to the specified values, only used for saving it with M500
443 float x
= 0, y
= 0, z
= 0;
444 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
445 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
446 if(gcode
->has_letter('Z')) z
= gcode
->get_value('Z');
447 g92_offset
= wcs_t(x
, y
, z
);
450 // standard setting of the g92 offsets, making current WCS position whatever the coordinate arguments are
452 std::tie(x
, y
, z
) = g92_offset
;
453 // get current position in WCS
454 wcs_t pos
= mcs2wcs(last_milestone
);
456 // adjust g92 offset to make the current wpos == the value requested
457 if(gcode
->has_letter('X')){
458 x
+= to_millimeters(gcode
->get_value('X')) - std::get
<X_AXIS
>(pos
);
460 if(gcode
->has_letter('Y')){
461 y
+= to_millimeters(gcode
->get_value('Y')) - std::get
<Y_AXIS
>(pos
);
463 if(gcode
->has_letter('Z')) {
464 z
+= to_millimeters(gcode
->get_value('Z')) - std::get
<Z_AXIS
>(pos
);
466 g92_offset
= wcs_t(x
, y
, z
);
473 } else if( gcode
->has_m
) {
475 // case 0: // M0 feed hold, (M0.1 is release feed hold, except we are in feed hold)
476 // if(THEKERNEL->is_grbl_mode()) THEKERNEL->set_feed_hold(gcode->subcode == 0);
479 case 30: // M30 end of program in grbl mode (otherwise it is delete sdcard file)
480 if(!THEKERNEL
->is_grbl_mode()) break;
481 // fall through to M2
482 case 2: // M2 end of program
484 absolute_mode
= true;
487 THEKERNEL
->call_event(ON_ENABLE
, (void*)1); // turn all enable pins on
490 case 18: // this used to support parameters, now it ignores them
492 THEKERNEL
->conveyor
->wait_for_empty_queue();
493 THEKERNEL
->call_event(ON_ENABLE
, nullptr); // turn all enable pins off
496 case 82: e_absolute_mode
= true; break;
497 case 83: e_absolute_mode
= false; break;
499 case 92: // M92 - set steps per mm
500 if (gcode
->has_letter('X'))
501 actuators
[0]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('X')));
502 if (gcode
->has_letter('Y'))
503 actuators
[1]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Y')));
504 if (gcode
->has_letter('Z'))
505 actuators
[2]->change_steps_per_mm(this->to_millimeters(gcode
->get_value('Z')));
507 gcode
->stream
->printf("X:%f Y:%f Z:%f ", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
508 gcode
->add_nl
= true;
509 check_max_actuator_speeds();
514 int n
= print_position(gcode
->subcode
, buf
, sizeof buf
);
515 if(n
> 0) gcode
->txt_after_ok
.append(buf
, n
);
519 case 120: // push state
523 case 121: // pop state
527 case 203: // M203 Set maximum feedrates in mm/sec
528 if (gcode
->has_letter('X'))
529 this->max_speeds
[X_AXIS
] = gcode
->get_value('X');
530 if (gcode
->has_letter('Y'))
531 this->max_speeds
[Y_AXIS
] = gcode
->get_value('Y');
532 if (gcode
->has_letter('Z'))
533 this->max_speeds
[Z_AXIS
] = gcode
->get_value('Z');
534 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
535 if (gcode
->has_letter('A' + i
))
536 actuators
[i
]->set_max_rate(gcode
->get_value('A' + i
));
538 check_max_actuator_speeds();
540 if(gcode
->get_num_args() == 0) {
541 gcode
->stream
->printf("X:%g Y:%g Z:%g",
542 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
]);
543 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
544 gcode
->stream
->printf(" %c : %g", 'A' + i
, actuators
[i
]->get_max_rate()); //xxx
546 gcode
->add_nl
= true;
550 case 204: // M204 Snnn - set default acceleration to nnn, Xnnn Ynnn Znnn sets axis specific acceleration
551 if (gcode
->has_letter('S')) {
552 float acc
= gcode
->get_value('S'); // mm/s^2
554 if (acc
< 1.0F
) acc
= 1.0F
;
555 this->default_acceleration
= acc
;
557 for (int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
558 if (gcode
->has_letter(i
+'X')) {
559 float acc
= gcode
->get_value(i
+'X'); // mm/s^2
561 if (acc
<= 0.0F
) acc
= NAN
;
562 actuators
[i
]->set_acceleration(acc
);
567 case 205: // M205 Xnnn - set junction deviation, Z - set Z junction deviation, Snnn - Set minimum planner speed, Ynnn - set minimum step rate
568 if (gcode
->has_letter('X')) {
569 float jd
= gcode
->get_value('X');
573 THEKERNEL
->planner
->junction_deviation
= jd
;
575 if (gcode
->has_letter('Z')) {
576 float jd
= gcode
->get_value('Z');
577 // enforce minimum, -1 disables it and uses regular junction deviation
580 THEKERNEL
->planner
->z_junction_deviation
= jd
;
582 if (gcode
->has_letter('S')) {
583 float mps
= gcode
->get_value('S');
587 THEKERNEL
->planner
->minimum_planner_speed
= mps
;
591 case 220: // M220 - speed override percentage
592 if (gcode
->has_letter('S')) {
593 float factor
= gcode
->get_value('S');
594 // enforce minimum 10% speed
597 // enforce maximum 10x speed
598 if (factor
> 1000.0F
)
601 seconds_per_minute
= 6000.0F
/ factor
;
603 gcode
->stream
->printf("Speed factor at %6.2f %%\n", 6000.0F
/ seconds_per_minute
);
607 case 400: // wait until all moves are done up to this point
608 THEKERNEL
->conveyor
->wait_for_empty_queue();
611 case 500: // M500 saves some volatile settings to config override file
612 case 503: { // M503 just prints the settings
613 gcode
->stream
->printf(";Steps per unit:\nM92 X%1.5f Y%1.5f Z%1.5f\n", actuators
[0]->steps_per_mm
, actuators
[1]->steps_per_mm
, actuators
[2]->steps_per_mm
);
614 gcode
->stream
->printf(";Acceleration mm/sec^2:\nM204 S%1.5f Z%1.5f\n", default_acceleration
, actuators
[Z_AXIS
]->get_acceleration()); // TODO only print XYZ if not NAN
615 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
, THEKERNEL
->planner
->z_junction_deviation
, THEKERNEL
->planner
->minimum_planner_speed
);
616 gcode
->stream
->printf(";Max feedrates in mm/sec, XYZ cartesian, ABC actuator:\nM203 X%1.5f Y%1.5f Z%1.5f A%1.5f B%1.5f C%1.5f",
617 this->max_speeds
[X_AXIS
], this->max_speeds
[Y_AXIS
], this->max_speeds
[Z_AXIS
],
618 actuators
[X_AXIS
]->get_max_rate(), actuators
[Y_AXIS
]->get_max_rate(), actuators
[Z_AXIS
]->get_max_rate());
619 gcode
->stream
->printf("\n");
621 // get or save any arm solution specific optional values
622 BaseSolution::arm_options_t options
;
623 if(arm_solution
->get_optional(options
) && !options
.empty()) {
624 gcode
->stream
->printf(";Optional arm solution specific settings:\nM665");
625 for(auto &i
: options
) {
626 gcode
->stream
->printf(" %c%1.4f", i
.first
, i
.second
);
628 gcode
->stream
->printf("\n");
631 // save wcs_offsets and current_wcs
632 // TODO this may need to be done whenever they change to be compliant
633 gcode
->stream
->printf(";WCS settings\n");
634 gcode
->stream
->printf("%s\n", wcs2gcode(current_wcs
).c_str());
636 for(auto &i
: wcs_offsets
) {
637 if(i
!= wcs_t(0, 0, 0)) {
639 std::tie(x
, y
, z
) = i
;
640 gcode
->stream
->printf("G10 L2 P%d X%f Y%f Z%f ; %s\n", n
, x
, y
, z
, wcs2gcode(n
-1).c_str());
645 // linuxcnc saves G92, so we do too if configured, default is to not save to maintain backward compatibility
646 // 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
647 if(g92_offset
!= wcs_t(0, 0, 0)) {
649 std::tie(x
, y
, z
) = g92_offset
;
650 gcode
->stream
->printf("G92.3 X%f Y%f Z%f\n", x
, y
, z
); // sets G92 to the specified values
656 case 665: { // M665 set optional arm solution variables based on arm solution.
657 // the parameter args could be any letter each arm solution only accepts certain ones
658 BaseSolution::arm_options_t options
= gcode
->get_args();
659 options
.erase('S'); // don't include the S
660 options
.erase('U'); // don't include the U
661 if(options
.size() > 0) {
662 // set the specified options
663 arm_solution
->set_optional(options
);
666 if(arm_solution
->get_optional(options
)) {
667 // foreach optional value
668 for(auto &i
: options
) {
669 // print all current values of supported options
670 gcode
->stream
->printf("%c: %8.4f ", i
.first
, i
.second
);
671 gcode
->add_nl
= true;
675 if(gcode
->has_letter('S')) { // set delta segments per second, not saved by M500
676 this->delta_segments_per_second
= gcode
->get_value('S');
677 gcode
->stream
->printf("Delta segments set to %8.4f segs/sec\n", this->delta_segments_per_second
);
679 } else if(gcode
->has_letter('U')) { // or set mm_per_line_segment, not saved by M500
680 this->mm_per_line_segment
= gcode
->get_value('U');
681 this->delta_segments_per_second
= 0;
682 gcode
->stream
->printf("mm per line segment set to %8.4f\n", this->mm_per_line_segment
);
690 if( motion_mode
!= NONE
) {
691 process_move(gcode
, motion_mode
);
694 next_command_is_MCS
= false; // must be on same line as G0 or G1
697 // process a G0/G1/G2/G3
698 void Robot::process_move(Gcode
*gcode
, enum MOTION_MODE_T motion_mode
)
700 // we have a G0/G1/G2/G3 so extract parameters and apply offsets to get machine coordinate target
701 float param
[4]{NAN
, NAN
, NAN
, NAN
};
703 // process primary axis
704 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
706 if( gcode
->has_letter(letter
) ) {
707 param
[i
] = this->to_millimeters(gcode
->get_value(letter
));
711 float offset
[3]{0,0,0};
712 for(char letter
= 'I'; letter
<= 'K'; letter
++) {
713 if( gcode
->has_letter(letter
) ) {
714 offset
[letter
- 'I'] = this->to_millimeters(gcode
->get_value(letter
));
718 // calculate target in machine coordinates (less compensation transform which needs to be done after segmentation)
719 float target
[n_motors
];
720 memcpy(target
, last_milestone
, n_motors
*sizeof(float));
722 if(!next_command_is_MCS
) {
723 if(this->absolute_mode
) {
724 // apply wcs offsets and g92 offset and tool offset
725 if(!isnan(param
[X_AXIS
])) {
726 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
);
729 if(!isnan(param
[Y_AXIS
])) {
730 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
);
733 if(!isnan(param
[Z_AXIS
])) {
734 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
);
738 // they are deltas from the last_milestone if specified
739 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
740 if(!isnan(param
[i
])) target
[i
] = param
[i
] + last_milestone
[i
];
745 // already in machine coordinates, we do not add tool offset for that
746 for(int i
= X_AXIS
; i
<= Z_AXIS
; ++i
) {
747 if(!isnan(param
[i
])) target
[i
] = param
[i
];
751 // process extruder parameters, for active extruder only (only one active extruder at a time)
752 selected_extruder
= 0;
753 if(gcode
->has_letter('E')) {
754 for (int i
= E_AXIS
; i
< n_motors
; ++i
) {
755 // find first selected extruder
756 if(actuators
[i
]->is_selected()) {
757 param
[E_AXIS
]= gcode
->get_value('E');
758 selected_extruder
= i
;
764 // do E for the selected extruder
766 if(selected_extruder
> 0 && !isnan(param
[E_AXIS
])) {
767 if(this->e_absolute_mode
) {
768 target
[selected_extruder
]= param
[E_AXIS
];
769 delta_e
= target
[selected_extruder
] - last_milestone
[selected_extruder
];
771 delta_e
= param
[E_AXIS
];
772 target
[selected_extruder
] = delta_e
+ last_milestone
[selected_extruder
];
776 if( gcode
->has_letter('F') ) {
777 if( motion_mode
== SEEK
)
778 this->seek_rate
= this->to_millimeters( gcode
->get_value('F') );
780 this->feed_rate
= this->to_millimeters( gcode
->get_value('F') );
785 // Perform any physical actions
786 switch(motion_mode
) {
790 moved
= this->append_line(gcode
, target
, this->seek_rate
/ seconds_per_minute
, delta_e
);
794 moved
= this->append_line(gcode
, target
, this->feed_rate
/ seconds_per_minute
, delta_e
);
799 moved
= this->compute_arc(gcode
, offset
, target
, motion_mode
);
804 // set last_milestone to the calculated target
805 memcpy(this->last_milestone
, target
, sizeof(this->last_milestone
));
809 // reset the machine position for all axis. Used for homing.
810 // During homing compensation is turned off (actually not used as it drives steppers directly)
811 // once homed and reset_axis called compensation is used for the move to origin and back off home if enabled,
812 // so in those cases the final position is compensated.
813 void Robot::reset_axis_position(float x
, float y
, float z
)
815 // these are set to the same as compensation was not used to get to the current position
816 last_machine_position
[X_AXIS
]= last_milestone
[X_AXIS
] = x
;
817 last_machine_position
[Y_AXIS
]= last_milestone
[Y_AXIS
] = y
;
818 last_machine_position
[Z_AXIS
]= last_milestone
[Z_AXIS
] = z
;
820 // now set the actuator positions to match
821 ActuatorCoordinates actuator_pos
;
822 arm_solution
->cartesian_to_actuator(this->last_machine_position
, actuator_pos
);
823 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
824 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
827 // Reset the position for an axis (used in homing)
828 void Robot::reset_axis_position(float position
, int axis
)
830 last_milestone
[axis
] = position
;
831 reset_axis_position(last_milestone
[X_AXIS
], last_milestone
[Y_AXIS
], last_milestone
[Z_AXIS
]);
834 // similar to reset_axis_position but directly sets the actuator positions in actuators units (eg mm for cartesian, degrees for rotary delta)
835 // then sets the axis positions to match. currently only called from Endstops.cpp
836 void Robot::reset_actuator_position(const ActuatorCoordinates
&ac
)
838 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
839 actuators
[i
]->change_last_milestone(ac
[i
]);
841 // now correct axis positions then recorrect actuator to account for rounding
842 reset_position_from_current_actuator_position();
845 // Use FK to find out where actuator is and reset to match
846 void Robot::reset_position_from_current_actuator_position()
848 ActuatorCoordinates actuator_pos
;
849 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++) {
850 // NOTE actuator::current_position is curently NOT the same as actuator::last_milestone after an abrupt abort
851 actuator_pos
[i
] = actuators
[i
]->get_current_position();
854 // discover machine position from where actuators actually are
855 arm_solution
->actuator_to_cartesian(actuator_pos
, last_machine_position
);
856 // FIXME problem is this includes any compensation transform, and without an inverse compensation we cannot get a correct last_milestone
857 memcpy(last_milestone
, last_machine_position
, sizeof last_milestone
);
859 // now reset actuator::last_milestone, NOTE this may lose a little precision as FK is not always entirely accurate.
860 // NOTE This is required to sync the machine position with the actuator position, we do a somewhat redundant cartesian_to_actuator() call
861 // to get everything in perfect sync.
862 arm_solution
->cartesian_to_actuator(last_machine_position
, actuator_pos
);
863 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
864 actuators
[i
]->change_last_milestone(actuator_pos
[i
]);
867 // Convert target (in machine coordinates) from millimeters to steps, and append this to the planner
868 // target is in machine coordinates without the compensation transform, however we save a last_machine_position that includes
869 // all transforms and is what we actually convert to actuator positions
870 bool Robot::append_milestone(Gcode
*gcode
, const float target
[], float rate_mm_s
)
872 float deltas
[n_motors
];
873 float transformed_target
[n_motors
]; // adjust target for bed compensation and WCS offsets
874 float unit_vec
[N_PRIMARY_AXIS
];
875 float millimeters_of_travel
= 0;
877 // catch negative or zero feed rates and return the same error as GRBL does
878 if(rate_mm_s
<= 0.0F
) {
879 gcode
->is_error
= true;
880 gcode
->txt_after_ok
= (rate_mm_s
== 0 ? "Undefined feed rate" : "feed rate < 0");
884 // unity transform by default
885 memcpy(transformed_target
, target
, n_motors
*sizeof(float));
887 // check function pointer and call if set to transform the target to compensate for bed
888 if(compensationTransform
) {
889 // some compensation strategies can transform XYZ, some just change Z
890 compensationTransform(transformed_target
);
896 // find distance moved by each axis, use transformed target from the current machine position
897 for (size_t i
= 0; i
<= n_motors
; i
++) {
898 deltas
[i
] = transformed_target
[i
] - last_machine_position
[i
];
899 if(deltas
[i
] == 0) continue;
900 // at least one non zero delta
903 sos
+= powf(deltas
[i
], 2);
908 if(!move
) return false;
910 // set if none of the primary axis is moving
911 bool auxilliary_move
= false;
913 millimeters_of_travel
= sqrtf(sos
);
915 } else if(n_motors
>= E_AXIS
) { // if we have more than 3 axis/actuators (XYZE)
916 // non primary axis move (like extrude)
917 // select the biggest one (usually just E)
918 auto mi
= std::max_element(&deltas
[E_AXIS
], &deltas
[n_motors
], [](float a
, float b
){ return std::abs(a
) < std::abs(b
); } );
919 millimeters_of_travel
= std::abs(*mi
);
920 auxilliary_move
= true;
923 // shouldn't happen but just in case
927 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
928 // 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
929 if(millimeters_of_travel
< 0.00001F
) return false;
931 // this is the machine position
932 memcpy(this->last_machine_position
, transformed_target
, n_motors
*sizeof(float));
934 if(!auxilliary_move
) {
935 // find distance unit vector for primary axis only
936 for (size_t i
= X_AXIS
; i
<= Z_AXIS
; i
++)
937 unit_vec
[i
] = deltas
[i
] / millimeters_of_travel
;
939 // Do not move faster than the configured cartesian limits for XYZ
940 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
941 if ( max_speeds
[axis
] > 0 ) {
942 float axis_speed
= fabsf(unit_vec
[axis
] * rate_mm_s
);
944 if (axis_speed
> max_speeds
[axis
])
945 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
950 // find actuator position given the machine position, use actual adjusted target
951 ActuatorCoordinates actuator_pos
;
952 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
955 // for the extruders just copy the position
956 for (size_t i
= E_AXIS
; i
< n_motors
; i
++) {
957 actuator_pos
[i
]= last_machine_position
[i
];
958 if(!isnan(this->e_scale
)) {
959 // NOTE this relies on the fact only one extruder is active at a time
960 // scale for volumetric or flow rate
961 // TODO is this correct? scaling the absolute target? what if the scale changes?
962 actuator_pos
[i
] *= this->e_scale
;
967 // use default acceleration to start with
968 float acceleration
= default_acceleration
;
970 float isecs
= rate_mm_s
/ millimeters_of_travel
;
972 // check per-actuator speed limits
973 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
974 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
975 if(d
== 0 || !actuators
[actuator
]->is_selected()) continue; // no movement for this actuator
977 float actuator_rate
= d
* isecs
;
978 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
979 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
980 isecs
= rate_mm_s
/ millimeters_of_travel
;
983 // adjust acceleration to lowest found in an active axis
984 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
985 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
986 float ca
= fabsf((deltas
[actuator
]/millimeters_of_travel
) * acceleration
);
988 acceleration
*= ( ma
/ ca
);
993 // Append the block to the planner
994 THEKERNEL
->planner
->append_block( actuator_pos
, rate_mm_s
, millimeters_of_travel
, auxilliary_move
? nullptr : unit_vec
, acceleration
);
999 // Used to plan a single move used by things like endstops when homing, zprobe, extruder retracts etc.
1000 bool Robot::solo_move(const float *delta
, float rate_mm_s
)
1002 if(THEKERNEL
->is_halted()) return false;
1004 // catch negative or zero feed rates and return the same error as GRBL does
1005 if(rate_mm_s
<= 0.0F
) {
1009 // Compute how long this move moves, so we can attach it to the block for later use
1010 float millimeters_of_travel
= sqrtf( powf( delta
[X_AXIS
], 2 ) + powf( delta
[Y_AXIS
], 2 ) + powf( delta
[Z_AXIS
], 2 ) );
1012 // it is unlikely but we need to protect against divide by zero, so ignore insanely small moves here
1013 // 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
1014 if(millimeters_of_travel
< 0.00001F
) return false;
1016 // this is the new machine position
1017 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
1018 this->last_machine_position
[axis
] += delta
[axis
];
1021 // Do not move faster than the configured cartesian limits
1022 for (int axis
= X_AXIS
; axis
<= Z_AXIS
; axis
++) {
1023 if ( max_speeds
[axis
] > 0 ) {
1024 float axis_speed
= fabsf(delta
[axis
] / millimeters_of_travel
* rate_mm_s
);
1026 if (axis_speed
> max_speeds
[axis
])
1027 rate_mm_s
*= ( max_speeds
[axis
] / axis_speed
);
1031 // find actuator position given the machine position, use actual adjusted target
1032 ActuatorCoordinates actuator_pos
;
1033 arm_solution
->cartesian_to_actuator( this->last_machine_position
, actuator_pos
);
1035 // use default acceleration to start with
1036 float acceleration
= default_acceleration
;
1037 float isecs
= rate_mm_s
/ millimeters_of_travel
;
1039 // check per-actuator speed limits
1040 for (size_t actuator
= 0; actuator
< n_motors
; actuator
++) {
1041 float d
= fabsf(actuator_pos
[actuator
] - actuators
[actuator
]->get_last_milestone());
1042 if(d
== 0) continue; // no movement for this actuator
1044 float actuator_rate
= d
* isecs
;
1045 if (actuator_rate
> actuators
[actuator
]->get_max_rate()) {
1046 rate_mm_s
*= (actuators
[actuator
]->get_max_rate() / actuator_rate
);
1047 isecs
= rate_mm_s
/ millimeters_of_travel
;
1050 // adjust acceleration to lowest found in an active axis
1051 float ma
= actuators
[actuator
]->get_acceleration(); // in mm/sec²
1052 if(!isnan(ma
)) { // if axis does not have acceleration set then it uses the default_acceleration
1053 float ca
= fabsf((d
/millimeters_of_travel
) * acceleration
);
1055 acceleration
*= ( ma
/ ca
);
1059 // Append the block to the planner
1060 THEKERNEL
->planner
->append_block(actuator_pos
, rate_mm_s
, millimeters_of_travel
, nullptr, acceleration
);
1065 // Append a move to the queue ( cutting it into segments if needed )
1066 bool Robot::append_line(Gcode
*gcode
, const float target
[], float rate_mm_s
, float delta_e
)
1068 // by default there is no e scaling required
1071 // Find out the distance for this move in XYZ in MCS
1072 float millimeters_of_travel
= sqrtf(powf( target
[X_AXIS
] - last_milestone
[X_AXIS
], 2 ) + powf( target
[Y_AXIS
] - last_milestone
[Y_AXIS
], 2 ) + powf( target
[Z_AXIS
] - last_milestone
[Z_AXIS
], 2 ));
1074 if(millimeters_of_travel
< 0.00001F
) { // we have no movement in XYZ, probably E only
1075 return this->append_milestone(gcode
, target
, rate_mm_s
);
1079 For extruders, we need to do some extra work...
1080 if we have volumetric limits enabled we calculate the volume for this move and limit the rate if it exceeds the stated limit.
1081 Note we need to be using volumetric extrusion for this to work as Ennn is in mm³ not mm
1082 We ask Extruder to do all the work but we need to pass in the relevant data.
1083 NOTE we need to do this before we segment the line (for deltas)
1084 This also sets any scaling due to flow rate and volumetric if a G1
1086 if(!isnan(delta_e
) && gcode
->has_g
&& gcode
->g
== 1) {
1087 float data
[2]= {delta_e
, rate_mm_s
/ millimeters_of_travel
};
1088 if(PublicData::set_value(extruder_checksum
, target_checksum
, data
)) {
1089 rate_mm_s
*= data
[1]; // adjust the feedrate
1090 // we may need to scale the amount moved too
1091 this->e_scale
= data
[0];
1095 // 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.
1096 // In delta robots either mm_per_line_segment can be used OR delta_segments_per_second
1097 // 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
1100 if(this->disable_segmentation
|| (!segment_z_moves
&& !gcode
->has_letter('X') && !gcode
->has_letter('Y'))) {
1103 } else if(this->delta_segments_per_second
> 1.0F
) {
1104 // enabled if set to something > 1, it is set to 0.0 by default
1105 // segment based on current speed and requested segments per second
1106 // the faster the travel speed the fewer segments needed
1107 // NOTE rate is mm/sec and we take into account any speed override
1108 float seconds
= millimeters_of_travel
/ rate_mm_s
;
1109 segments
= max(1.0F
, ceilf(this->delta_segments_per_second
* seconds
));
1110 // TODO if we are only moving in Z on a delta we don't really need to segment at all
1113 if(this->mm_per_line_segment
== 0.0F
) {
1114 segments
= 1; // don't split it up
1116 segments
= ceilf( millimeters_of_travel
/ this->mm_per_line_segment
);
1122 // A vector to keep track of the endpoint of each segment
1123 float segment_delta
[n_motors
];
1124 float segment_end
[n_motors
];
1125 memcpy(segment_end
, last_milestone
, n_motors
*sizeof(float));
1127 // How far do we move each segment?
1128 for (int i
= 0; i
< n_motors
; i
++)
1129 segment_delta
[i
] = (target
[i
] - last_milestone
[i
]) / segments
;
1131 // segment 0 is already done - it's the end point of the previous move so we start at segment 1
1132 // We always add another point after this loop so we stop at segments-1, ie i < segments
1133 for (int i
= 1; i
< segments
; i
++) {
1134 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1135 for (int i
= 0; i
< n_motors
; i
++)
1136 segment_end
[i
] += segment_delta
[i
];
1138 // Append the end of this segment to the queue
1139 bool b
= this->append_milestone(gcode
, segment_end
, rate_mm_s
);
1144 // Append the end of this full move to the queue
1145 if(this->append_milestone(gcode
, target
, rate_mm_s
)) moved
= true;
1147 this->next_command_is_MCS
= false; // always reset this
1153 // Append an arc to the queue ( cutting it into segments as needed )
1154 bool Robot::append_arc(Gcode
* gcode
, const float target
[], const float offset
[], float radius
, bool is_clockwise
)
1158 float center_axis0
= this->last_milestone
[this->plane_axis_0
] + offset
[this->plane_axis_0
];
1159 float center_axis1
= this->last_milestone
[this->plane_axis_1
] + offset
[this->plane_axis_1
];
1160 float linear_travel
= target
[this->plane_axis_2
] - this->last_milestone
[this->plane_axis_2
];
1161 float r_axis0
= -offset
[this->plane_axis_0
]; // Radius vector from center to current location
1162 float r_axis1
= -offset
[this->plane_axis_1
];
1163 float rt_axis0
= target
[this->plane_axis_0
] - center_axis0
;
1164 float rt_axis1
= target
[this->plane_axis_1
] - center_axis1
;
1166 // Patch from GRBL Firmware - Christoph Baumann 04072015
1167 // CCW angle between position and target from circle center. Only one atan2() trig computation required.
1168 float angular_travel
= atan2f(r_axis0
* rt_axis1
- r_axis1
* rt_axis0
, r_axis0
* rt_axis0
+ r_axis1
* rt_axis1
);
1169 if (is_clockwise
) { // Correct atan2 output per direction
1170 if (angular_travel
>= -ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
-= (2 * PI
); }
1172 if (angular_travel
<= ARC_ANGULAR_TRAVEL_EPSILON
) { angular_travel
+= (2 * PI
); }
1175 // Find the distance for this gcode
1176 float millimeters_of_travel
= hypotf(angular_travel
* radius
, fabsf(linear_travel
));
1178 // We don't care about non-XYZ moves ( for example the extruder produces some of those )
1179 if( millimeters_of_travel
< 0.00001F
) {
1183 // limit segments by maximum arc error
1184 float arc_segment
= this->mm_per_arc_segment
;
1185 if ((this->mm_max_arc_error
> 0) && (2 * radius
> this->mm_max_arc_error
)) {
1186 float min_err_segment
= 2 * sqrtf((this->mm_max_arc_error
* (2 * radius
- this->mm_max_arc_error
)));
1187 if (this->mm_per_arc_segment
< min_err_segment
) {
1188 arc_segment
= min_err_segment
;
1191 // Figure out how many segments for this gcode
1192 uint16_t segments
= ceilf(millimeters_of_travel
/ arc_segment
);
1194 //printf("Radius %f - Segment Length %f - Number of Segments %d\r\n",radius,arc_segment,segments); // Testing Purposes ONLY
1195 float theta_per_segment
= angular_travel
/ segments
;
1196 float linear_per_segment
= linear_travel
/ segments
;
1198 /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
1199 and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
1200 r_T = [cos(phi) -sin(phi);
1201 sin(phi) cos(phi] * r ;
1202 For arc generation, the center of the circle is the axis of rotation and the radius vector is
1203 defined from the circle center to the initial position. Each line segment is formed by successive
1204 vector rotations. This requires only two cos() and sin() computations to form the rotation
1205 matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
1206 all float numbers are single precision on the Arduino. (True float precision will not have
1207 round off issues for CNC applications.) Single precision error can accumulate to be greater than
1208 tool precision in some cases. Therefore, arc path correction is implemented.
1210 Small angle approximation may be used to reduce computation overhead further. This approximation
1211 holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
1212 theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
1213 to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
1214 numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
1215 issue for CNC machines with the single precision Arduino calculations.
1216 This approximation also allows mc_arc to immediately insert a line segment into the planner
1217 without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
1218 a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
1219 This is important when there are successive arc motions.
1221 // Vector rotation matrix values
1222 float cos_T
= 1 - 0.5F
* theta_per_segment
* theta_per_segment
; // Small angle approximation
1223 float sin_T
= theta_per_segment
;
1225 float arc_target
[3];
1232 // Initialize the linear axis
1233 arc_target
[this->plane_axis_2
] = this->last_milestone
[this->plane_axis_2
];
1236 for (i
= 1; i
< segments
; i
++) { // Increment (segments-1)
1237 if(THEKERNEL
->is_halted()) return false; // don't queue any more segments
1239 if (count
< this->arc_correction
) {
1240 // Apply vector rotation matrix
1241 r_axisi
= r_axis0
* sin_T
+ r_axis1
* cos_T
;
1242 r_axis0
= r_axis0
* cos_T
- r_axis1
* sin_T
;
1246 // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
1247 // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
1248 cos_Ti
= cosf(i
* theta_per_segment
);
1249 sin_Ti
= sinf(i
* theta_per_segment
);
1250 r_axis0
= -offset
[this->plane_axis_0
] * cos_Ti
+ offset
[this->plane_axis_1
] * sin_Ti
;
1251 r_axis1
= -offset
[this->plane_axis_0
] * sin_Ti
- offset
[this->plane_axis_1
] * cos_Ti
;
1255 // Update arc_target location
1256 arc_target
[this->plane_axis_0
] = center_axis0
+ r_axis0
;
1257 arc_target
[this->plane_axis_1
] = center_axis1
+ r_axis1
;
1258 arc_target
[this->plane_axis_2
] += linear_per_segment
;
1260 // Append this segment to the queue
1261 bool b
= this->append_milestone(gcode
, arc_target
, this->feed_rate
/ seconds_per_minute
);
1265 // Ensure last segment arrives at target location.
1266 if(this->append_milestone(gcode
, target
, this->feed_rate
/ seconds_per_minute
)) moved
= true;
1271 // Do the math for an arc and add it to the queue
1272 bool Robot::compute_arc(Gcode
* gcode
, const float offset
[], const float target
[], enum MOTION_MODE_T motion_mode
)
1276 float radius
= hypotf(offset
[this->plane_axis_0
], offset
[this->plane_axis_1
]);
1278 // Set clockwise/counter-clockwise sign for mc_arc computations
1279 bool is_clockwise
= false;
1280 if( motion_mode
== CW_ARC
) {
1281 is_clockwise
= true;
1285 return this->append_arc(gcode
, target
, offset
, radius
, is_clockwise
);
1289 float Robot::theta(float x
, float y
)
1291 float t
= atanf(x
/ fabs(y
));
1303 void Robot::select_plane(uint8_t axis_0
, uint8_t axis_1
, uint8_t axis_2
)
1305 this->plane_axis_0
= axis_0
;
1306 this->plane_axis_1
= axis_1
;
1307 this->plane_axis_2
= axis_2
;
1310 void Robot::clearToolOffset()
1312 this->tool_offset
= wcs_t(0,0,0);
1315 void Robot::setToolOffset(const float offset
[3])
1317 this->tool_offset
= wcs_t(offset
[0], offset
[1], offset
[2]);
1320 float Robot::get_feed_rate() const
1322 return THEKERNEL
->gcode_dispatch
->get_modal_command() == 0 ? seek_rate
: feed_rate
;